diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp
index 848774ab4..0f496933a 100644
--- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp
+++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp
@@ -21,6 +21,7 @@
 
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/resource_manager.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "rclcpp/macros.hpp"
@@ -50,7 +51,8 @@ namespace webots_ros2_control {
   class Ros2ControlSystem : public Ros2ControlSystemInterface {
   public:
     Ros2ControlSystem();
-    void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) override;
+    void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info,
+              const hardware_interface::ResourceManager &resource) override;
 
     rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init(
       const hardware_interface::HardwareInfo &info) override;
@@ -61,6 +63,10 @@ namespace webots_ros2_control {
 
     std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
     std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+    hardware_interface::return_type perform_command_mode_switch(const std::vector<std::string> &start_interfaces,
+                                                                const std::vector<std::string> &stop_interfaces) override;
+
     hardware_interface::return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
     hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
 
diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp
index 56e69a76e..49799a2b8 100644
--- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp
+++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp
@@ -22,6 +22,7 @@
 #include <webots/supervisor.h>
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/resource_manager.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "webots_ros2_driver/PluginInterface.hpp"
@@ -30,7 +31,8 @@
 namespace webots_ros2_control {
   class Ros2ControlSystemInterface : public hardware_interface::SystemInterface {
   public:
-    virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) = 0;
+    virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info,
+                      const hardware_interface::ResourceManager &resource) = 0;
   };
 }  // namespace webots_ros2_control
 
diff --git a/webots_ros2_control/src/Ros2Control.cpp b/webots_ros2_control/src/Ros2Control.cpp
index 94694a5fe..777d1777d 100644
--- a/webots_ros2_control/src/Ros2Control.cpp
+++ b/webots_ros2_control/src/Ros2Control.cpp
@@ -87,7 +87,7 @@ namespace webots_ros2_control {
       auto webotsSystem = std::unique_ptr<webots_ros2_control::Ros2ControlSystemInterface>(
         mHardwareLoader->createUnmanagedInstance(hardwareType));
 #endif
-      webotsSystem->init(mNode, controlHardware[i]);
+      webotsSystem->init(mNode, controlHardware[i], *resourceManager);
       resourceManager->import_component(std::move(webotsSystem), controlHardware[i]);
 
       // Configure and activate all components
diff --git a/webots_ros2_control/src/Ros2ControlSystem.cpp b/webots_ros2_control/src/Ros2ControlSystem.cpp
index 8771c64bd..ec8e85018 100644
--- a/webots_ros2_control/src/Ros2ControlSystem.cpp
+++ b/webots_ros2_control/src/Ros2ControlSystem.cpp
@@ -31,7 +31,8 @@ namespace webots_ros2_control {
   Ros2ControlSystem::Ros2ControlSystem() {
     mNode = NULL;
   }
-  void Ros2ControlSystem::init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) {
+  void Ros2ControlSystem::init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info,
+                               const hardware_interface::ResourceManager &resource) {
     mNode = node;
     for (hardware_interface::ComponentInfo component : info.joints) {
       Joint joint;
@@ -64,14 +65,16 @@ namespace webots_ros2_control {
 
       // Configure the command interface
       for (hardware_interface::InterfaceInfo commandInterface : component.command_interfaces) {
-        if (commandInterface.name == "position")
-          joint.controlPosition = true;
-        else if (commandInterface.name == "velocity")
-          joint.controlVelocity = true;
-        else if (commandInterface.name == "effort")
-          joint.controlEffort = true;
-        else
-          throw std::runtime_error("Invalid hardware info name `" + commandInterface.name + "`");
+        if (resource.command_interface_is_claimed(commandInterface.name)) {
+          if (commandInterface.name == "position")
+            joint.controlPosition = true;
+          else if (commandInterface.name == "velocity")
+            joint.controlVelocity = true;
+          else if (commandInterface.name == "effort")
+            joint.controlEffort = true;
+          else
+            throw std::runtime_error("Invalid hardware info name `" + commandInterface.name + "`");
+        }
       }
       if (joint.motor && joint.controlVelocity && !joint.controlPosition) {
         wb_motor_set_position(joint.motor, INFINITY);
@@ -133,6 +136,43 @@ namespace webots_ros2_control {
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
   }
 
+  hardware_interface::return_type Ros2ControlSystem::perform_command_mode_switch(
+    const std::vector<std::string> &start_interfaces, const std::vector<std::string> &stop_interfaces) {
+    for (Joint &joint : mJoints) {
+      for (const std::string &interface_name : stop_interfaces) {
+        // Clear joint control method bits corresponding to stop interfaces
+        if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_POSITION)) {
+          joint.controlPosition = false;
+        }
+        if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_VELOCITY)) {
+          joint.controlVelocity = false;
+        }
+        if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_EFFORT)) {
+          joint.controlEffort = false;
+        }
+      }
+
+      // Set joint control method bits corresponding to start interfaces
+      for (const std::string &interface_name : start_interfaces) {
+        if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_POSITION)) {
+          joint.controlPosition = true;
+        }
+        if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_VELOCITY)) {
+          joint.controlVelocity = true;
+        }
+        if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_EFFORT)) {
+          joint.controlEffort = true;
+        }
+      }
+      if (joint.motor && joint.controlVelocity && !joint.controlPosition) {
+        wb_motor_set_position(joint.motor, INFINITY);
+        wb_motor_set_velocity(joint.motor, 0.0);
+      }
+    }
+
+    return hardware_interface::return_type::OK;
+  }
+
   hardware_interface::return_type Ros2ControlSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
     static double lastReadTime = 0;