Low Level Controllers

MoveIt executes planned trajectories by means of MoveIt controller managers. These are plugins that handle the interaction with low-level controllers. There are different options available:

In the following sections, we will look at each of these options in more detail.

Simple Controller Manager

The JointTrajectoryController and GripperActionController from the ROS controllers package are supported out of the box and can be easily configured using the MoveIt Setup Assistant (MSA) on the Controllers page.

Controller Mapping

The ROS Controller Manager loads these controllers as plugins via their type name configured in the ros_controllers.yaml file generated by MSA:

panda_arm_controller:
  type: velocity_controllers/JointTrajectoryController
  joints:
    - panda_joint1
    - panda_joint2
    - panda_joint3
    - panda_joint4
    - panda_joint5
    - panda_joint6
    - panda_joint7
  gains:
    shoulder_joint:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    upperarm_joint:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    forearm_joint:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
gripper_controller:
  type: position_controllers/GripperActionController
  joint: panda_finger_joint1

The MoveItSimpleControllerManager configured by MSA as the default MoveIt controller manager will bridge these controllers with MoveIt pipeline by reading the simple_moveit_controllers.yaml configuration file, for example:

controller_list:
- name: panda_arm_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
- name: gripper_controller
  action_ns: gripper_action
  type: GripperCommand
  default: true
  parallel: true
  joints:
    - panda_finger_joint1
    - panda_finger_joint2

The mapping from ROS controller name to a MoveItControllerHandle type is done by using the pre-defined integration types FollowJointTrajectory and GripperCommand, which can interface any controller implementing the corresponding action interface (i.e. not only ROS Control controllers).

The action_ns setting specifies the action server topic exposed by the ROS controller. The full topic name is <name>/<action_ns>. If you were to list topics by using rostopic list with the above two ROS controllers loaded, you would see something like the following:

/panda_arm_controller/follow_joint_trajectory/goal
/panda_arm_controller/follow_joint_trajectory/feedback
/panda_arm_controller/follow_joint_trajectory/result
/gripper_controller/gripper_action/goal
/gripper_controller/gripper_action/feedback
/gripper_controller/gripper_action/result

There are many different parameters that can be defined for the two types of simple controller interfaces.

FollowJointTrajectory Controller Interface

  • name: The name of the controller. (See debugging information below for important notes).
  • action_ns: The action namespace for the controller. (See debugging information below for important notes).
  • type: The type of action being used (here FollowJointTrajectory).
  • default: The default controller is the primary controller chosen by MoveIt for communicating with a particular set of joints. This is useful when additional controllers are defined for the same set of joints:
    • One such scenario is using the Motion Planning RViz Plugin with a joystick. In this case, JointGroupVelocityController or JointGroupPositionController could be configured in ros_controllers.yaml for the same set of joints.
    • Another scenario is configuring the robot for use with MoveIt Servo which lets you control the robot by using a joystick or a SpaceMouse. MoveIt Servo supports trajectory_msgs/JointTrajectory and std_msgs/Float64MultiArray so a JointGroupVelocityController or JointGroupPositionController could be configured as well.
  • joints: Names of all the joints that are being addressed by this interface.

GripperCommand Controller Interface

  • name: The name of the controller. (See debugging information below for important notes).
  • action_ns: The action namespace for the controller. (See debugging information below for important notes).
  • type: The type of action being used (here GripperCommand).
  • default: The default controller is the primary controller chosen by MoveIt for communicating with a particular set of joints.
  • joints: Names of all the joints that are being addressed by this interface.
  • command_joint: The single joint, controlling the actual state of the gripper. This is the only value that is sent to the controller. Has to be one of the joints above. If not specified, the first entry in joints will be used instead.
  • parallel: When this is set, joints should be of size 2, and the command will be the sum of the two joints.

Optional Allowed Trajectory Execution Duration Parameters

For each controller it is optionally possible to set the allowed_execution_duration_scaling and allowed_goal_duration_margin parameters. These are controller-specific overrides of the global values trajectory_execution/allowed_execution_duration_scaling and trajectory_execution/allowed_goal_duration_margin. As opposed to the global values, the contoller-specific ones cannot be dynamically reconfigured at runtime. The parameters are used to compute the allowed trajectory execution duration by scaling the expected execution duration and adding the margin afterwards. If this duration is exceeded the trajectory will be cancelled.

Trajectory execution parameters can be configured to fine-tune the allowed trajectory execution duration, overriding the global settings trajectory_execution/allowed_execution_duration_scaling and trajectory_execution/allowed_goal_duration_margin, where the former scales the allowed execution duration by a given factor and the latter allows for a fixed (duration-independent) margin (applied after scaling). If the execution does not finish within the specified margins, execution will be canceled.

Additional options for tuning the behavior and safety checks of MoveIt’s execution pipeline can be configured in trajectory_execution.launch.xml file generated by MSA:

  • execution_duration_monitoring: when false, will not throw error if a controller takes longer than expected to complete a trajectory.
  • allowed_goal_duration_margin: same as above, but configured globally as a default for all controllers.
  • allowed_start_tolerance: joint state tolerance when validating that a trajectory’s first point matches current robot state. If set to 0 MoveIt will skip waiting for the robot to stop after execution.

ROS Control Controller Manager

Alternatively to the simple controller manager described above, MoveIt also provides a controller manager that directly interfaces the ROS Controller Manager. Instead of using a bridging configuration file like simple_moveit_controllers.yaml, this controller manager directly queries the ROS Controller Manager for available controllers.

The MoveIt ROS Control Controller Manager keeps track of all loaded and started ROS controllers, as well as the subset of these controllers that can be used with MoveIt. All loaded and started controllers are designated as active, and the subset of these controllers that can be used with MoveIt because they have associated controller handles is designated as managed.

Multi Controller Manager

The MoveIt ROS Control Controller Manager can only interface controllers from the single ROS controller manager found in the ROS namespace defined by the ROS parameter ~ros_control_namespace (defaults to /).

To overcome this limitation, there also exists MoveItMultiControllerManager, which queries all existing ROS controller managers and instantiates all controllers with their respective namespace taking care of proper delegation. This type of manager can be configured by setting moveit_controller_manager to moveit_ros_control_interface::MoveItMultiControllerManager:

<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItMultiControllerManager" />

Note

The Simple Controller Manager is generic as it can interface controllers from multiple ROS controller managers by specifying different prefix names in the bridging configuration file.

Controller Switching and Namespaces

All controller names get prefixed by the namespace of their ROS Control node. For this reason, controller names should not contain slashes.

For a particular ROS Control node, MoveIt can decide which controllers to start or stop. MoveIt will take care of stopping controllers based on their claimed joint resources if a to-be-started controller needs any of those resources.

Fake Controller Manager

MoveIt comes with a series of fake trajectory controllers that can be used for simulations. For example, the demo.launch file generated by MSA employs fake controllers for nice visualization in RViz.

The configuration for these controllers is stored in fake_controllers.yaml also generated by MSA, for example:

controller_list:
- name: fake_arm_controller
    type: $(arg fake_execution_type)
    joints:
    - panda_joint1
    - panda_joint2
    - panda_joint3
    - panda_joint4
    - panda_joint5
    - panda_joint6
    - panda_joint7
- name: fake_gripper_controller
    type: $(arg fake_execution_type)
    joints:
    - panda_finger_joint1
    - panda_finger_joint2
initial:  # Define initial robot poses per group
- group: panda_arm
    pose: ready
- group: panda_hand
    pose: open

The type setting specifies the fake controller interpolation type:

  • interpolate: performs smooth interpolation between trajectory waypoints - the default for visualization.
  • via points: jumps to the position specified by each trajectory waypoint without interpolation in between - useful for visual debugging.
  • last point: warps directly to the last trajectory waypoint - the fastest method for off-line benchmarking and unit tests.

Integrating Controllers

MoveIt Controller Managers only support controllers that implement the FollowJointTrajectory action out of the box.

This is because only a ControllerHandleAllocator for this action type is exported as a plugin. Even though there is a ControllerHandle for GripperCommand actions, a corresponding ControllerHandleAllocator plugin that enables the controller handle to be dynamically created from the ROS controller type name, does not exist.

ROS Controllers with a FollowJointTrajectory action interface

Controller handles implemented by MoveIt bridge ROS Controllers with the MoveIt motion planning pipeline by means of an Action Client, as long as the controller starts an Action Server that handles one of the two types of supported action interfaces:

The MoveIt ROS Control Controller Manager will regard any controllers loaded by ROS Controller Manager as managed if it finds a plugin registration that links the type of the ROS controller with a MoveIt Controller Handle Allocator. If no such registration is found, the controller is regarded as unmanaged (merely active) and cannot be used to receive trajectory commands from MoveIt.

For example, see the stock Joint Trajectory Controller plugin registration, which links several flavors of this controller exported from ros_controllers package with the corresponding MoveIt Controller Handle that supports Follow Joint Trajectory Action via an exported MoveIt Controller Handle Allocator plugin.

The same pattern can be followed to link any other ROS controller that exposes a Follow Joint Trajectory Action server with an existing MoveIt Controller Handle so that it can receive trajectory commands.

First, create a plugin description file:

<library path="libmoveit_ros_control_interface_trajectory_plugin">
    <class
        name="my_controller_package_name/my_controller_type_name"
        type="moveit_ros_control_interface::JointTrajectoryControllerAllocator"
        base_class_type="moveit_ros_control_interface::ControllerHandleAllocator"
    >
        <description>
            Controller description
        </description>
    </class>
</library>

Reference this plugin description file in your package.xml’s export section:

<export>
    <moveit_ros_control_interface plugin="${prefix}/controller_moveit_plugin.xml"/>
</export>

After building the package, any controllers in ros_controllers.yaml that reference controller_package_name/controller_type_name will become available for use with MoveIt.

The MoveIt ROS Control Controller Manager can be configured by changing the moveit_controller_manager setting to ros_control. The MoveIt configuration package auto-generated by MSA includes the demo_gazebo.launch file that already configures this manager type in addition to launching Gazebo simulation and visualizing the robot state in RViz.

To test ROS controller integration with MoveIt ROS Control Controller Manager, launch the package generated by MSA by using the demo_gazebo.launch file. This will load your robot description, start the motion planning pipeline hosted in move_group node, and enable you to use the Motion Planning Plugin in RViz to send goals to MoveIt, simulating the effect your ROS controllers will have on the real robot in Gazebo.

Note

Since the GripperActionController is not supported by the MoveIt ROS Control Controller Manager, it can be replaced in the above example by a flavor of JointTrajectoryController supported by your hardware, for example:

gripper_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - gripper
  gains:
    gripper:
      p: 100
      d: 1
      i: 1
      i_clamp: 1

ROS Controllers with another interface

What if you need to use a ROS controller that does not support Follow Joint Trajectory Action with MoveIt ROS Control Controller Manager? Some examples from ROS controllers package include:

In this case, a Controller Handle and a Controller Handle Allocator may need to be implemented. The allocator will also need to be exported by your package as a plugin.

The following package dependencies are required for implementing controller handles and allocators:

  • moveit_ros_control_interface - Provides base classes for controller handles and allocators.
  • pluginlib - Provides macros for exporting a class as a plugin, only needed to export the controller handle allocator.

The actionlib package may also be needed for communicating with the ROS controller via an Action Client if it exposes an Action Server.

The following headers declare the relevant classes and macros:

  • #include <moveit_ros_control_interface/ControllerHandle.h>
    • declares moveit_controller_manager::MoveItControllerHandle class
    • declares moveit_ros_control_interface::ControllerHandleAllocator class
  • #include <pluginlib/class_list_macros.h>
    • declares PLUGINLIB_EXPORT_CLASS macro for exporting plugins

Two example controller handle implementations are included with MoveIt:

As you can see, writing a controller handle comes down to implementing:

  • sendTrajectory method that translates moveit_msgs::RobotTrajectory to a format the controller can understand
  • cancelExecution method to tell the controller to stop any active trajectories
  • waitForExecution method that will block the calling thread until the controller finishes or the timeout is reached
  • getLastExecutionStatus method that returns the status of the last requested trajectory.

One example controller handle allocator plugin implementation is included with MoveIt:

The only job of a controller handle allocator is to create a new instance of the controller handle. The following example implements an allocator for a custom controller handle of type example::controller_handle_example (the code listing for this class is provided in the following section):

// declares example::controller_handle_example class
#include "controller_handle_example.h"
#include <moveit_ros_control_interface/ControllerHandle.h>
#include <pluginlib/class_list_macros.h>

namespace example
{
class controller_handle_allocator_example : public moveit_ros_control_interface::ControllerHandleAllocator
{
public:
  moveit_controller_manager::MoveItControllerHandlePtr alloc(const std::string& name,
                                                             const std::vector<std::string>& resources) override
  {
    return std::make_shared<controller_handle_example>(name);
  }
};
}  // namespace example

PLUGINLIB_EXPORT_CLASS(example::controller_handle_allocator_example,
                       moveit_ros_control_interface::ControllerHandleAllocator);

This example controller handle allocator can be exported by creating a plugin definition file which is then referenced in the exports section of package.xml:

<library path="libtrajectory_controller_example">
    <class
        name="example/trajectory_controller_example"
        type="example::controller_handle_allocator_example"
        base_class_type="moveit_ros_control_interface::ControllerHandleAllocator"
    >
        <description>
            Example Controller Handle Allocator for MoveIt!
        </description>
    </class>
</library>

This plugin definition links the name of a controller you are integrating with MoveIt (specified by the name attribute) with the type of the allocator you implemented (specified by the type attribute), such as the one in the example above.

The base_class_type must be set to moveit_ros_control_interface::ControllerHandleAllocator to make the allocator discoverable by MoveIt.

The plugin definition can then be referenced in the package manifest:

<export>
    <!-- other exports... -->
    <moveit_ros_control_interface plugin="${prefix}/controller_handle_allocator_plugin.xml"/>
</export>

The translation between moveit_msgs::RobotTrajectory message and the type of command supported by the controller would be done by implementing a controller handle.

The following code listing demonstrates how to write a controller handle that translates robot trajectory message into joint trajectory message. Of course this is already taken care of by existing controller handles included with MoveIt, so you would substitute the message that your custom controller understands here instead of FollowJointTrajectory:

#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <memory>
#include <moveit_ros_control_interface/ControllerHandle.h>

namespace example
{
class controller_handle_example : public moveit_controller_manager::MoveItControllerHandle
{
private:
  // Idle or done executing trajectory
  bool done_;

  // Connects to Action Server exposed by the controller
  std::shared_ptr<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>> actionClient_;

public:
  controller_handle_example(const std::string& name)
  {
    std::string actionName = name + "/follow_joint_trajectory";

    actionClient_ =
        std::make_shared<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>>(actionName, true);

    // Timeout can be loaded from settings
    actionClient_->waitForServer(ros::Duration(20.0));

    if (!actionClient_->isServerConnected())
    {
      // Report connection error
      actionClient_.reset();
    }
  }

public:
  // MoveIt calls this method when it wants to send a trajectory goal to execute
  bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory) override
  {
    if (!actionClient_)
    {
      // Report connection error
      return false;
    }

    control_msgs::FollowJointTrajectoryGoal goal;
    goal.trajectory = trajectory.joint_trajectory;

    actionClient_->sendGoal(
        goal,
        [this](const auto& state, const auto& result) {
          // Complete trajectory callback
          done_ = true;
        },
        [this] {
          // Begin trajectory callback
        },
        [this](const auto& feedback) {
          // Trajectory state callback
        });

    done_ = false;

    return true;
  }

  // MoveIt calls this method when it wants a blocking call until done
  bool waitForExecution(const ros::Duration& timeout = ros::Duration(0)) override
  {
    if (actionClient_ && !done_)
      return actionClient_->waitForResult(ros::Duration(5.0));

    return true;
  }

  // MoveIt calls this method to get status updates
  moveit_controller_manager::ExecutionStatus getLastExecutionStatus() override
  {
    // Report last status here
    return moveit_controller_manager::ExecutionStatus::SUCCEEDED;
  }

  // MoveIt calls this method to abort trajectory goal execution
  bool cancelExecution() override
  {
    if (!actionClient_)
      return false;

    actionClient_->cancelGoal();
    done_ = true;

    return true;
  }
};
}  // namespace example

Once implemented, the controller handle does not need to be exported, since it’s returned by the controller handle allocator, which is exported.

Remapping /joint_states topic

MoveIt requires joint states to be published on the /joint_states topic to internally maintain the robot’s state. If the joint states are published on another topic specific to your project, such as /robot/joint_states, add a remap to the move_group node in move_group.launch file generated by MSA:

<!-- Start the actual move_group node/action server -->
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
    <remap
        from="joint_states"
        to="robot/joint_states"
    />
    <!-- Other settings -->
</node>

Open Source Feedback

See something that needs improvement? Please open a pull request on this GitHub page