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 d89ce1e..3c13aa6 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
@@ -67,6 +67,8 @@ public:
 
   void handleRobotProgramState(bool program_running);
 
+  bool shouldResetControllers();
+
 protected:
   /*!
    * \brief Transforms force-torque measurements reported from the robot from base to tool frame
@@ -103,6 +105,8 @@ protected:
   std::string tcp_link_;
   bool robot_program_running_;
   ros::Publisher program_state_pub_;
+
+  bool controller_reset_necessary_;
 };
 
 }  // namespace ur_driver
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index a5b4de5..db2c53b 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -344,10 +344,27 @@ bool HardwareInterface ::isRobotProgramRunning() const
 
 void HardwareInterface ::handleRobotProgramState(bool program_running)
 {
+  if (robot_program_running_ == false && program_running)
+  {
+    controller_reset_necessary_ = true;
+  }
   robot_program_running_ = program_running;
   std_msgs::Bool msg;
   msg.data = robot_program_running_;
   program_state_pub_.publish(msg);
 }
 
+bool HardwareInterface ::shouldResetControllers()
+{
+  if (controller_reset_necessary_)
+  {
+    controller_reset_necessary_ = false;
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
 }  // namespace ur_driver
diff --git a/ur_rtde_driver/src/ros/hardware_interface_node.cpp b/ur_rtde_driver/src/ros/hardware_interface_node.cpp
index 8924d4c..44614ad 100644
--- a/ur_rtde_driver/src/ros/hardware_interface_node.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface_node.cpp
@@ -141,8 +141,7 @@ int main(int argc, char** argv)
     period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
     stopwatch_last = stopwatch_now;
 
-    // cm.update(timestamp, period, !g_hw_interface->isRobotProgramRunning());
-    cm.update(timestamp, period);
+    cm.update(timestamp, period, g_hw_interface->shouldResetControllers());
 
     g_hw_interface->write(timestamp, period);
     // if (!control_rate.sleep())