Executing Trajectories with MoveIt!

=Output from MoveIt!=

If you got to this tutorial you probably have produced motion plans with MoveIt!. Regardless of how the request for planning was issued (e.g., via the Rviz plugin, command line interface, your own code, etc.), the output of the motion planning code is a moveit_msgs::RobotTrajectory message. For the case of pick & place, the output is simply a sequence of moveit_msgs::RobotTrajectory messages.

If you are calling motion planning functionality in code (e.g., directly from [/doxygen/classplanning__interface_1_1PlanningContext.html the planning_interface plugin], or using a [/doxygen/classplanning__pipeline_1_1PlanningPipeline.html planning pipeline]), or you are using ROS API such as the planning service, or the action interface for MoveGroup with the plan_only flag set to true, no motion is attempted. Only the motion plan is computed.

If you use the action interface with plan_only set to false, or you are calling the ROS service for execution of trajectories, then MoveIt! attempts to send commands to controllers.

The moveit_msgs::RobotTrajectory message
This is simply a sequence of time-stamped waypoints with velocities (optional), accelerations (optional). This message is built upon trajectory messages existing in ROS. The joints this message specifies values for always correspond to the group motion planning was called for. Execution components of MoveIt! do not assume however that only joints belonging to a particular group can be included in a moveit_msgs::RobotTrajectory message, so it is possible to generate your own message and execute it using MoveIt! API.

=Execution of Trajectories=

The MoveIt! Controller Manager Plugin
MoveIt! does not require a particular control framework. Instead, a MoveIt!-specific plugin for connecting to controllers must be provided for every robot. This plugin cannot be automatically generated by the Setup Assistant as its implementation depends on your robot. This plugin has to inherit from a specified base class. This class is expected to allow MoveIt! to reason about the controllers available for use. The main class -- the manager allows retrieving the list of known controllers (string names) and allows creating controller handles for individual controllers. A controller handle offers the ability to send a trajectory to the controller (non-blocking) and cancel any active execution (non-blocking).

The Trajectory Execution Manager
Bits of code in MoveIt! that need to deal with execution of trajectories use a helper class: the trajectory execution manager. Given a moveit_msgs::RobotTrajectory message, the trajectory execution manager is used to pass the moveit_msgs::RobotTrajectory message to a MoveIt! Controller Manager Plugin implemented as above (the plugin specified under the ROS parameter ~/moveit_controller_manager is loaded by the trajectory execution manager).
 * First, the set of joints that need to be moved is identified by inspecting the moveit_msgs::RobotTrajectory message to be forwarded to the MoveIt! Controller Manager plugin.
 * Given the set of joints to be actuated, the MoveIt! Controller Manager plugin is queried for the known controllers, and the joints each of those controllers operates on, to identify the controller moving fewest degrees of freedom that still covers all the joints that need to be actuated.
 * If no such controller is identified, a set of controllers that covers the joints to be actuated is identified. If this also fails, execution of the trajectory fails.
 * Given a set of controllers to be used (in parallel), the input moveit_msgs::RobotTrajectory message is split into multiple moveit_msgs::RobotTrajectory messages (if needed) such that each controller to be used receives a request for execution only for joints it actuates.
 * Handles for controllers are constructed using the MoveIt! Controller Manager plugin for each of the controllers that are about to be sent trajectories. If any handle fails to initialize, the trajectory fails to execute.
 * The constructed controller handles receive the moveit_msgs::RobotTrajectory messages that are to be executed.
 * The trajectory execution manager waits for the amount of time declared by moveit_msgs::RobotTrajectory for the execution to complete. If that time is exceeded by a significant margin (more than 10% longer), the execution is cancelled. This behaviour can be disabled by calling enableExecutionDurationMonitoring(false) for the trajectory execution manager instance or by setting the ROS parameter ~/execution_duration_monitoring (e.g., as part of the move_group node). To scale the amount of time to wait (e.g., more than 10%), the setAllowedExecutionDurationScaling function can be called or the ROS parameter ~/allowed_execution_duration_scaling can be set.

Reasoning about which controllers are used

 * When multiple controllers can be used for executing the same trajectory, preference is given to controllers that are marked as default (see the plugin interface described above). If this criterion does not make a difference, the controller that actuates fewest joints is preferred. If this still does not make a difference, preference is given to currently active controllers.
 * When a controller needs to be activated, other active controllers that have overlapping sets of joints are deactivated (using the switchControllers function). This functionality is optional and likely does not need to be implemented by simple plugins. If controllers are deactivated, it is ensured that joints previously covered by controllers do not become 'dangling' -- some other controller is identified and activated to ensure the joints remain covered. For example, if the controller 'arms' is active, but we need to execute a trajectory for 'right_arm', the 'arms' controller is deactivated and 'right_arm' is activated. This would leave the joints in the left arm no longer covered by a controller, so the 'left_arm' controller is found and activated as well, even though no commands are to be sent to it.

=Simple MoveIt! Controller Manager Plugin=

For simple use cases where the typical JointTrajectoryAction used in ROS is employed for control, a simple plugin is already provided in the moveit_simple_controller_manager. This controller manager assumes all controllers are embedded on the robot via micro controllers or other non-PC locations.

An example of how to use this controller manager can be found in the Maxwell robot's MoveIt! configuration file controllers.yaml. With this scenario you would need two components:


 * follow_joint_trajectoy action server, that accepts a JointTrajectoryAction
 * gripper_action action server, that accepts a GripperCommandAction

=ros_control Controllers=

For more complicated use cases such as where a PID controller needs to be run on the PC, a more powerful set of control tools is available in the ros_control project. This set of packages provides various types of controllers, transmissions, joint limits, and other tools. It allows different controllers to be loaded, unloaded, and reset. The ros_control project is aiming at becoming the standard control method for ROS-enabled robots.