diff --git a/.vscode/settings.json b/.vscode/settings.json
index 31b6015..ab9d225 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -78,7 +78,9 @@
       "typeindex": "cpp",
       "typeinfo": "cpp",
       "variant": "cpp",
-      "*.ipp": "cpp"
+      "*.ipp": "cpp",
+      "callback": "cpp",
+      "image": "cpp"
     },
     "ros.distro": "iron",
     "C_Cpp.default.includePath": [
@@ -97,5 +99,8 @@
       "build": true,
       "install": true,
       "log": true
-    }
-}
\ No newline at end of file
+    },
+    "cSpell.words": [
+      "RCLCPP"
+    ]
+}
diff --git a/README.md b/README.md
index 2ba664e..08ff6e8 100644
--- a/README.md
+++ b/README.md
@@ -183,3 +183,18 @@ You can now plan in RViz and control the simulated arm.
 ### Hand-Eye Calibration
 
 See [ar4_hand_eye_calibration](https://github.com/ycheng517/ar4_hand_eye_calibration)
+
+## Tuning and Tweaks
+
+### Tuning Joint Offsets
+
+If for some reason your robot's joint positions appear misaligned after moving
+to the home position, you can adjust the joint offsets in the
+[joint_offsets/](./ar_hardware_interface/config/joint_offsets/) config folder.
+Select and modify the YAML file corresponding to your AR model to fine-tune the joint positions.
+
+### Switching to Position Control
+
+By default this repo uses velocity-based joint trajectory control. It allows the arm to move a lot faster and the arm movement is also a lot smoother. If for any
+reason you'd like to use the simpler classic position-only control mode, you can
+set `velocity_control_enabled: false` in [driver.yaml](./ar_hardware_interface/config/driver.yaml). Note that you'll need to reduce velocity and acceleration scaling in order for larger motions to succeed.
diff --git a/ar_description/package.xml b/ar_description/package.xml
index 8f73e98..df55ef5 100644
--- a/ar_description/package.xml
+++ b/ar_description/package.xml
@@ -1,6 +1,6 @@
 <package format="3">
   <name>ar_description</name>
-  <version>1.0.0</version>
+  <version>2.0.0</version>
   <description>
     URDF Description package for Annin Robotics robotic arms
   </description>
diff --git a/ar_description/urdf/ar.ros2_control.xacro b/ar_description/urdf/ar.ros2_control.xacro
index 8442670..9beb06c 100644
--- a/ar_description/urdf/ar.ros2_control.xacro
+++ b/ar_description/urdf/ar.ros2_control.xacro
@@ -8,6 +8,7 @@
     calibrate
     robot_parameters_file
     joint_offset_parameters_file
+    driver_parameters_file
   ">
 
     <xacro:property name="robot_parameters"
@@ -16,6 +17,9 @@
     <xacro:property name="joint_offset_parameters"
       value="${xacro.load_yaml(joint_offset_parameters_file)}"/>
 
+    <xacro:property name="driver_parameters"
+      value="${xacro.load_yaml(driver_parameters_file)}"/>
+
     <ros2_control name="${ar_model}" type="system">
 
       <hardware>
@@ -23,6 +27,7 @@
         <param name="ar_model">${ar_model}</param>
         <param name="serial_port">${serial_port}</param>
         <param name="calibrate">${calibrate}</param>
+        <param name="velocity_control_enabled">${driver_parameters['velocity_control_enabled']}</param>
       </hardware>
 
       <joint name="joint_1">
@@ -30,9 +35,13 @@
           <param name="min">${robot_parameters['j1_limit_min']}</param>
           <param name="max">${robot_parameters['j1_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_1']}</param>
       </joint>
 
@@ -41,9 +50,13 @@
           <param name="min">${robot_parameters['j2_limit_min']}</param>
           <param name="max">${robot_parameters['j2_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_2']}</param>
       </joint>
 
@@ -52,9 +65,13 @@
           <param name="min">${robot_parameters['j3_limit_min']}</param>
           <param name="max">${robot_parameters['j3_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_3']}</param>
       </joint>
 
@@ -63,9 +80,13 @@
           <param name="min">${robot_parameters['j4_limit_min']}</param>
           <param name="max">${robot_parameters['j4_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_4']}</param>
       </joint>
 
@@ -74,9 +95,13 @@
           <param name="min">${robot_parameters['j5_limit_min']}</param>
           <param name="max">${robot_parameters['j5_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_5']}</param>
       </joint>
 
@@ -85,9 +110,13 @@
           <param name="min">${robot_parameters['j6_limit_min']}</param>
           <param name="max">${robot_parameters['j6_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_6']}</param>
       </joint>
 
diff --git a/ar_description/urdf/ar_gazebo.urdf.xacro b/ar_description/urdf/ar_gazebo.urdf.xacro
index 4552bc0..c2aa624 100644
--- a/ar_description/urdf/ar_gazebo.urdf.xacro
+++ b/ar_description/urdf/ar_gazebo.urdf.xacro
@@ -23,6 +23,7 @@
     calibrate="False"
     robot_parameters_file="$(find ar_description)/config/$(arg ar_model).yaml"
     joint_offset_parameters_file="$(find ar_hardware_interface)/config/joint_offsets/$(arg ar_model).yaml"
+    driver_parameters_file="$(find ar_hardware_interface)/config/driver.yaml"
   />
 
   <xacro:ar_gripper_ros2_control
diff --git a/ar_description/urdf/ar_macro.xacro b/ar_description/urdf/ar_macro.xacro
index eb7f80b..c06be16 100644
--- a/ar_description/urdf/ar_macro.xacro
+++ b/ar_description/urdf/ar_macro.xacro
@@ -63,7 +63,12 @@
       <parent link="base_link"/>
       <child link="link_1"/>
       <axis xyz="0 0 1"/>
-      <limit lower="${robot_parameters['j1_limit_min']}" upper="${robot_parameters['j1_limit_max']}" effort="0" velocity="0.5"/>
+      <limit
+        lower="${robot_parameters['j1_limit_min']}"
+        upper="${robot_parameters['j1_limit_max']}"
+        effort="-1"
+        velocity="1.0472"
+      />
     </joint>
     <link name="link_2">
       <inertial>
@@ -92,7 +97,12 @@
       <parent link="link_1"/>
       <child link="link_2"/>
       <axis xyz="0 0 -1"/>
-      <limit lower="${robot_parameters['j2_limit_min']}" upper="${robot_parameters['j2_limit_max']}" effort="0.5" velocity="0.5"/>
+      <limit 
+        lower="${robot_parameters['j2_limit_min']}"
+        upper="${robot_parameters['j2_limit_max']}"
+        effort="-1" 
+        velocity="1.0472"
+      />
     </joint>
     <link name="link_3">
       <inertial>
@@ -121,7 +131,12 @@
       <parent link="link_2"/>
       <child link="link_3"/>
       <axis xyz="0 0 -1"/>
-      <limit lower="${robot_parameters['j3_limit_min']}" upper="${robot_parameters['j3_limit_max']}" effort="0" velocity="0.5"/>
+      <limit
+        lower="${robot_parameters['j3_limit_min']}"
+        upper="${robot_parameters['j3_limit_max']}"
+        effort="-1"
+        velocity="1.0472"
+      />
     </joint>
     <link name="link_4">
       <inertial>
@@ -150,7 +165,12 @@
       <parent link="link_3"/>
       <child link="link_4"/>
       <axis xyz="0 0 -1"/>
-      <limit lower="${robot_parameters['j4_limit_min']}" upper="${robot_parameters['j4_limit_max']}" effort="0" velocity="0.5"/>
+      <limit
+        lower="${robot_parameters['j4_limit_min']}"
+        upper="${robot_parameters['j4_limit_max']}"
+        effort="-1"
+        velocity="1.0472"
+      />
     </joint>
     <link name="link_5">
       <inertial>
@@ -179,7 +199,12 @@
       <parent link="link_4"/>
       <child link="link_5"/>
       <axis xyz="1 0 0"/>
-      <limit lower="${robot_parameters['j5_limit_min']}" upper="${robot_parameters['j5_limit_max']}" effort="0" velocity="1.0"/>
+      <limit
+        lower="${robot_parameters['j5_limit_min']}"
+        upper="${robot_parameters['j5_limit_max']}"
+        effort="-1"
+        velocity="1.0472"
+      />
     </joint>
     <link name="link_6">
       <inertial>
@@ -208,7 +233,12 @@
       <parent link="link_5"/>
       <child link="link_6"/>
       <axis xyz="0 0 1"/>
-      <limit lower="${robot_parameters['j6_limit_min']}" upper="${robot_parameters['j6_limit_max']}" effort="0" velocity="1.0"/>
+      <limit
+        lower="${robot_parameters['j6_limit_min']}"
+        upper="${robot_parameters['j6_limit_max']}"
+        effort="-1"
+        velocity="1.0472"
+      />
     </joint>
 
     <!-- center of the end effector mounting surface on link_6 -->
diff --git a/ar_gazebo/package.xml b/ar_gazebo/package.xml
index c3c11fe..041d1a9 100644
--- a/ar_gazebo/package.xml
+++ b/ar_gazebo/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ar_gazebo</name>
-  <version>0.1.0</version>
+  <version>2.0.0</version>
   <description>Gazebo simulation the AR4 robot</description>
   <author>Yifei Cheng</author>
   <maintainer email="ycheng517@gmail.com">Yifei Cheng</maintainer>
diff --git a/ar_hardware_interface/config/controllers.yaml b/ar_hardware_interface/config/controllers.yaml
index a6abf6d..0e9e883 100644
--- a/ar_hardware_interface/config/controllers.yaml
+++ b/ar_hardware_interface/config/controllers.yaml
@@ -22,8 +22,13 @@ joint_trajectory_controller:
       - joint_6
     command_interfaces:
       - position
+      - velocity
     state_interfaces:
       - position
+      - velocity
+    
+    action_monitor_rate: 20.0
+
     allow_partial_joints_goal: false
     constraints:
       stopped_velocity_tolerance: 0.02
@@ -35,6 +40,24 @@ joint_trajectory_controller:
       joint_5: { trajectory: 0.2, goal: 0.02 }
       joint_6: { trajectory: 0.2, goal: 0.02 }
 
+    gains: # Required because we're controlling a velocity interface
+      joint_1: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}  # Smaller 'p' term, since ff term does most of the work
+      joint_2: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}
+      joint_3: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}
+      joint_4: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}
+      joint_5: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}
+      joint_6: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}
+
+    velocity_ff:
+      joint_1: 1.0
+      joint_2: 1.0
+      joint_3: 1.0
+      joint_4: 1.0
+      joint_5: 1.0
+      joint_6: 1.0
+
+    state_interfaces: ["position", "velocity"]
+
 gripper_controller:
   ros__parameters:
     joint: gripper_jaw1_joint
diff --git a/ar_hardware_interface/config/driver.yaml b/ar_hardware_interface/config/driver.yaml
new file mode 100644
index 0000000..1f86b48
--- /dev/null
+++ b/ar_hardware_interface/config/driver.yaml
@@ -0,0 +1 @@
+velocity_control_enabled: true
diff --git a/ar_hardware_interface/include/ar_hardware_interface/ar_hardware_interface.hpp b/ar_hardware_interface/include/ar_hardware_interface/ar_hardware_interface.hpp
index 4f2f809..d8faec8 100644
--- a/ar_hardware_interface/include/ar_hardware_interface/ar_hardware_interface.hpp
+++ b/ar_hardware_interface/include/ar_hardware_interface/ar_hardware_interface.hpp
@@ -36,8 +36,10 @@ class ARHardwareInterface : public hardware_interface::SystemInterface {
 
   // Motor driver
   TeensyDriver driver_;
-  std::vector<double> actuator_commands_;
+  std::vector<double> actuator_pos_commands_;
+  std::vector<double> actuator_vel_commands_;
   std::vector<double> actuator_positions_;
+  std::vector<double> actuator_velocities_;
 
   // Shared memory
   std::vector<double> joint_offsets_;
diff --git a/ar_hardware_interface/include/ar_hardware_interface/teensy_driver.hpp b/ar_hardware_interface/include/ar_hardware_interface/teensy_driver.hpp
index 7c921f3..b164e45 100644
--- a/ar_hardware_interface/include/ar_hardware_interface/teensy_driver.hpp
+++ b/ar_hardware_interface/include/ar_hardware_interface/teensy_driver.hpp
@@ -15,12 +15,13 @@ namespace ar_hardware_interface {
 class TeensyDriver {
  public:
   bool init(std::string ar_model, std::string port, int baudrate,
-            int num_joints);
-  void setStepperSpeed(std::vector<double>& max_speed,
-                       std::vector<double>& max_accel);
+            int num_joints, bool velocity_control_enabled);
   void update(std::vector<double>& pos_commands,
-              std::vector<double>& joint_states);
+              std::vector<double>& vel_commands,
+              std::vector<double>& joint_states,
+              std::vector<double>& joint_velocities);
   void getJointPositions(std::vector<double>& joint_positions);
+  void getJointVelocities(std::vector<double>& joint_velocities);
   bool calibrateJoints();
   bool resetEStop();
   bool isEStopped();
@@ -35,8 +36,11 @@ class TeensyDriver {
   boost::asio::serial_port serial_port_;
   int num_joints_;
   std::vector<double> joint_positions_deg_;
+  std::vector<double> joint_velocities_deg_;
   std::vector<int> enc_calibrations_;
+  bool velocity_control_enabled_ = true;
   bool is_estopped_;
+
   rclcpp::Logger logger_ = rclcpp::get_logger("teensy_driver");
   rclcpp::Clock clock_ = rclcpp::Clock(RCL_ROS_TIME);
 
@@ -47,10 +51,13 @@ class TeensyDriver {
   bool sendCommand(std::string outMsg);  // send arbitrary commands
 
   void checkInit(std::string msg);
+
+  template <typename T>
+  void parseValuesToVector(const std::string msg, std::vector<T>& values);
   void updateEncoderCalibrations(std::string msg);
   void updateJointPositions(std::string msg);
+  void updateJointVelocities(std::string msg);
   void updateEStopStatus(std::string msg);
-  bool succeeded(std::string msg);
 };
 
 }  // namespace ar_hardware_interface
diff --git a/ar_hardware_interface/package.xml b/ar_hardware_interface/package.xml
index 4b12f6e..604855e 100644
--- a/ar_hardware_interface/package.xml
+++ b/ar_hardware_interface/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ar_hardware_interface</name>
-  <version>0.1.0</version>
+  <version>2.0.0</version>
   <description>
     The ar_hardware_interface package provides the hardware interface for
     running an AR4 robot using the ros2_control software framework.
@@ -29,10 +29,8 @@
   <exec_depend>joint_state_publisher_gui</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>ros2_aruco</exec_depend>
   <exec_depend>ros2controlcli</exec_depend>
   <exec_depend>ros2launch</exec_depend>
-  <exec_depend>rviz2</exec_depend>
   <exec_depend>urdf</exec_depend>
   <exec_depend>xacro</exec_depend>
 
diff --git a/ar_hardware_interface/src/ar_hardware_interface.cpp b/ar_hardware_interface/src/ar_hardware_interface.cpp
index f9370f1..4a9d743 100644
--- a/ar_hardware_interface/src/ar_hardware_interface.cpp
+++ b/ar_hardware_interface/src/ar_hardware_interface.cpp
@@ -18,9 +18,13 @@ hardware_interface::CallbackReturn ARHardwareInterface::on_init(
   // init motor driver
   std::string serial_port = info_.hardware_parameters.at("serial_port");
   std::string ar_model = info_.hardware_parameters.at("ar_model");
+  std::string velocity_control_p =
+      info_.hardware_parameters.at("velocity_control_enabled");
+  bool velocity_control_enabled =
+      velocity_control_p == "True" || velocity_control_p == "true";
   int baud_rate = 9600;
-  bool success =
-      driver_.init(ar_model, serial_port, baud_rate, info_.joints.size());
+  bool success = driver_.init(ar_model, serial_port, baud_rate,
+                              info_.joints.size(), velocity_control_enabled);
   if (!success) {
     return hardware_interface::CallbackReturn::ERROR;
   }
@@ -30,8 +34,10 @@ hardware_interface::CallbackReturn ARHardwareInterface::on_init(
 void ARHardwareInterface::init_variables() {
   // resize vectors
   int num_joints = info_.joints.size();
-  actuator_commands_.resize(num_joints);
+  actuator_pos_commands_.resize(num_joints);
+  actuator_vel_commands_.resize(num_joints);
   actuator_positions_.resize(num_joints);
+  actuator_velocities_.resize(num_joints);
   joint_positions_.resize(num_joints);
   joint_velocities_.resize(num_joints);
   joint_efforts_.resize(num_joints);
@@ -68,14 +74,6 @@ hardware_interface::CallbackReturn ARHardwareInterface::on_activate(
     calibrated_ = true;
   }
 
-  // init position commands at current positions
-  driver_.getJointPositions(actuator_positions_);
-  for (size_t i = 0; i < info_.joints.size(); ++i) {
-    // apply offsets, convert from deg to rad for moveit
-    joint_positions_[i] = degToRad(actuator_positions_[i] + joint_offsets_[i]);
-    joint_position_commands_[i] = joint_positions_[i];
-  }
-
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
@@ -92,6 +90,8 @@ ARHardwareInterface::export_state_interfaces() {
   for (size_t i = 0; i < info_.joints.size(); ++i) {
     state_interfaces.emplace_back(info_.joints[i].name, "position",
                                   &joint_positions_[i]);
+    state_interfaces.emplace_back(info_.joints[i].name, "velocity",
+                                  &joint_velocities_[i]);
   }
   return state_interfaces;
 }
@@ -102,6 +102,8 @@ ARHardwareInterface::export_command_interfaces() {
   for (size_t i = 0; i < info_.joints.size(); ++i) {
     command_interfaces.emplace_back(info_.joints[i].name, "position",
                                     &joint_position_commands_[i]);
+    command_interfaces.emplace_back(info_.joints[i].name, "velocity",
+                                    &joint_velocity_commands_[i]);
   }
   return command_interfaces;
 }
@@ -109,18 +111,12 @@ ARHardwareInterface::export_command_interfaces() {
 hardware_interface::return_type ARHardwareInterface::read(
     const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
   driver_.getJointPositions(actuator_positions_);
+  driver_.getJointVelocities(actuator_velocities_);
   for (size_t i = 0; i < info_.joints.size(); ++i) {
     // apply offsets, convert from deg to rad for moveit
     joint_positions_[i] = degToRad(actuator_positions_[i] + joint_offsets_[i]);
+    joint_velocities_[i] = degToRad(actuator_velocities_[i]);
   }
-  std::string logInfo = "Joint Pos: ";
-  for (size_t i = 0; i < info_.joints.size(); i++) {
-    std::stringstream jointPositionStm;
-    jointPositionStm << std::fixed << std::setprecision(2)
-                     << radToDeg(joint_positions_[i]);
-    logInfo += info_.joints[i].name + ": " + jointPositionStm.str() + " | ";
-  }
-  RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str());
   return hardware_interface::return_type::OK;
 }
 
@@ -128,24 +124,19 @@ hardware_interface::return_type ARHardwareInterface::write(
     const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
   for (size_t i = 0; i < info_.joints.size(); ++i) {
     // convert from rad to deg, apply offsets
-    actuator_commands_[i] =
+    actuator_pos_commands_[i] =
         radToDeg(joint_position_commands_[i]) - joint_offsets_[i];
+    actuator_vel_commands_[i] = radToDeg(joint_velocity_commands_[i]);
   }
-  std::string logInfo = "Joint Cmd: ";
-  for (size_t i = 0; i < info_.joints.size(); i++) {
-    std::stringstream jointPositionStm;
-    jointPositionStm << std::fixed << std::setprecision(2)
-                     << radToDeg(joint_position_commands_[i]);
-    logInfo += info_.joints[i].name + ": " + jointPositionStm.str() + " | ";
-  }
-  RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str());
-  driver_.update(actuator_commands_, actuator_positions_);
+  driver_.update(actuator_pos_commands_, actuator_vel_commands_,
+                 actuator_positions_, actuator_velocities_);
   if (driver_.isEStopped()) {
     std::string logWarn =
         "Hardware in EStop state. To reset the EStop "
         "reactivate the hardware component using 'ros2 "
         "run ar_hardware_interface reset_estop.sh'.";
     RCLCPP_WARN(logger_, logWarn.c_str());
+
     return hardware_interface::return_type::ERROR;
   }
   return hardware_interface::return_type::OK;
diff --git a/ar_hardware_interface/src/arduino_nano_driver.cpp b/ar_hardware_interface/src/arduino_nano_driver.cpp
index 6c99069..606a0a2 100644
--- a/ar_hardware_interface/src/arduino_nano_driver.cpp
+++ b/ar_hardware_interface/src/arduino_nano_driver.cpp
@@ -61,7 +61,7 @@ bool ArduinoNanoDriver::transmit(std::string msg, std::string& err) {
   if (!ec) {
     return true;
   } else {
-    err = "Error in transmit";
+    err = ec.message();
     return false;
   }
 }
diff --git a/ar_hardware_interface/src/teensy_driver.cpp b/ar_hardware_interface/src/teensy_driver.cpp
index 9866643..222716e 100644
--- a/ar_hardware_interface/src/teensy_driver.cpp
+++ b/ar_hardware_interface/src/teensy_driver.cpp
@@ -1,14 +1,15 @@
 #include "ar_hardware_interface/teensy_driver.hpp"
 
 #include <chrono>
+#include <stdexcept>
 #include <thread>
 
-#define FW_VERSION "0.1.0"
+#define FW_VERSION "2.0.0"
 
 namespace ar_hardware_interface {
 
 bool TeensyDriver::init(std::string ar_model, std::string port, int baudrate,
-                        int num_joints) {
+                        int num_joints, bool velocity_control_enabled) {
   // @TODO read version from config
   version_ = FW_VERSION;
   ar_model_ = ar_model;
@@ -30,7 +31,7 @@ bool TeensyDriver::init(std::string ar_model, std::string port, int baudrate,
   }
 
   initialised_ = false;
-  std::string msg = "STA" + version_ + +"B" + ar_model_ + "\n";
+  std::string msg = "STA" + version_ + "B" + ar_model_ + "\n";
 
   while (!initialised_) {
     RCLCPP_INFO(logger_, "Waiting for response from Teensy on port %s",
@@ -44,34 +45,52 @@ bool TeensyDriver::init(std::string ar_model, std::string port, int baudrate,
   // initialise joint and encoder calibration
   num_joints_ = num_joints;
   joint_positions_deg_.resize(num_joints_);
+  joint_velocities_deg_.resize(num_joints_);
   enc_calibrations_.resize(num_joints_);
+  velocity_control_enabled_ = velocity_control_enabled;
   is_estopped_ = false;
   return true;
 }
 
 TeensyDriver::TeensyDriver() : serial_port_(io_service_) {}
 
-void TeensyDriver::setStepperSpeed(std::vector<double>& max_speed,
-                                   std::vector<double>& max_accel) {
-  std::string outMsg = "SS";
-  for (int i = 0, charIdx = 0; i < num_joints_; ++i, charIdx += 2) {
-    outMsg += 'A' + charIdx;
-    outMsg += std::to_string(max_speed[i]);
-    outMsg += 'A' + charIdx + 1;
-    outMsg += std::to_string(max_accel[i]);
-  }
-  outMsg += "\n";
-  exchange(outMsg);
-}
-
 // Update between hardware interface and hardware driver
 void TeensyDriver::update(std::vector<double>& pos_commands,
-                          std::vector<double>& joint_positions) {
+                          std::vector<double>& vel_commands,
+                          std::vector<double>& joint_positions,
+                          std::vector<double>& joint_velocities) {
+  // log pos_commands
+  std::string logInfo = "Joint Pos Cmd: ";
+  for (int i = 0; i < num_joints_; i++) {
+    std::stringstream jointPositionStm;
+    jointPositionStm << std::fixed << std::setprecision(2) << pos_commands[i];
+    logInfo += std::to_string(i) + ": " + jointPositionStm.str() + " | ";
+  }
+  RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str());
+
+  // log vel_commands
+  logInfo = "Joint Vel Cmd: ";
+  for (int i = 0; i < num_joints_; i++) {
+    std::stringstream jointVelocityStm;
+    jointVelocityStm << std::fixed << std::setprecision(2) << vel_commands[i];
+    logInfo += std::to_string(i) + ": " + jointVelocityStm.str() + " | ";
+  }
+  RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str());
+
+  std::string outMsg = "";
   // construct update message
-  std::string outMsg = "MT";
-  for (int i = 0; i < num_joints_; ++i) {
-    outMsg += 'A' + i;
-    outMsg += std::to_string(pos_commands[i]);
+  if (velocity_control_enabled_) {
+    outMsg += "MV";
+    for (int i = 0; i < num_joints_; ++i) {
+      outMsg += 'A' + i;
+      outMsg += std::to_string(vel_commands[i]);
+    }
+  } else {
+    outMsg += "MT";
+    for (int i = 0; i < num_joints_; ++i) {
+      outMsg += 'A' + i;
+      outMsg += std::to_string(pos_commands[i]);
+    }
   }
   outMsg += "\n";
 
@@ -79,6 +98,27 @@ void TeensyDriver::update(std::vector<double>& pos_commands,
   exchange(outMsg);
 
   joint_positions = joint_positions_deg_;
+  joint_velocities = joint_velocities_deg_;
+
+  // print joint_positions
+  logInfo = "Joint Pos: ";
+  for (int i = 0; i < num_joints_; i++) {
+    std::stringstream jointPositionStm;
+    jointPositionStm << std::fixed << std::setprecision(2)
+                     << joint_positions[i];
+    logInfo += std::to_string(i) + ": " + jointPositionStm.str() + " | ";
+  }
+  RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str());
+
+  // print joint_velocities
+  logInfo = "Joint Vel: ";
+  for (int i = 0; i < num_joints_; i++) {
+    std::stringstream jointVelocityStm;
+    jointVelocityStm << std::fixed << std::setprecision(2)
+                     << joint_velocities[i];
+    logInfo += std::to_string(i) + ": " + jointVelocityStm.str() + " | ";
+  }
+  RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str());
 }
 
 bool TeensyDriver::calibrateJoints() {
@@ -99,11 +139,15 @@ bool TeensyDriver::resetEStop() {
   return !is_estopped_;
 }
 
-bool TeensyDriver::isEStopped() {
-  return is_estopped_;
+bool TeensyDriver::isEStopped() { return is_estopped_; }
+
+void TeensyDriver::getJointVelocities(std::vector<double>& joint_velocities) {
+  // get current joint velocities
+  std::string msg = "JV\n";
+  exchange(msg);
+  joint_velocities = joint_velocities_deg_;
 }
 
-// Send specific commands
 bool TeensyDriver::sendCommand(std::string outMsg) { return exchange(outMsg); }
 
 // Send msg to board and collect data
@@ -116,30 +160,40 @@ bool TeensyDriver::exchange(std::string outMsg) {
     return false;
   }
 
-  receive(inMsg);
-  // parse msg
-  std::string header = inMsg.substr(0, 2);
-  if (header == "ST") {
-    // init acknowledgement
-    checkInit(inMsg);
-  } else if (header == "JC") {
-    if (!succeeded(inMsg)) {
-      return false;
+  while (true) {
+    receive(inMsg);
+    std::string header = inMsg.substr(0, 2);
+
+    if (header == "DB") {
+      // debug message
+      RCLCPP_DEBUG(logger_, "Debug message: %s", inMsg.c_str());
+    } else {
+      if (header == "ST") {
+        // init acknowledgement
+        checkInit(inMsg);
+      } else if (header == "JC") {
+        // encoder calibration values
+        updateEncoderCalibrations(inMsg);
+      } else if (header == "JP") {
+        // encoder steps
+        updateJointPositions(inMsg);
+      } else if (header == "JV") {
+        // encoder steps
+        updateJointVelocities(inMsg);
+      } else if (header == "ES") {
+        // estop status
+        updateEStopStatus(inMsg);
+      } else if (header == "ER") {
+        // error message
+        RCLCPP_INFO(logger_, "ERROR message: %s", inMsg.c_str());
+        return false;
+      } else {
+        // unknown header
+        RCLCPP_WARN(logger_, "Unknown header %s", header.c_str());
+        return false;
+      }
+      return true;
     }
-    // encoder calibration values
-    updateEncoderCalibrations(inMsg);
-  } else if (header == "JP") {
-    // encoder steps
-    updateJointPositions(inMsg);
-  } else if (header == "ES") {
-    // estop status
-    updateEStopStatus(inMsg);
-  } else if (header == "DB") {
-    // debug message
-    RCLCPP_DEBUG(logger_, "Debug message: %s", inMsg.c_str());
-  } else {
-    // unknown header
-    RCLCPP_WARN(logger_, "Unknown header %s", header.c_str());
   }
   return true;
 }
@@ -153,7 +207,7 @@ bool TeensyDriver::transmit(std::string msg, std::string& err) {
   if (!ec) {
     return true;
   } else {
-    err = "Error in transmit";
+    err = ec.message();
     return false;
   }
 }
@@ -198,56 +252,51 @@ void TeensyDriver::checkInit(std::string msg) {
   }
 }
 
-void TeensyDriver::updateEncoderCalibrations(std::string msg) {
-  // Skip the first 5 letters as they are the header: JCRES
-  size_t idx1 = msg.find("A", 5) + 1;
-  size_t idx2 = msg.find("B", 5) + 1;
-  size_t idx3 = msg.find("C", 5) + 1;
-  size_t idx4 = msg.find("D", 5) + 1;
-  size_t idx5 = msg.find("E", 5) + 1;
-  size_t idx6 = msg.find("F", 5) + 1;
-  enc_calibrations_[0] = std::stoi(msg.substr(idx1, idx2 - idx1));
-  enc_calibrations_[1] = std::stoi(msg.substr(idx2, idx3 - idx2));
-  enc_calibrations_[2] = std::stoi(msg.substr(idx3, idx4 - idx3));
-  enc_calibrations_[3] = std::stoi(msg.substr(idx4, idx5 - idx4));
-  enc_calibrations_[4] = std::stoi(msg.substr(idx5, idx6 - idx5));
-  enc_calibrations_[5] = std::stoi(msg.substr(idx6));
-
-  // @TODO update config file
-  RCLCPP_INFO(logger_, "Successfully updated encoder calibrations");
+void TeensyDriver::updateJointPositions(const std::string msg) {
+  parseValuesToVector(msg, joint_positions_deg_);
 }
 
-void TeensyDriver::updateJointPositions(std::string msg) {
-  size_t idx1 = msg.find("A", 2) + 1;
-  size_t idx2 = msg.find("B", 2) + 1;
-  size_t idx3 = msg.find("C", 2) + 1;
-  size_t idx4 = msg.find("D", 2) + 1;
-  size_t idx5 = msg.find("E", 2) + 1;
-  size_t idx6 = msg.find("F", 2) + 1;
-  joint_positions_deg_[0] = std::stod(msg.substr(idx1, idx2 - idx1));
-  joint_positions_deg_[1] = std::stod(msg.substr(idx2, idx3 - idx2));
-  joint_positions_deg_[2] = std::stod(msg.substr(idx3, idx4 - idx3));
-  joint_positions_deg_[3] = std::stod(msg.substr(idx4, idx5 - idx4));
-  joint_positions_deg_[4] = std::stod(msg.substr(idx5, idx6 - idx5));
-  joint_positions_deg_[5] = std::stod(msg.substr(idx6));
+void TeensyDriver::updateJointVelocities(const std::string msg) {
+  parseValuesToVector(msg, joint_velocities_deg_);
 }
 
 void TeensyDriver::updateEStopStatus(std::string msg) {
   is_estopped_ = msg.substr(2) == "1" ? true : false;
 }
 
-bool TeensyDriver::succeeded(std::string msg) {
-  size_t res_idx = msg.find("RES", 2) + 3;
-  if (res_idx != std::string::npos && msg[res_idx] == '0') {
-    const std::string errmsg_code = "MSG";
-    size_t idx = msg.find(errmsg_code, 5);
-    if (idx != std::string::npos) {
-      RCLCPP_ERROR(logger_, msg.substr(idx + errmsg_code.size()).c_str());
+void TeensyDriver::updateEncoderCalibrations(const std::string msg) {
+  parseValuesToVector(msg, enc_calibrations_);
+}
+
+template <typename T>
+void TeensyDriver::parseValuesToVector(const std::string msg,
+                                       std::vector<T>& values) {
+  values.clear();
+  size_t prevIdx = msg.find('A', 2) + 1;
+
+  for (size_t i = 1;; ++i) {
+    char currentIdentifier = 'A' + i;
+    size_t currentIdx = msg.find(currentIdentifier, 2);
+
+    try {
+      if (currentIdx == std::string::npos) {
+        if constexpr (std::is_same<T, int>::value) {
+          values.push_back(std::stoi(msg.substr(prevIdx)));
+        } else if constexpr (std::is_same<T, double>::value) {
+          values.push_back(std::stod(msg.substr(prevIdx)));
+        }
+        break;
+      }
+      if constexpr (std::is_same<T, int>::value) {
+        values.push_back(std::stoi(msg.substr(prevIdx, currentIdx - prevIdx)));
+      } else if constexpr (std::is_same<T, double>::value) {
+        values.push_back(std::stod(msg.substr(prevIdx, currentIdx - prevIdx)));
+      }
+    } catch (const std::invalid_argument&) {
+      RCLCPP_WARN(logger_, "Invalid argument, can't parse %s", msg.c_str());
     }
-    return false;
+    prevIdx = currentIdx + 1;
   }
-
-  return true;
 }
 
 }  // namespace ar_hardware_interface
diff --git a/ar_hardware_interface/urdf/ar.urdf.xacro b/ar_hardware_interface/urdf/ar.urdf.xacro
index cda6121..c3d3abb 100644
--- a/ar_hardware_interface/urdf/ar.urdf.xacro
+++ b/ar_hardware_interface/urdf/ar.urdf.xacro
@@ -22,6 +22,7 @@
     calibrate="$(arg calibrate)"
     robot_parameters_file="$(find ar_description)/config/$(arg ar_model).yaml"
     joint_offset_parameters_file="$(find ar_hardware_interface)/config/joint_offsets/$(arg ar_model).yaml"
+    driver_parameters_file="$(find ar_hardware_interface)/config/driver.yaml"
   />
 
   <xacro:if value="$(arg include_gripper)">
diff --git a/ar_microcontrollers/AR4_teensy/AR4_teensy.ino b/ar_microcontrollers/AR4_teensy/AR4_teensy.ino
index 2d832e7..521083a 100644
--- a/ar_microcontrollers/AR4_teensy/AR4_teensy.ino
+++ b/ar_microcontrollers/AR4_teensy/AR4_teensy.ino
@@ -1,4 +1,5 @@
 #include <AccelStepper.h>
+#include <Bounce2.h>
 #include <Encoder.h>
 #include <avr/pgmspace.h>
 #include <math.h>
@@ -6,7 +7,7 @@
 #include <map>
 
 // Firmware version
-const char* VERSION = "0.1.0";
+const char* VERSION = "2.0.0";
 
 // Model of the AR4, i.e. mk1, mk2, mk3
 String MODEL = "";
@@ -54,17 +55,19 @@ int JOINT_LIMIT_MAX_MK3[] = {170, 90, 52, 180, 105, 180};
 // ROS Driver Params
 ///////////////////////////////////////////////////////////////////////////////
 
-// roughly equals 0, 0, 0, 0, 0, 0 degrees
-std::map<String, const int*> REST_ENC_POSITIONS;
-const int REST_ENC_POSITIONS_MK1[] = {75556, 23333, 49444, 70489, 11477, 34311};
-const int REST_ENC_POSITIONS_MK2[] = {75556, 23333, 49444, 70489, 11477, 34311};
-const int REST_ENC_POSITIONS_MK3[] = {75556, 23333, 49444, 89600, 11477, 40000};
+// roughly equals 0, 0, 0, 0, 0, 0 degrees without any user-defined offsets.
+std::map<String, const int*> REST_MOTOR_STEPS;
+const int REST_MOTOR_STEPS_MK1[] = {7555, 2333, 4944, 7049, 2295, 3431};
+const int REST_MOTOR_STEPS_MK2[] = {7555, 2333, 4944, 7049, 2295, 3431};
+const int REST_MOTOR_STEPS_MK3[] = {7555, 2333, 4944, 8960, 2295, 4000};
 
 enum SM { STATE_TRAJ, STATE_ERR };
 SM STATE = STATE_TRAJ;
 
 const int NUM_JOINTS = 6;
 AccelStepper stepperJoints[NUM_JOINTS];
+Bounce2::Button limitSwitches[NUM_JOINTS];
+const int DEBOUCE_INTERVAL = 10;  // ms
 
 // calibration settings
 const int LIMIT_SWITCH_HIGH[] = {
@@ -74,20 +77,17 @@ const int CAL_DIR[] = {-1, -1, 1,
 const int CAL_SPEED = 500;          // motor steps per second
 const int CAL_SPEED_MULT[] = {
     1, 1, 1, 2, 1, 1};  // multiplier to account for motor steps/rev
+// num of encoder steps in range of motion of joint
+int ENC_RANGE_STEPS[NUM_JOINTS];
 
 // speed and acceleration settings
-float JOINT_MAX_SPEED[] = {30.0, 30.0, 30.0, 30.0, 30.0, 30.0};  // deg/s
-float JOINT_MAX_ACCEL[] = {10.0, 10.0, 10.0, 10.0, 10.0, 10.0};  // deg/s^2
+float JOINT_MAX_SPEED[] = {60.0, 60.0, 60.0, 60.0, 60.0, 60.0};  // deg/s
+float JOINT_MAX_ACCEL[] = {30.0, 30.0, 30.0, 30.0, 30.0, 30.0};  // deg/s^2
 char JOINT_NAMES[] = {'A', 'B', 'C', 'D', 'E', 'F'};
 
-// num of encoder steps in range of motion of joint
-int ENC_RANGE_STEPS[NUM_JOINTS];
-
 bool estop_pressed = false;
 
-void estopPressed() {
-  estop_pressed = true;
-}
+void estopPressed() { estop_pressed = true; }
 
 void resetEstop() {
   // if ESTOP button is pressed still, do not reset the flag!
@@ -101,6 +101,7 @@ void resetEstop() {
     // _stepInterval and _n to 0, which is required to avoid a jerk resume
     // when Estop is reset after interruption of an accelerated motion
     stepperJoints[i].setCurrentPosition(stepperJoints[i].currentPosition());
+    stepperJoints[i].setSpeed(0);
   }
 
   estop_pressed = false;
@@ -129,11 +130,9 @@ void setup() {
   JOINT_LIMIT_MAX["mk2"] = JOINT_LIMIT_MAX_MK2;
   JOINT_LIMIT_MAX["mk3"] = JOINT_LIMIT_MAX_MK3;
 
-  REST_ENC_POSITIONS["mk1"] = REST_ENC_POSITIONS_MK1;
-  REST_ENC_POSITIONS["mk2"] = REST_ENC_POSITIONS_MK2;
-  REST_ENC_POSITIONS["mk3"] = REST_ENC_POSITIONS_MK3;
-
-  Serial.begin(9600);
+  REST_MOTOR_STEPS["mk1"] = REST_MOTOR_STEPS_MK1;
+  REST_MOTOR_STEPS["mk2"] = REST_MOTOR_STEPS_MK2;
+  REST_MOTOR_STEPS["mk3"] = REST_MOTOR_STEPS_MK3;
 
   for (int i = 0; i < NUM_JOINTS; ++i) {
     pinMode(STEP_PINS[i], OUTPUT);
@@ -141,8 +140,17 @@ void setup() {
     pinMode(LIMIT_PINS[i], INPUT);
   }
 
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    limitSwitches[i] = Bounce2::Button();
+    limitSwitches[i].attach(LIMIT_PINS[i], INPUT);
+    limitSwitches[i].interval(DEBOUCE_INTERVAL);
+    limitSwitches[i].setPressedState(LIMIT_SWITCH_HIGH[i]);
+  }
+
   pinMode(ESTOP_PIN, INPUT_PULLUP);
   attachInterrupt(digitalPinToInterrupt(ESTOP_PIN), estopPressed, FALLING);
+
+  Serial.begin(9600);
 }
 
 void setupSteppersMK1() {
@@ -228,6 +236,11 @@ bool initStateTraj(String inData) {
   return false;
 }
 
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+
 void readMotorSteps(int* motorSteps) {
   for (int i = 0; i < NUM_JOINTS; ++i) {
     motorSteps[i] = encPos[i].read() / ENC_MULT[i];
@@ -255,35 +268,100 @@ String JointPosToString(double* jointPos) {
   return out;
 }
 
-void updateStepperSpeed(String inData) {
-  int idxSpeedJ1 = inData.indexOf('A');
-  int idxAccelJ1 = inData.indexOf('B');
-  int idxSpeedJ2 = inData.indexOf('C');
-  int idxAccelJ2 = inData.indexOf('D');
-  int idxSpeedJ3 = inData.indexOf('E');
-  int idxAccelJ3 = inData.indexOf('F');
-  int idxSpeedJ4 = inData.indexOf('G');
-  int idxAccelJ4 = inData.indexOf('H');
-  int idxSpeedJ5 = inData.indexOf('I');
-  int idxAccelJ5 = inData.indexOf('J');
-  int idxSpeedJ6 = inData.indexOf('K');
-  int idxAccelJ6 = inData.indexOf('L');
-
-  JOINT_MAX_SPEED[0] = inData.substring(idxSpeedJ1 + 1, idxAccelJ1).toFloat();
-  JOINT_MAX_ACCEL[0] = inData.substring(idxAccelJ1 + 1, idxSpeedJ2).toFloat();
-  JOINT_MAX_SPEED[1] = inData.substring(idxSpeedJ2 + 1, idxAccelJ2).toFloat();
-  JOINT_MAX_ACCEL[1] = inData.substring(idxAccelJ2 + 1, idxSpeedJ3).toFloat();
-  JOINT_MAX_SPEED[2] = inData.substring(idxSpeedJ3 + 1, idxAccelJ3).toFloat();
-  JOINT_MAX_ACCEL[2] = inData.substring(idxAccelJ3 + 1, idxSpeedJ4).toFloat();
-  JOINT_MAX_SPEED[3] = inData.substring(idxSpeedJ4 + 1, idxAccelJ4).toFloat();
-  JOINT_MAX_ACCEL[3] = inData.substring(idxAccelJ4 + 1, idxSpeedJ5).toFloat();
-  JOINT_MAX_SPEED[4] = inData.substring(idxSpeedJ5 + 1, idxAccelJ5).toFloat();
-  JOINT_MAX_ACCEL[4] = inData.substring(idxAccelJ5 + 1, idxSpeedJ6).toFloat();
-  JOINT_MAX_SPEED[5] = inData.substring(idxSpeedJ6 + 1, idxAccelJ6).toFloat();
-  JOINT_MAX_ACCEL[5] = inData.substring(idxAccelJ6 + 1).toFloat();
+String JointVelToString(double* lastVelocity) {
+  String out;
+
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    out += JOINT_NAMES[i];
+    out += String(lastVelocity[i], 6);
+  }
+
+  return out;
+}
+
+void ParseMessage(String& inData, double* cmdJointPos) {
+  for (int i = 0; i < NUM_JOINTS; i++) {
+    bool lastJoint = i == NUM_JOINTS - 1;
+    int msgIdxJ_S, msgIdxJ_E = 0;
+    msgIdxJ_S = inData.indexOf(JOINT_NAMES[i]);
+    msgIdxJ_E = (lastJoint) ? -1 : inData.indexOf(JOINT_NAMES[i + 1]);
+    if (msgIdxJ_S == -1) {
+      Serial.printf("ER: panic, missing joint %c\n", JOINT_NAMES[i]);
+      return;
+    }
+    if (msgIdxJ_E != -1) {
+      cmdJointPos[i] = inData.substring(msgIdxJ_S + 1, msgIdxJ_E).toFloat();
+    } else {
+      cmdJointPos[i] = inData.substring(msgIdxJ_S + 1).toFloat();
+    }
+  }
 }
 
-bool calibrateJoints(int* calJoints) {
+void MoveVelocity(String inData) {
+  double cmdJointVel[NUM_JOINTS];
+  ParseMessage(inData, cmdJointVel);
+
+  for (int i = 0; i < NUM_JOINTS; i++) {
+    if (abs(cmdJointVel[i]) > JOINT_MAX_SPEED[i]) {
+      Serial.printf("DB: joint %c speed %f > %f, clipping.\n", JOINT_NAMES[i],
+                    cmdJointVel[i], JOINT_MAX_SPEED[i]);
+      cmdJointVel[i] = sgn(cmdJointVel[i]) * JOINT_MAX_SPEED[i];
+    }
+    cmdJointVel[i] *= MOTOR_STEPS_PER_DEG[MODEL][i];
+    stepperJoints[i].setMaxSpeed(abs(cmdJointVel[i]));
+    stepperJoints[i].setSpeed(cmdJointVel[i]);
+    stepperJoints[i].move(sgn(cmdJointVel[i]) * __LONG_MAX__);
+  }
+}
+
+void MoveTo(const int* cmdSteps, int* motorSteps) {
+  setAllMaxSpeeds();
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    int diffEncSteps = cmdSteps[i] - motorSteps[i];
+    if (abs(diffEncSteps) > 2) {
+      int diffMotSteps = diffEncSteps * ENC_DIR[i];
+      stepperJoints[i].move(diffMotSteps);
+    }
+  }
+}
+
+void MoveTo(String inData, int* motorSteps) {
+  double cmdJointPos[NUM_JOINTS] = {0};
+  ParseMessage(inData, cmdJointPos);
+
+  for (int i = 0; i < NUM_JOINTS; i++) {
+    if (abs(cmdJointPos[i] > 380.0)) {
+      Serial.printf("ER: panic, joint %c value %f out of range\n",
+                    JOINT_NAMES[i], cmdJointPos[i]);
+      return;
+    }
+  }
+
+  // get current joint position
+  double curJointPos[NUM_JOINTS];
+  encStepsToJointPos(motorSteps, curJointPos);
+
+  // update target joint position
+  int cmdEncSteps[NUM_JOINTS] = {0};
+  jointPosToEncSteps(cmdJointPos, cmdEncSteps);
+
+  MoveTo(cmdEncSteps, motorSteps);
+}
+
+void setAllMaxSpeeds() {
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    stepperJoints[i].setMaxSpeed(JOINT_MAX_SPEED[i] *
+                                 MOTOR_STEPS_PER_DEG[MODEL][i]);
+  }
+}
+
+void updateAllLimitSwitches() {
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    limitSwitches[i].update();
+  }
+}
+
+bool moveToLimitSwitches(int* calJoints) {
   // check which joints to calibrate
   bool calAllDone = false;
   bool calJointsDone[NUM_JOINTS];
@@ -291,18 +369,18 @@ bool calibrateJoints(int* calJoints) {
     calJointsDone[i] = !calJoints[i];
   }
 
-  // first pass of calibration, fast speed
   for (int i = 0; i < NUM_JOINTS; i++) {
     stepperJoints[i].setSpeed(CAL_SPEED * CAL_SPEED_MULT[i] * CAL_DIR[i]);
   }
   unsigned long startTime = millis();
   while (!calAllDone) {
+    updateAllLimitSwitches();
     calAllDone = true;
     for (int i = 0; i < NUM_JOINTS; ++i) {
       // if joint is not calibrated yet
       if (!calJointsDone[i]) {
         // check limit switches
-        if (!reachedLimitSwitch(i)) {
+        if (!limitSwitches[i].isPressed()) {
           // limit switch not reached, continue moving
           safeRunSpeed(stepperJoints[i]);
           calAllDone = false;
@@ -322,46 +400,62 @@ bool calibrateJoints(int* calJoints) {
   return true;
 }
 
-void moveAwayFromLimitSwitch() {
+bool moveAwayFromLimitSwitch(int* calJoints) {
   for (int i = 0; i < NUM_JOINTS; i++) {
-    stepperJoints[i].setSpeed(CAL_SPEED * CAL_SPEED_MULT[i] * CAL_DIR[i] * -1);
+    if (calJoints[i]) {
+      stepperJoints[i].setSpeed(CAL_SPEED * CAL_SPEED_MULT[i] * CAL_DIR[i] *
+                                -1);
+    }
   }
-  for (int j = 0; j < 10000000; j++) {
+
+  bool limitSwitchesActive = true;
+  unsigned long startTime = millis();
+  while (limitSwitchesActive || millis() - startTime < 4000) {
+    limitSwitchesActive = false;
+    updateAllLimitSwitches();
     for (int i = 0; i < NUM_JOINTS; ++i) {
-      safeRunSpeed(stepperJoints[i]);
+      if (calJoints[i]) {
+        if (limitSwitches[i].isPressed()) {
+          limitSwitchesActive = true;
+        }
+        safeRunSpeed(stepperJoints[i]);
+      }
+    }
+
+    if (millis() - startTime > 10000) {
+      return false;
     }
   }
+
   for (int i = 0; i < NUM_JOINTS; i++) {
-    stepperJoints[i].setSpeed(0);
+    stepperJoints[i].setSpeed(0);  // redundancy
   }
-  delay(2000);
-  return;
+  delay(1000);
+  return true;
 }
 
-bool reachedLimitSwitch(int joint) {
-  int pin = LIMIT_PINS[joint];
-  // check multiple times to deal with noise
-  // possibly EMI from motor cables?
-  for (int i = 0; i < 5; ++i) {
-    if (digitalRead(pin) != LIMIT_SWITCH_HIGH[joint]) {
-      return false;
-    }
+bool moveLimitedAwayFromLimitSwitch(int* calJoints) {
+  // move the ones that already hit a limit away from it before start of
+  // calibration
+  int limitedJoints[NUM_JOINTS] = {0};
+  updateAllLimitSwitches();
+  for (int i = 0; i < NUM_JOINTS; i++) {
+    limitedJoints[i] = (calJoints[i] && limitSwitches[i].isPressed());
   }
-  return true;
+  return moveAwayFromLimitSwitch(limitedJoints);
 }
 
-void doCalibrationRoutine() {
+bool doCalibrationRoutine(String& outputMsg) {
   // calibrate all joints
   int calJoints[] = {1, 1, 1, 1, 1, 1};
-  int success = calibrateJoints(calJoints);
-  if (!success) {
-    // set all speeds to zero
-    for (int i = 0; i < NUM_JOINTS; ++i) {
-      stepperJoints[i].setSpeed(0);
-    }
-    String msg = String("JCRES0MSG") + "Failed to calibrate joints.";
-    Serial.println(msg);
-    return;
+  if (!moveLimitedAwayFromLimitSwitch(calJoints)) {
+    outputMsg = "ER: Failed to move away from limit switches at the start.";
+    return false;
+  }
+
+  if (!moveToLimitSwitches(calJoints)) {
+    outputMsg = "ER: Failed to move to limit switches.";
+    return false;
   }
 
   // record encoder steps
@@ -376,53 +470,74 @@ void doCalibrationRoutine() {
   // move away from the limit switches a bit so that if the next command
   // is in the wrong direction, the limit switches will not be run over
   // immediately and become damaged.
-  moveAwayFromLimitSwitch();
+  if (!moveAwayFromLimitSwitch(calJoints)) {
+    outputMsg = "ER: Failed to move away from limit switches.";
+    return false;
+  }
+
+  // restore original max speed
+  //
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    stepperJoints[i].setMaxSpeed(JOINT_MAX_SPEED[i] *
+                                 MOTOR_STEPS_PER_DEG[MODEL][i]);
+  }
 
   // return to original position
   int curMotorSteps[NUM_JOINTS];
   readMotorSteps(curMotorSteps);
-  // repeatedly set pos, spd, acc 3x due to some joints occasionally not
-  // moving
-  for (int j = 0; j < 3; j++) {
-    for (int i = 0; i < NUM_JOINTS; ++i) {
-      stepperJoints[i].setAcceleration(JOINT_MAX_ACCEL[i] *
-                                       MOTOR_STEPS_PER_DEG[MODEL][i]);
-      stepperJoints[i].setMaxSpeed(JOINT_MAX_SPEED[i] *
-                                   MOTOR_STEPS_PER_DEG[MODEL][i]);
-      float target_pos =
-          (REST_ENC_POSITIONS[MODEL][i] / ENC_MULT[i] - curMotorSteps[i]) *
-          ENC_DIR[i];
-      stepperJoints[i].move(target_pos);
-      safeRun(stepperJoints[i]);
+  MoveTo(REST_MOTOR_STEPS[MODEL], curMotorSteps);
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    if (estop_pressed) {
+      return false;
     }
+    stepperJoints[i].runToPosition();
   }
-  bool restPosReached = false;
-  unsigned long startTime = millis();
-  while (!restPosReached && millis() - startTime < 10000) {
-    restPosReached = true;
-    for (int i = 0; i < NUM_JOINTS; ++i) {
-      if (abs(stepperJoints[i].distanceToGo()) > 5) {
-        safeRun(stepperJoints[i]);
-        restPosReached = false;
-      }
+
+  // calibration done, send calibration values
+  // N.B. calibration values aren't used right now
+  outputMsg = String("JC") + "A" + calSteps[0] + "B" + calSteps[1] + "C" +
+              calSteps[2] + "D" + calSteps[3] + "E" + calSteps[4] + "F" +
+              calSteps[5];
+  return true;
+}
+
+void updateMotorVelocities(int* motorSteps, int* lastMotorSteps,
+                           int* checksteps, unsigned long* lastVelocityCalc,
+                           double* lastVelocity) {
+  for (int i = 0; i < NUM_JOINTS; i++) {
+    // for really small velocities we still get
+    // artifacts, but quite manageable now!
+
+    if (micros() - lastVelocityCalc[i] < 5000) {
+      // we want to trigger calculation only after x ms but
+      // immediately when steps change after that
+      checksteps[i] = motorSteps[i];
+      continue;
     }
-  }
-  if (!restPosReached) {
-    // set all speeds to zero
-    for (int i = 0; i < NUM_JOINTS; ++i) {
-      stepperJoints[i].setSpeed(0);
-      safeRun(stepperJoints[i]);
+
+    // have to add some sort of outlier-filter here , maybe moving average ..
+    if (abs(stepperJoints[i].speed() / MOTOR_STEPS_PER_DEG[MODEL][i]) < 5) {
+      // NB! trying to fix artifacts at low velocity
+      if (abs(motorSteps[i] - checksteps[i]) > 0) {
+        lastVelocity[i] =
+            stepperJoints[i].speed() / MOTOR_STEPS_PER_DEG[MODEL][i];
+        lastMotorSteps[i] = motorSteps[i];
+        lastVelocityCalc[i] = micros();
+      } else if (stepperJoints[i].speed() == 0) {
+        lastVelocity[i] = 0;
+      }
+    } else {
+      unsigned long currentMicros = micros();
+      double delta = (currentMicros - lastVelocityCalc[i]);
+      if (abs(motorSteps[i] - checksteps[i]) > 0) {
+        // calculate TRUE motor velocity
+        lastVelocity[i] = ENC_DIR[i] * (motorSteps[i] - lastMotorSteps[i]) /
+                          MOTOR_STEPS_PER_DEG[MODEL][i] / (delta / 1000000.0);
+        lastMotorSteps[i] = motorSteps[i];
+        lastVelocityCalc[i] = currentMicros;
+      }
     }
-    String msg = String("JCRES0MSG") + "Failed to return to rest position.";
-    Serial.println(msg);
-    return;
   }
-
-  // calibration done, send calibration values
-  String msg = String("JCRES1") + "A" + calSteps[0] + "B" + calSteps[1] + "C" +
-               calSteps[2] + "D" + calSteps[3] + "E" + calSteps[4] + "F" +
-               calSteps[5];
-  Serial.println(msg);
 }
 
 void stateTRAJ() {
@@ -432,12 +547,16 @@ void stateTRAJ() {
   // initialise joint steps
   double curJointPos[NUM_JOINTS];
   int curMotorSteps[NUM_JOINTS];
+  int lastMotorSteps[NUM_JOINTS];
+  int checksteps[NUM_JOINTS];
+  double lastVelocity[NUM_JOINTS];
+  unsigned long lastVelocityCalc[NUM_JOINTS];
+
   readMotorSteps(curMotorSteps);
 
-  double cmdJointPos[NUM_JOINTS];
-  int cmdEncSteps[NUM_JOINTS];
   for (int i = 0; i < NUM_JOINTS; ++i) {
-    cmdEncSteps[i] = curMotorSteps[i];
+    lastVelocityCalc[i] = micros();
+    lastMotorSteps[i] = curMotorSteps[i];
   }
 
   // start loop
@@ -449,6 +568,12 @@ void stateTRAJ() {
       inData += received;
     }
 
+    if (MODEL != "") {
+      readMotorSteps(curMotorSteps);
+      updateMotorVelocities(curMotorSteps, lastMotorSteps, checksteps,
+                            lastVelocityCalc, lastVelocity);
+    }
+
     // process message when new line character is received
     if (received == '\n') {
       String function = inData.substring(0, 2);
@@ -464,66 +589,48 @@ void stateTRAJ() {
       }
 
       if (function == "MT") {
-        // update trajectory information
-        readMotorSteps(curMotorSteps);
+        // clear speed counter
+        for (int i = 0; i < NUM_JOINTS; i++) {
+          if (stepperJoints[i].speed() == 0) {
+            lastVelocityCalc[i] = micros();
+          }
+        }
+
+        MoveTo(inData, curMotorSteps);
 
         // update the host about estop state
         String msg = String("ES") + estop_pressed;
         Serial.println(msg);
 
-        // get new position commands
-        int msgIdxJ1 = inData.indexOf('A');
-        int msgIdxJ2 = inData.indexOf('B');
-        int msgIdxJ3 = inData.indexOf('C');
-        int msgIdxJ4 = inData.indexOf('D');
-        int msgIdxJ5 = inData.indexOf('E');
-        int msgIdxJ6 = inData.indexOf('F');
-        cmdJointPos[0] = inData.substring(msgIdxJ1 + 1, msgIdxJ2).toFloat();
-        cmdJointPos[1] = inData.substring(msgIdxJ2 + 1, msgIdxJ3).toFloat();
-        cmdJointPos[2] = inData.substring(msgIdxJ3 + 1, msgIdxJ4).toFloat();
-        cmdJointPos[3] = inData.substring(msgIdxJ4 + 1, msgIdxJ5).toFloat();
-        cmdJointPos[4] = inData.substring(msgIdxJ5 + 1, msgIdxJ6).toFloat();
-        cmdJointPos[5] = inData.substring(msgIdxJ6 + 1).toFloat();
-        jointPosToEncSteps(cmdJointPos, cmdEncSteps);
-
-        // update target joint positions
-        readMotorSteps(curMotorSteps);
-        for (int i = 0; i < NUM_JOINTS; ++i) {
-          int diffEncSteps = cmdEncSteps[i] - curMotorSteps[i];
-          if (abs(diffEncSteps) > 2) {
-            int diffMotSteps = diffEncSteps * ENC_DIR[i];
-            // if (diffMotSteps < MOTOR_STEPS_PER_REV[i]) {
-            //   // for the last rev of motor, introduce artificial
-            //   decceleration
-            //   // to help prevent overshoot
-            //   diffMotSteps = diffMotSteps / 2;
-            // }
-            stepperJoints[i].move(diffMotSteps);
-            safeRun(stepperJoints[i]);
+      } else if (function == "MV") {
+        // clear speed counter
+        for (int i = 0; i < NUM_JOINTS; i++) {
+          if (stepperJoints[i].speed() == 0) {
+            lastVelocityCalc[i] = micros();
           }
         }
-      } else if (function == "JC") {
-        doCalibrationRoutine();
+
+        MoveVelocity(inData);
+
+        // update the host about estop state
+        String msg = String("ES") + estop_pressed;
+        Serial.println(msg);
+
       } else if (function == "JP") {
         readMotorSteps(curMotorSteps);
         encStepsToJointPos(curMotorSteps, curJointPos);
         String msg = String("JP") + JointPosToString(curJointPos);
         Serial.println(msg);
-      } else if (function == "SS") {
-        updateStepperSpeed(inData);
-        // set motor speed and acceleration
-        for (int i = 0; i < NUM_JOINTS; ++i) {
-          stepperJoints[i].setAcceleration(JOINT_MAX_ACCEL[i] *
-                                           MOTOR_STEPS_PER_DEG[MODEL][i]);
-          stepperJoints[i].setMaxSpeed(JOINT_MAX_SPEED[i] *
-                                       MOTOR_STEPS_PER_DEG[MODEL][i]);
+      } else if (function == "JV") {
+        String msg = String("JV") + JointVelToString(lastVelocity);
+        Serial.println(msg);
+      } else if (function == "JC") {
+        String msg;
+        if (!doCalibrationRoutine(msg)) {
+          for (int i = 0; i < NUM_JOINTS; ++i) {
+            stepperJoints[i].setSpeed(0);
+          }
         }
-        // read current joint positions
-        readMotorSteps(curMotorSteps);
-        encStepsToJointPos(curMotorSteps, curJointPos);
-
-        // update host with joint positions
-        String msg = String("JP") + JointPosToString(curJointPos);
         Serial.println(msg);
       } else if (function == "RE") {
         resetEstop();
@@ -534,6 +641,7 @@ void stateTRAJ() {
 
       inData = "";  // clear message
     }
+
     for (int i = 0; i < NUM_JOINTS; ++i) {
       safeRun(stepperJoints[i]);
     }
@@ -547,7 +655,7 @@ void stateERR() {
   }
 
   while (STATE == STATE_ERR) {
-    Serial.println("DB: Unrecoverable error state entered. Please reset.");
+    Serial.println("ER: Unrecoverable error state entered. Please reset.");
     delay(1000);
   }
 }
diff --git a/ar_moveit_config/config/joint_limits.yaml b/ar_moveit_config/config/joint_limits.yaml
index 3cd511d..e461643 100644
--- a/ar_moveit_config/config/joint_limits.yaml
+++ b/ar_moveit_config/config/joint_limits.yaml
@@ -1,38 +1,60 @@
 # These limits are used by MoveIt and augment/override the definitions in
 # ur_description.
+# You can confirm parameters are loaded despite warning, by:
+# ros2 param get /move_group robot_description_planning.joint_limits.joint_1.max_deceleration
 joint_limits:
   joint_1:
     has_acceleration_limits: true
-    max_acceleration: 0.08727 # 5 deg/s^2
+    max_acceleration: 0.5236 # 30 deg/s^2
+    has_deceleration_limits: true
+    max_deceleration: 0.5236 # 30 deg/s^2
     has_velocity_limits: true
-    max_velocity: 0.2618 # 15 deg/s
+    max_velocity: 1.0472 # 60 deg/s
   joint_2:
     has_acceleration_limits: true
-    max_acceleration: 0.08727 # 5 deg/s^2
+    max_acceleration: 0.5236 # 30 deg/s^2
+    has_deceleration_limits: true
+    max_deceleration: 0.5236 # 30 deg/s^2
     has_velocity_limits: true
-    max_velocity: 0.2618 # 15 deg/s
+    max_velocity: 1.0472 # 60 deg/s
   joint_3:
     has_acceleration_limits: true
-    max_acceleration: 0.08727 # 5 deg/s^2
+    max_acceleration: 0.5236 # 30 deg/s^2
+    has_deceleration_limits: true
+    max_deceleration: 0.5236 # 30 deg/s^2
     has_velocity_limits: true
-    max_velocity: 0.2618 # 15 deg/s
+    max_velocity: 1.0472 # 60 deg/s
   joint_4:
     has_acceleration_limits: true
-    max_acceleration: 0.08727 # 5 deg/s^2
+    max_acceleration: 0.5236 # 30 deg/s^2
+    has_deceleration_limits: true
+    max_deceleration: 0.5236 # 30 deg/s^2
     has_velocity_limits: true
-    max_velocity: 0.2618 # 15 deg/s
+    max_velocity: 1.0472 # 60 deg/s
   joint_5:
     has_acceleration_limits: true
-    max_acceleration: 0.174533 # 10 deg/s^2
+    max_acceleration: 0.5236 # 30 deg/s^2
+    has_deceleration_limits: true
+    max_deceleration: 0.5236 # 30 deg/s^2
     has_velocity_limits: true
-    max_velocity: 0.261799 # 30 deg/s
+    max_velocity: 1.0472 # 60 deg/s
   joint_6:
     has_acceleration_limits: true
-    max_acceleration: 0.174533 # 10 deg/s^2
+    max_acceleration: 0.5236 # 30 deg/s^2
+    has_deceleration_limits: true
+    max_deceleration: 0.5236 # 30 deg/s^2
     has_velocity_limits: true
-    max_velocity: 0.261799 # 30 deg/s
+    max_velocity: 1.0472 # 60 deg/s
   gripper_jaw1_joint:
     has_acceleration_limits: true
-    max_acceleration: 0.01 # m/s^2
+    max_acceleration: 0.1 # m/s^2
+    has_deceleration_limits: true
+    max_deceleration: 0.1 # m/s^2
     has_velocity_limits: true
-    max_velocity: 0.02 # m/s
+    max_velocity: 0.05 # m/s
+
+cartesian_limits:
+  max_trans_vel: 0.05 # m/s
+  max_trans_acc: 0.01 # m/s^2
+  max_trans_dec: -0.01 # m/s^2
+  max_rot_vel: 0.08727 # 5 deg/s
diff --git a/ar_moveit_config/config/ompl_planning.yaml b/ar_moveit_config/config/ompl_planning.yaml
index 6dce493..675bf2c 100644
--- a/ar_moveit_config/config/ompl_planning.yaml
+++ b/ar_moveit_config/config/ompl_planning.yaml
@@ -1,5 +1,4 @@
-planning_plugins:
-  - ompl_interface/OMPLPlanner
+planning_plugin: ompl_interface/OMPLPlanner
 request_adapters: >-
   default_planner_request_adapters/AddTimeOptimalParameterization
   default_planner_request_adapters/ResolveConstraintFrames
@@ -62,6 +61,7 @@ planner_configs:
   PRMstarkConfigDefault:
     type: geometric::PRMstar
 ar_manipulator:
+  default_planner_config: RRTConnectkConfigDefault
   planner_configs:
     - SBLkConfigDefault
     - ESTkConfigDefault
@@ -78,6 +78,7 @@ ar_manipulator:
   #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
   longest_valid_segment_fraction: 0.01
 ar_gripper:
+  default_planner_config: RRTConnectkConfigDefault
   planner_configs:
     - SBLkConfigDefault
     - ESTkConfigDefault
@@ -90,3 +91,5 @@ ar_gripper:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
+
+start_state_max_bounds_error: 0.1
diff --git a/ar_moveit_config/config/pilz_planning.yaml b/ar_moveit_config/config/pilz_planning.yaml
new file mode 100644
index 0000000..e19e28b
--- /dev/null
+++ b/ar_moveit_config/config/pilz_planning.yaml
@@ -0,0 +1,14 @@
+planning_plugin: pilz_industrial_motion_planner/CommandPlanner
+request_adapters: >-
+  default_planner_request_adapters/FixWorkspaceBounds
+  default_planner_request_adapters/FixStartStateBounds
+  default_planner_request_adapters/FixStartStateCollision
+  default_planner_request_adapters/FixStartStatePathConstraints
+capabilities: >-
+  pilz_industrial_motion_planner/MoveGroupSequenceAction
+  pilz_industrial_motion_planner/MoveGroupSequenceService
+
+ar_manipulator:
+  default_planner_config: PTP
+
+start_state_max_bounds_error: 0.1
diff --git a/ar_moveit_config/launch/ar_moveit.launch.py b/ar_moveit_config/launch/ar_moveit.launch.py
index 5c5fd54..6ec896c 100644
--- a/ar_moveit_config/launch/ar_moveit.launch.py
+++ b/ar_moveit_config/launch/ar_moveit.launch.py
@@ -134,17 +134,16 @@ def generate_launch_description():
     }
 
     # Planning Configuration
-    ompl_planning_pipeline_config = {
-        "default_planning_pipeline": "ompl",
-        "planning_pipelines": ["ompl"],
-        "ompl": {
-            "planning_plugin": "ompl_interface/OMPLPlanner",
-            "start_state_max_bounds_error": 0.1,
-        }
-    }
     ompl_planning_yaml = load_yaml("ar_moveit_config",
                                    "config/ompl_planning.yaml")
-    ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml)
+    pilz_planning_yaml = load_yaml("ar_moveit_config",
+                                   "config/pilz_planning.yaml")
+    planning_pipeline_config = {
+        "default_planning_pipeline": "ompl",
+        "planning_pipelines": ["ompl", "pilz"],
+        "ompl": ompl_planning_yaml,
+        "pilz": pilz_planning_yaml,
+    }
 
     # Trajectory Execution Configuration
     controllers_yaml = load_yaml("ar_moveit_config", "config/controllers.yaml")
@@ -169,10 +168,15 @@ def generate_launch_description():
         "publish_state_updates": True,
         "publish_transforms_updates": True,
         "publish_robot_description": True,
-        "publish_robot_description_semantic": True
+        "publish_robot_description_semantic": True,
         # Two above added due to https://github.com/moveit/moveit2_tutorials/issues/528
     }
 
+    # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers
+    move_group_capabilities = {
+        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
+    }
+
     # Start the actual move_group node/action server
     move_group_node = Node(
         package="moveit_ros_move_group",
@@ -183,10 +187,11 @@ def generate_launch_description():
             robot_description_semantic,
             robot_description_kinematics,
             robot_description_planning,
-            ompl_planning_pipeline_config,
+            planning_pipeline_config,
             trajectory_execution,
             moveit_controllers,
             planning_scene_monitor_parameters,
+            move_group_capabilities,
             {
                 "use_sim_time": use_sim_time
             },
@@ -205,9 +210,10 @@ def generate_launch_description():
         parameters=[
             robot_description,
             robot_description_semantic,
-            ompl_planning_pipeline_config,
+            planning_pipeline_config,
             robot_description_kinematics,
             robot_description_planning,
+            {"use_sim_time": use_sim_time},
         ],
     )
 
diff --git a/ar_moveit_config/launch/demo.launch.py b/ar_moveit_config/launch/demo.launch.py
index 0166a40..0e0e0fb 100644
--- a/ar_moveit_config/launch/demo.launch.py
+++ b/ar_moveit_config/launch/demo.launch.py
@@ -78,19 +78,16 @@ def generate_launch_description():
     }
 
     # Planning Configuration
-    ompl_planning_pipeline_config = {
-        "default_planning_pipeline": "ompl",
-        "planning_pipelines": ["ompl"],
-        "ompl": {
-            "planning_plugin": "ompl_interface/OMPLPlanner",
-            "request_adapters":
-            """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
-            "start_state_max_bounds_error": 0.1,
-        }
-    }
     ompl_planning_yaml = load_yaml("ar_moveit_config",
                                    "config/ompl_planning.yaml")
-    ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml)
+    pilz_planning_yaml = load_yaml("ar_moveit_config",
+                                   "config/pilz_planning.yaml")
+    planning_pipeline_config = {
+        "default_planning_pipeline": "ompl",
+        "planning_pipelines": ["ompl", "pilz"],
+        "ompl": ompl_planning_yaml,
+        "pilz": pilz_planning_yaml,
+    }
 
     # Trajectory Execution Configuration
     controllers_yaml = load_yaml("ar_moveit_config", "config/controllers.yaml")
@@ -116,6 +113,11 @@ def generate_launch_description():
         "publish_transforms_updates": True,
     }
 
+    # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers
+    move_group_capabilities = {
+        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
+    }
+
     # Start the actual move_group node/action server
     run_move_group_node = Node(
         package="moveit_ros_move_group",
@@ -126,10 +128,11 @@ def generate_launch_description():
             robot_description_semantic,
             robot_description_kinematics,
             robot_description_planning,
-            ompl_planning_pipeline_config,
+            planning_pipeline_config,
             trajectory_execution,
             moveit_controllers,
             planning_scene_monitor_parameters,
+            move_group_capabilities,
         ],
     )
 
@@ -149,7 +152,7 @@ def generate_launch_description():
             robot_description_semantic,
             robot_description_kinematics,
             robot_description_planning,
-            ompl_planning_pipeline_config,
+            planning_pipeline_config,
             trajectory_execution,
             moveit_controllers,
             planning_scene_monitor_parameters,
diff --git a/ar_moveit_config/package.xml b/ar_moveit_config/package.xml
index cb7ae35..b9c7e8f 100644
--- a/ar_moveit_config/package.xml
+++ b/ar_moveit_config/package.xml
@@ -1,6 +1,6 @@
 <package format="3">
   <name>ar_moveit_config</name>
-  <version>0.1.0</version>
+  <version>2.0.0</version>
   <description>
     Configuration and launch files for using the AR4 robot with MoveIt 2
   </description>
diff --git a/ar_moveit_config/srdf/ar_macro.srdf.xacro b/ar_moveit_config/srdf/ar_macro.srdf.xacro
index 4da8e4d..ce9e88d 100644
--- a/ar_moveit_config/srdf/ar_macro.srdf.xacro
+++ b/ar_moveit_config/srdf/ar_macro.srdf.xacro
@@ -25,7 +25,7 @@
     <group_state name="upright" group="ar_manipulator">
       <joint name="joint_1" value="0" />
       <joint name="joint_2" value="-0.087" />
-      <joint name="joint_3" value="-1.34" />
+      <joint name="joint_3" value="-1.4" />
       <joint name="joint_4" value="0" />
       <joint name="joint_5" value="0" />
       <joint name="joint_6" value="0" />
diff --git a/ar_moveit_config/urdf/fake_ar.urdf.xacro b/ar_moveit_config/urdf/fake_ar.urdf.xacro
index 8aba322..536b7a4 100644
--- a/ar_moveit_config/urdf/fake_ar.urdf.xacro
+++ b/ar_moveit_config/urdf/fake_ar.urdf.xacro
@@ -21,6 +21,7 @@
     calibrate="False"
     robot_parameters_file="$(find ar_description)/config/$(arg ar_model).yaml"
     joint_offset_parameters_file="$(find ar_hardware_interface)/config/joint_offsets/$(arg ar_model).yaml"
+    driver_parameters_file="$(find ar_hardware_interface)/config/driver.yaml"
   />
   <xacro:ar_gripper_ros2_control
     name="ARGripperFakeSystem"