diff --git a/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h b/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h
index 7a828f1..edec13e 100644
--- a/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h
+++ b/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h
@@ -44,28 +44,26 @@
 
 #include <hardware_interface/internal/hardware_resource_manager.h>
 #include <hardware_interface/joint_state_interface.h>
+#include <hardware_interface/joint_command_interface.h>
 
 namespace ur_controllers
 {
-class ScaledJointHandle : public hardware_interface::JointStateHandle
+class ScaledJointHandle : public hardware_interface::JointHandle
 {
 public:
-  ScaledJointHandle() : cmd_(0), scaling_factor_(0)
+  ScaledJointHandle() : hardware_interface::JointHandle(), scaling_factor_(0)
+
   {
   }
 
   /**
    * \param js This joint's state handle
    * \param cmd A pointer to the storage for this joint's output command
+   * \param scaling_factor A pointer to the storage for this joint's scaling factor
    */
   ScaledJointHandle(const hardware_interface::JointStateHandle& js, double* cmd, double* scaling_factor)
-    : hardware_interface::JointStateHandle(js), cmd_(cmd), scaling_factor_(scaling_factor)
+    : hardware_interface::JointHandle(js, cmd), scaling_factor_(scaling_factor)
   {
-    if (cmd_ == nullptr)
-    {
-      throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
-                                                           "'. Command data pointer is null.");
-    }
     if (scaling_factor_ == nullptr)
     {
       throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
@@ -75,16 +73,6 @@ public:
 
   virtual ~ScaledJointHandle() = default;
 
-  void setCommand(double command)
-  {
-    assert(cmd_);
-    *cmd_ = command;
-  }
-  double getCommand() const
-  {
-    assert(cmd_);
-    return *cmd_;
-  }
   void setScalingFactor(double scaling_factor)
   {
     assert(scaling_factor_);
@@ -97,7 +85,6 @@ public:
   }
 
 private:
-  double* cmd_;
   double* scaling_factor_;
 };
 
diff --git a/ur_rtde_driver/config/ur10_controllers.yaml b/ur_rtde_driver/config/ur10_controllers.yaml
index 42bb62d..ebbd6be 100644
--- a/ur_rtde_driver/config/ur10_controllers.yaml
+++ b/ur_rtde_driver/config/ur10_controllers.yaml
@@ -29,7 +29,7 @@ speed_scaling_state_controller:
 
 # Joint Trajectory Controller - position based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
-pos_based_pos_traj_controller:
+scaled_pos_traj_controller:
    type: position_controllers/ScaledJointTrajectoryController
    joints:
      - shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
    stop_trajectory_duration: 0.5
    state_publish_rate: 500
    action_monitor_rate: 10
+
+pos_traj_controller:
+   type: position_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
+      elbow_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate: 500
+   action_monitor_rate: 10
diff --git a/ur_rtde_driver/config/ur10e_controllers.yaml b/ur_rtde_driver/config/ur10e_controllers.yaml
index 345f4f5..c100dfc 100644
--- a/ur_rtde_driver/config/ur10e_controllers.yaml
+++ b/ur_rtde_driver/config/ur10e_controllers.yaml
@@ -29,7 +29,7 @@ speed_scaling_state_controller:
 
 # Joint Trajectory Controller - position based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
-pos_based_pos_traj_controller:
+scaled_pos_traj_controller:
    type: position_controllers/ScaledJointTrajectoryController
    joints:
      - shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
    stop_trajectory_duration: 0.5
    state_publish_rate: 500
    action_monitor_rate: 10
+
+pos_traj_controller:
+   type: position_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
+      elbow_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate: 500
+   action_monitor_rate: 10
diff --git a/ur_rtde_driver/config/ur3_controllers.yaml b/ur_rtde_driver/config/ur3_controllers.yaml
index 42bb62d..ebbd6be 100644
--- a/ur_rtde_driver/config/ur3_controllers.yaml
+++ b/ur_rtde_driver/config/ur3_controllers.yaml
@@ -29,7 +29,7 @@ speed_scaling_state_controller:
 
 # Joint Trajectory Controller - position based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
-pos_based_pos_traj_controller:
+scaled_pos_traj_controller:
    type: position_controllers/ScaledJointTrajectoryController
    joints:
      - shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
    stop_trajectory_duration: 0.5
    state_publish_rate: 500
    action_monitor_rate: 10
+
+pos_traj_controller:
+   type: position_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
+      elbow_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate: 500
+   action_monitor_rate: 10
diff --git a/ur_rtde_driver/config/ur3e_controllers.yaml b/ur_rtde_driver/config/ur3e_controllers.yaml
index 345f4f5..c100dfc 100644
--- a/ur_rtde_driver/config/ur3e_controllers.yaml
+++ b/ur_rtde_driver/config/ur3e_controllers.yaml
@@ -29,7 +29,7 @@ speed_scaling_state_controller:
 
 # Joint Trajectory Controller - position based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
-pos_based_pos_traj_controller:
+scaled_pos_traj_controller:
    type: position_controllers/ScaledJointTrajectoryController
    joints:
      - shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
    stop_trajectory_duration: 0.5
    state_publish_rate: 500
    action_monitor_rate: 10
+
+pos_traj_controller:
+   type: position_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
+      elbow_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate: 500
+   action_monitor_rate: 10
diff --git a/ur_rtde_driver/config/ur5_controllers.yaml b/ur_rtde_driver/config/ur5_controllers.yaml
index 42bb62d..ebbd6be 100644
--- a/ur_rtde_driver/config/ur5_controllers.yaml
+++ b/ur_rtde_driver/config/ur5_controllers.yaml
@@ -29,7 +29,7 @@ speed_scaling_state_controller:
 
 # Joint Trajectory Controller - position based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
-pos_based_pos_traj_controller:
+scaled_pos_traj_controller:
    type: position_controllers/ScaledJointTrajectoryController
    joints:
      - shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
    stop_trajectory_duration: 0.5
    state_publish_rate: 500
    action_monitor_rate: 10
+
+pos_traj_controller:
+   type: position_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
+      elbow_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate: 500
+   action_monitor_rate: 10
diff --git a/ur_rtde_driver/config/ur5e_controllers.yaml b/ur_rtde_driver/config/ur5e_controllers.yaml
index 345f4f5..c100dfc 100644
--- a/ur_rtde_driver/config/ur5e_controllers.yaml
+++ b/ur_rtde_driver/config/ur5e_controllers.yaml
@@ -29,7 +29,7 @@ speed_scaling_state_controller:
 
 # Joint Trajectory Controller - position based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
-pos_based_pos_traj_controller:
+scaled_pos_traj_controller:
    type: position_controllers/ScaledJointTrajectoryController
    joints:
      - shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
    stop_trajectory_duration: 0.5
    state_publish_rate: 500
    action_monitor_rate: 10
+
+pos_traj_controller:
+   type: position_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
+      elbow_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate: 500
+   action_monitor_rate: 10
diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h
index 24a190b..d8ecfe1 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h
@@ -57,20 +57,23 @@ public:
     server_.disconnectClient();
   }
 
-  bool write(const vector6d_t& positions)
+  bool write(const vector6d_t* positions, const int32_t type = 2)
   {
     uint8_t buffer[sizeof(uint32_t) * 7];
     uint8_t* b_pos = buffer;
-    for (auto const& pos : positions)
-    {
-      int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
-      val = htobe32(val);
-      b_pos += append(b_pos, val);
-    }
 
-    // TODO: Make this a parameter: Number of allowed missed packages
-    int32_t val = htobe32(5);
-    append(b_pos, val);
+    int32_t val = htobe32(type);
+    b_pos += append(b_pos, val);
+
+    if (positions != nullptr)
+    {
+      for (auto const& pos : *positions)
+      {
+        int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
+        val = htobe32(val);
+        b_pos += append(b_pos, val);
+      }
+    }
 
     size_t written;
 
diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
index a4f0605..a8f8d58 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
@@ -133,6 +133,7 @@ protected:
   ros::Publisher program_state_pub_;
 
   bool controller_reset_necessary_;
+  bool controllers_initialized_;
 
   std::string robot_ip_;
 };
diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h
index a376286..ce29f37 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h
@@ -81,8 +81,26 @@ public:
     return rtde_frequency_;
   }
 
+  /*!
+   * \brief Writes a joint command together with a keepalive signal onto the socket being sent to
+   * the robot.
+   *
+   * \param values Desired joint positions
+   *
+   * \returns True on successful write.
+   */
   bool writeJointCommand(const vector6d_t& values);
 
+  /*!
+   * \brief Write a keepalive signal only.
+   *
+   * This signals the robot that the connection is still
+   * active in times when no commands are to be sent (e.g. no controller is active.)
+   *
+   * \returns True on successful write.
+   */
+  bool writeKeepalive();
+
   void startWatchdog();
 
 private:
diff --git a/ur_rtde_driver/launch/ur10_bringup.launch b/ur_rtde_driver/launch/ur10_bringup.launch
index b60464c..ef2d990 100644
--- a/ur_rtde_driver/launch/ur10_bringup.launch
+++ b/ur_rtde_driver/launch/ur10_bringup.launch
@@ -10,8 +10,8 @@
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="tf_prefix" default="" />
-  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
-  <arg name="stopped_controllers" default=""/>
+  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default="pos_traj_controller"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
diff --git a/ur_rtde_driver/launch/ur10e_bringup.launch b/ur_rtde_driver/launch/ur10e_bringup.launch
index dadd662..317b44c 100644
--- a/ur_rtde_driver/launch/ur10e_bringup.launch
+++ b/ur_rtde_driver/launch/ur10e_bringup.launch
@@ -11,8 +11,8 @@
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="tf_prefix" default="" />
-  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
-  <arg name="stopped_controllers" default=""/>
+  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default="pos_traj_controller"/>
   <arg name="tool_voltage" default="12"/>
   <arg name="tool_parity" default="0"/>
   <arg name="tool_baud_rate" default="115200"/>
diff --git a/ur_rtde_driver/launch/ur3_bringup.launch b/ur_rtde_driver/launch/ur3_bringup.launch
index ea0ea84..3ecd9eb 100644
--- a/ur_rtde_driver/launch/ur3_bringup.launch
+++ b/ur_rtde_driver/launch/ur3_bringup.launch
@@ -10,8 +10,8 @@
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="tf_prefix" default="" />
-  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
-  <arg name="stopped_controllers" default=""/>
+  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default="pos_traj_controller"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
diff --git a/ur_rtde_driver/launch/ur3e_bringup.launch b/ur_rtde_driver/launch/ur3e_bringup.launch
index aef3632..02ff890 100644
--- a/ur_rtde_driver/launch/ur3e_bringup.launch
+++ b/ur_rtde_driver/launch/ur3e_bringup.launch
@@ -11,8 +11,8 @@
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="tf_prefix" default="" />
-  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
-  <arg name="stopped_controllers" default=""/>
+  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default="pos_traj_controller"/>
   <arg name="tool_voltage" default="12"/>
   <arg name="tool_parity" default="0"/>
   <arg name="tool_baud_rate" default="115200"/>
diff --git a/ur_rtde_driver/launch/ur5_bringup.launch b/ur_rtde_driver/launch/ur5_bringup.launch
index a28b4ee..60a2363 100644
--- a/ur_rtde_driver/launch/ur5_bringup.launch
+++ b/ur_rtde_driver/launch/ur5_bringup.launch
@@ -10,8 +10,8 @@
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="tf_prefix" default="" />
-  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
-  <arg name="stopped_controllers" default=""/>
+  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default="pos_traj_controller"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
diff --git a/ur_rtde_driver/launch/ur5e_bringup.launch b/ur_rtde_driver/launch/ur5e_bringup.launch
index 783f916..ad46d76 100644
--- a/ur_rtde_driver/launch/ur5e_bringup.launch
+++ b/ur_rtde_driver/launch/ur5e_bringup.launch
@@ -11,8 +11,8 @@
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="tf_prefix" default="" />
-  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
-  <arg name="stopped_controllers" default=""/>
+  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default="pos_traj_controller"/>
   <arg name="tool_voltage" default="12"/>
   <arg name="tool_parity" default="0"/>
   <arg name="tool_baud_rate" default="115200"/>
diff --git a/ur_rtde_driver/launch/ur_common.launch b/ur_rtde_driver/launch/ur_common.launch
index d591941..646d378 100644
--- a/ur_rtde_driver/launch/ur_common.launch
+++ b/ur_rtde_driver/launch/ur_common.launch
@@ -8,8 +8,8 @@
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="tf_prefix" default="" />
-  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
-  <arg name="stopped_controllers" default=""/>
+  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default="pos_traj_controller"/>
   <arg name="tool_voltage" default="12"/>
   <arg name="tool_parity" default="0"/>
   <arg name="tool_baud_rate" default="115200"/>
diff --git a/ur_rtde_driver/launch/ur_control.launch b/ur_rtde_driver/launch/ur_control.launch
index 7c98798..e2a6d40 100644
--- a/ur_rtde_driver/launch/ur_control.launch
+++ b/ur_rtde_driver/launch/ur_control.launch
@@ -11,8 +11,8 @@
   <arg name="robot_ip"/>
   <arg name="kinematics_config"/>
   <arg name="tf_prefix" default="" />
-  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
-  <arg name="stopped_controllers" default=""/>
+  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default="pos_traj_controller"/>
   <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
   <arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
   <arg name="tool_voltage" default="12"/>
diff --git a/ur_rtde_driver/resources/servoj.urscript b/ur_rtde_driver/resources/servoj.urscript
index 0811219..f12b785 100644
--- a/ur_rtde_driver/resources/servoj.urscript
+++ b/ur_rtde_driver/resources/servoj.urscript
@@ -71,16 +71,20 @@ socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
 
 thread_servo = run servoThread()
 keepalive = -2
-params_mult = socket_read_binary_integer(6+1, "reverse_socket")
-keepalive = params_mult[7]
+params_mult = socket_read_binary_integer(1+6, "reverse_socket")
+keepalive = params_mult[1]
 while keepalive > 0:
   enter_critical
   socket_send_line(1, "reverse_socket")
-  params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
+  params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
   if params_mult[0] > 0:
-    keepalive = params_mult[7]
-    q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
-    set_servo_setpoint(q)
+    if params_mult[1] > 1:
+      keepalive = params_mult[1]
+      q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
+      set_servo_setpoint(q)
+    else:
+      keepalive = 1
+    end
   else:
     keepalive = keepalive - 1
   end
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index c78cc03..e65edf6 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -47,6 +47,7 @@ HardwareInterface::HardwareInterface()
   , position_controller_running_(false)
   , pausing_state_(PausingState::RUNNING)
   , pausing_ramp_up_increment_(0.01)
+  , controllers_initialized_(false)
 {
 }
 
@@ -171,8 +172,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
                                                                       &joint_velocities_[i], &joint_efforts_[i]));
 
     // Create joint position control interface
-    // pj_interface_.registerHandle(
-    // hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
+    pj_interface_.registerHandle(
+        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
     spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
         js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
   }
@@ -186,7 +187,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   // Register interfaces
   registerInterface(&js_interface_);
   registerInterface(&spj_interface_);
-  // registerInterface(&pj_interface_);
+  registerInterface(&pj_interface_);
   registerInterface(&speedsc_interface_);
   registerInterface(&fts_interface_);
 
@@ -292,13 +293,32 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio
   {
     ur_driver_->writeJointCommand(joint_position_command_);
   }
+  else
+  {
+    ur_driver_->writeKeepalive();
+  }
 }
 
 bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                                        const std::list<hardware_interface::ControllerInfo>& stop_list)
 {
-  // TODO: Implement
-  return true;
+  bool ret_val = true;
+  if (controllers_initialized_ && !isRobotProgramRunning() && !start_list.empty())
+  {
+    for (auto& controller : start_list)
+    {
+      if (!controller.claimed_resources.empty())
+      {
+        ROS_ERROR_STREAM("Robot control is currently inactive. Starting controllers that claim resources is currently "
+                         "not possible. Not starting controller '"
+                         << controller.name << "'");
+        ret_val = false;
+      }
+    }
+  }
+
+  controllers_initialized_ = true;
+  return ret_val;
 }
 
 void HardwareInterface ::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
@@ -313,6 +333,10 @@ void HardwareInterface ::doSwitch(const std::list<hardware_interface::Controller
       {
         position_controller_running_ = true;
       }
+      if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
+      {
+        position_controller_running_ = true;
+      }
     }
   }
 }
diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp
index 479fe09..d8df01b 100644
--- a/ur_rtde_driver/src/ur/ur_driver.cpp
+++ b/ur_rtde_driver/src/ur/ur_driver.cpp
@@ -138,14 +138,19 @@ bool UrDriver::writeJointCommand(const vector6d_t& values)
 {
   if (reverse_interface_active_)
   {
-    reverse_interface_->write(values);
-  }
-  else
-  {
-    return false;
+    return reverse_interface_->write(&values);
   }
+  return false;
+}
 
-  return true;
+bool UrDriver::writeKeepalive()
+{
+  if (reverse_interface_active_)
+  {
+    vector6d_t* fake = nullptr;
+    return reverse_interface_->write(fake, 1);
+  }
+  return false;
 }
 
 void UrDriver::startWatchdog()