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/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index 2ea6bfc..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)
 {
 }
 
@@ -301,8 +302,23 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio
 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,