Switching controllers during runtime while using MoveIt

[Cody-Vu]

Hello everyone,

I work with the franka panda robot using moveit and have a question about switching controllers. Is it possible to configure multiple controllers in the moveit_controllers.yaml file, for example position and effort controller:
controller_list:

- name: effort_joint_trajectory_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   joints:
     - panda_joint1
     - panda_joint2
     - panda_joint3
     - panda_joint4
     - panda_joint5
     - panda_joint6
     - panda_joint7
 - name: position_joint_trajectory_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: True
   joints:
     - panda_joint1
     - panda_joint2
     - panda_joint3
     - panda_joint4
     - panda_joint5
     - panda_joint6
     - panda_joint7

and switch them during runtime while the moveit node is constantly running? Right now I have the issue that moveit is only sending the trajectory to the default controller, even if I stop the default controller and run the other one instead. In the franka control node I get the error:

Can't accept new action goals. Controller is not running.

Is there a way to switch the controllers during runtime while using moveit ?
Thanks in advance!

[Danilrivero]

In short, yes, you can switch controllers during runtime.

The moveit_controllers.yaml file defines how MoveIt interfaces with your robot's controllers. It allows MoveIt to know which controllers it can send trajectories to during operation and maps joint names in MoveIt's planning model to the actual controllers.

To switch controllers at runtime as you asked earlier, you'll need to properly define multiple controllers in this file and then use the controller manager services to switch between them.

By checking ros2 service list you should notice /controller_manager/switch_controller. There are other services available as well under /controller_manager.

During runtime you could call this function and pass the expected controller to start.

    switch_controller_client_ = this->create_client<controller_manager_msgs::srv::SwitchController>(
      "/controller_manager/switch_controller");

...

  bool switchControllers(const std::vector<std::string>& start_controllers, 
                        const std::vector<std::string>& stop_controllers)
  {
    // Wait for service to be available
    while (!switch_controller_client_->wait_for_service(1s)) {
      RCLCPP_INFO(this->get_logger(), "Controller manager service not available, waiting...");
      if (!rclcpp::ok()) {
        return false;
      }
    }
    
    // Create request
    auto request = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
    request->start_controllers = start_controllers;
    request->stop_controllers = stop_controllers;
    request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
    request->start_asap = true;
    request->timeout.sec = 5;
    
    // Call service
    auto future = switch_controller_client_->async_send_request(request);
    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) !=
        rclcpp::FutureReturnCode::SUCCESS)
    {
      RCLCPP_ERROR(this->get_logger(), "Failed to call switch_controller service");
      return false;
    }
    
    auto result = future.get();
    return result->ok;
  }

At https://github.com/frankaemika/franka_ros2 they could probably give you more examples and information about it.

[Cody-Vu]

Hello @Danilrivero, thank you very much for your response. I think I described my issue not that clear. Switching the controller through rosservice call /controller_manager/... wasn't the problem, rather that MoveIt tried to communicate with only the controller which was set as default. I forgot to mention that I used the Simple moveit controller manager. Just now I tried to use the moveit ros control interface with the moveit controller manager plugin.

[Cody-Vu]

@Danilrivero, I just tried it out on the real robot by launching move_group.launch. The issue that I am facing now is that internally it is switching the controller back. For example the position controller is running and set as default. It sends the trajectory to the position controller and it executes it successfully. Then I stop the position controller and start the effort controller. Now when I try to execute a motion, moveit somehow switches back to the position controller. Do you know how to fix that? I just want moveit to use the controller which is currently active and switch between multiple controllers during runtime. I just realized that also in gazebo I face the same issue. I would really appreciate your help!

[Danilrivero]

You seem to be using ROS1, so I am unsure of the details, but I would start by verifying the parameter values:

    # Check which controller manager MoveIt is configured to use
    rosparam get /move_group/moveit_controller_manager

    # If using SimpleControllerManager, check the list of controllers it knows
    rosparam get /move_group/moveit_simple_controller_manager/controller_names

Verify you are indeed not using moveit_simple_controller_manager/MoveItSimpleControllerManager or joint_trajectory_controller. Also, I would remove the default flag as well.

I just realized that also in gazebo I face the same issue.

Regarding Gazebo, if you're facing issues with the effort controller specifically in Gazebo, the problem might lie in:

• The ros_control configuration (*.yaml files) defining the controllers and their interfaces for your robot model in Gazebo.
• Ensuring the gazebo_ros_control plugin is correctly loaded and configured within your robot's URDF or the Gazebo world file.
• The specific implementation or configuration of the effort_joint_trajectory_controller itself. Gazebo needs properly tuned PID gains (often within the controller's config or the URDF's tags) to simulate effort control effectively.

I am unsure of the specifics for the Franka, you may need to go to https://github.com/frankaemika/franka_ros and check for any info. related

[Cody-Vu]

Source: MoveIt can decide which controllers have to be started and stopped. Since only controller names with registered allocator plugins are handed over to MoveIt, this implementation takes care of stopping other conflicting controllers based on their claimed resources and the resources for the to-be-started controllers.

So far MoveIt starts and stopps controller itself. I don't know if it is possible that moveit uses the controller which I manually start myself. So that moveit only sends the trajectory to the controller that is currently active without somehow starting or stopping controllers itself.
Do you have further information on that @Danilrivero?