diff --git a/doc/controller_configuration/controller_configuration_tutorial.rst b/doc/controller_configuration/controller_configuration_tutorial.rst index e11a07921..4368be4d8 100644 --- a/doc/controller_configuration/controller_configuration_tutorial.rst +++ b/doc/controller_configuration/controller_configuration_tutorial.rst @@ -1,37 +1,38 @@ Low Level Controllers ===================== -There are a few paths for integrating a controller that moves robot joints with the MoveIt! framework, accommodating simple usage scenarios all the way to advanced customization: +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 -* The `JointTrajectoryController `_ and `GripperActionController `_ from `ROS controllers `_ package are supported out of the box because MoveIt implements integration interfaces and plugins that bridge them with MoveIt motion planning pipeline. -* Any other controllers managed by `ROS Controller Manager `_ can be used by linking them with an existing MoveIt integration plugin if they support `Follow Joint Trajectory Action `_. -* ROS controllers that don't support `Follow Joint Trajectory Action `_ can be bridged with MoveIt by implementing integration interfaces and exporting a plugin, as long as they can be made to fit into the `Controller Handle `_ interface. -* Custom controllers that are not managed by `ROS Controller Manager `_, or for which the `MoveIt Controller Handle `_ is a poor fit, can be integrated by writing a custom MoveIt Controller Manager. +* MoveIt Simple Controller Manager allows to interface any low-level controller supporting either the `JointTrajectoryController `_ or `GripperActionController `_ API from the `ROS controllers `_ package +* ROS Control Controller Manager and MoveIt Multi Controller Manager allow to interface ros_control controllers that support the `FollowJointTrajectory action `_ without further configuration. This manager directly interacts with the corresponding `ROS Controller Manager `_. +* Fake Controller Manager provides fake trajectory execution w/o a real robot. It is used for visualization in RViz and can be configured to use different methods for interpolation between waypoints. +* ROS controllers that don't support the `FollowJointTrajectory action `_ can be used with a ROS Control Controller Manager by implementing integration interfaces satisfying the `ControllerHandle `_ API. +* Completely custom controllers can be integrated by writing and using your own controller manager plugin (not recommended). -We will look at each of these options in more detail. +In the following, we will look at each of these options in more detail. -Stock ROS Controllers ---------------------- +MoveIt Simple Controller Manager +-------------------------------- -The `JointTrajectoryController `_ and `GripperActionController `_ from `ROS controllers `_ package are supported out of the box for simple usage scenarios, and can be easily configured by using the `MoveIt Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_ (*MSA*) on the *Controllers* page. - -These controllers are integrated with MoveIt by using existing `MoveIt Controller Handles `_: - -* Stock `JointTrajectoryController `_ is integrated through `Joint Trajectory Controller Handle `_. -* Stock `GripperActionController `_ is integrated through `Gripper Controller Handle `_. +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 <../setup_assistant/setup_assistant_tutorial.html>`_ (*MSA*) on the *Controllers* page. Controller Mapping ^^^^^^^^^^^^^^^^^^ -The `ROS Controller Manager `_ loads these controllers from ``ros_controllers.yaml`` configuration file generated by MSA because they are `exported as plugins `_ by the `ROS Controllers `_ package using their ``type`` names: +The `ROS Controller Manager `_ loads these controllers `as plugins `_ via their ``type`` name configured in the ``ros_controllers.yaml`` file generated by MSA: .. code-block:: yaml - arm_controller: + panda_arm_controller: type: velocity_controllers/JointTrajectoryController joints: - - shoulder_joint - - upperarm_joint - - forearm_joint + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 gains: shoulder_joint: p: 100 @@ -50,109 +51,146 @@ The `ROS Controller Manager `_ loads the i_clamp: 1 gripper_controller: type: position_controllers/GripperActionController - joint: gripper + joint: panda_finger_joint1 -The *MoveIt Simple Controller Manager* configured by MSA `as the default `_ will load the *handles* that bridge these controllers with MoveIt by reading the ``simple_moveit_controllers.yaml`` configuration file, for example: +The ``MoveItSimpleControllerManager`` configured by MSA `as the default `_ MoveIt controller manager will bridge these controllers by reading the ``simple_moveit_controllers.yaml`` configuration file, for example: .. code-block:: yaml controller_list: - - name: arm_controller + - name: panda_arm_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory - default: True + default: true joints: - - shoulder_joint - - upperarm_joint - - forearm_joint + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 - name: gripper_controller - action_ns: gripper_cmd + action_ns: gripper_action type: GripperCommand - default: True + default: true + parallel: true joints: - - gripper - -The mapping from ROS controller ``name`` to MoveIt *controller handle* ``type`` is done by using pre-defined *simple integration types*: - -* ``FollowJointTrajectory``: specifying this integration type will bridge the ROS controller specified by the ``name`` setting to the MoveIt pipeline through the `Joint Trajectory Controller Handle `_. -* ``GripperCommand``: specifying this integration type will bridge the ROS controller specified by the ``name`` setting to the MoveIt pipeline through the `Gripper Controller Handle `_. - -The ``action_ns`` setting specifies the *Action Server* topic exposed by the ROS controller. The full topic name consists of the ROS controller ``name`` and ``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_finger_joint1 + - panda_finger_joint2 -/arm_controller/command -/arm_controller/follow_joint_trajectory/cancel -/arm_controller/follow_joint_trajectory/feedback -/arm_controller/follow_joint_trajectory/goal -/arm_controller/follow_joint_trajectory/result -/arm_controller/follow_joint_trajectory/status -/arm_controller/gains/forearm_joint/parameter_descriptions -/arm_controller/gains/forearm_joint/parameter_updates -/arm_controller/gains/shoulder_joint/parameter_descriptions -/arm_controller/gains/shoulder_joint/parameter_updates -/arm_controller/gains/upperarm_joint/parameter_descriptions -/arm_controller/gains/upperarm_joint/parameter_updates -/arm_controller/state -/gripper_controller/gripper_cmd/cancel -/gripper_controller/gripper_cmd/feedback -/gripper_controller/gripper_cmd/goal -/gripper_controller/gripper_cmd/result -/gripper_controller/gripper_cmd/status +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). -Both controllers expose action servers under controller-specific topics like ``follow_joint_trajectory`` or ``gripper_cmd``. +The ``action_ns`` setting specifies the *action server* topic exposed by the ROS controller. The full topic name is ``/``. +If you were to list topics by using ``rostopic list`` with the above two ROS controllers loaded, you would see something like the following: :: -Controller Settings -^^^^^^^^^^^^^^^^^^^ +/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 -The controlled joints are listed under the ``joints`` setting. MoveIt requires joint states for these joints to be published on the ``/joint_states`` topic to maintain its internal state representation of the robot, used as a starting state when executing a trajectory. +There are many different parameters that can be defined for the two types of controllers. -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: +FollowJointTrajectory Controller Interface +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. code-block:: XML + * ``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 <../joystick_control_teleoperation/joystick_control_teleoperation_tutorial.html?highlight=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 <../realtime_servo/realtime_servo_tutorial.html>`_ 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. -The ``default`` setting is used to indicate a default controller that will be chosen to control this set of joints. This is useful when additional controllers are defined for the same joints: + * ``joints``: Names of all the joints that are being addressed by this interface. -* One such scenario is using the `Motion Planning RViz Plugin with a joystick <../joystick_control_teleoperation/joystick_control_teleoperation_tutorial.html?highlight=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 <../realtime_servo/realtime_servo_tutorial.html>`_ 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. +GripperCommand Controller Interface +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Trajectory execution parameters can also 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``. + * ``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. -* ``allowed_execution_duration_scaling`` - scales the allowed execution duration. -* ``allowed_goal_duration_margin`` - allows more than the expected execution time before triggering a trajectory cancel (applied after scaling). +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. -.. note:: - Unlike the global settings, the controller-specific settings cannot be dynamically reconfigured at runtime. +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 the Move execution pipeline can be configured in ``trajectory_execution.launch.xml`` file generated by MSA: +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. -To test simple controller integration with *MoveIt Simple Controller Manager*, launch the package generated by MSA by using the ``move_group.launch`` file. This will load your robot description and the MoveIt motion planning pipeline hosted in ``move_group`` node from ``moveit_ros_move_group`` package. +Directly interfacing a ROS controller manager +--------------------------------------------- -.. note:: - This launch file assumes that your robot's `hardware interface `_ is already running, since any ROS controllers you use will attempt to connect to this interface and send commands. It does not not include any `visualization `_ and does not `simulate the hardware interface `_. In the absence of visualization and/or simulation tools, you can use the `C++ `_, `Python `_, or `Command Line <../moveit_commander_scripting/moveit_commander_scripting_tutorial.html>`_ interface to interact with MoveIt. +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. -ROS Controllers with Joint Trajectory Action --------------------------------------------- +This 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 ``/``). By providing different names in the simple controller manager, the latter can interface multiple ROS controller managers. +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``: -The *MoveIt ROS Control Controller Manager* which is the default configured by MSA for visualizing and/or simulating the robot does not use the configuration in ``simple_moveit_controllers.yaml`` to discover controllers. Instead it queries `ROS Controller Manager `_ for loaded and active controllers. +.. code-block:: XML -Since this discovery process does not use the pre-defined types ``FollowJointTrajectory`` and ``GripperCommand`` (only supported by *Simple Controller Manager*), `Controller Handle Allocator `_ plugins also need to be exported for each controller used in this fashion to link ROS controllers to MoveIt *Controller Handles* by their type names. + -While `JointTrajectoryController `_ from `ROS controllers `_ is supported by *MoveIt ROS Control Controller Manager* out of the box because its `Controller Handle Allocator `__ is `exported `_ as a plugin, the `GripperActionController `_ is not. Even though it has a `Controller Handle `__, it does not implement or export the corresponding *Controller Handle Allocator* plugin that enables the controller handle to be dynamically created from ROS controller type name. The next few sections will explain how to implement and export such a plugin. +Another limitation of the these controller managers is that, out of the box, they only support controllers that implement the ``FollowJointTrajectory`` action. +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. -.. note:: - In the specific case of *Gripper Action Controller*, the corresponding allocator is not exported because this controller is only used with *MoveIt Simple Controller Manager*. It ignores the commanded trajectory and simply sends the last point, thus it can only be used to open or close the gripper given the maximal force and does not provide fine-grained control over the trajectory. Advanced users configure one of the flavors of the Joint Trajectory Controller instead. +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: + +.. code-block:: yaml + + 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. + +Enabling a GripperCommand controller for a ROS controller manager +----------------------------------------------------------------- *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: @@ -430,69 +468,18 @@ The translation between `moveit_msgs::RobotTrajectory `_ interface is a poor fit for your custom controller, a custom MoveIt Controller Manager can be written that will take care of loading or unloading the controller as well as managing its state and lifecycle. - -Controller managers implemented and exported by MoveIt framework can be used as examples when implementing a custom controller manager plugin: - -* `Test MoveIt Controller Manager `_ - a bare bones example of what it takes to implement a MoveIt Controller Manager plugin. -* `MoveIt Fake Controller Manager `_ - `exported `__ by ``moveit_plugins`` package and configured by ``demo.launch`` file generated by MSA to preview the visual effect the chosen controllers would have on the robot by launching RViz with Motion Planning plugin, but without simulating robot hardware. -* `MoveIt Simple Controller Manager `_ - `exported `__ by ``moveit_plugins`` package. Configured as the default by ``move_group.launch`` file which is auto-generated by MSA, but can also be used for simulation, visualization, or with real robot hardware. Described in detail in previous sections. -* `MoveIt ROS Control Controller Manager `_ - `exported `__ by ``moveit_plugins`` package. Discussed in detail in the previous section. Configured by the ``demo_gazebo.launch`` file which is auto-generated by MSA. -* `MoveIt Multi Controller Manager `_ - supports multiple running ``ros_control`` nodes for advanced scenarios. Keeps track of which controller belongs to which node. - -While *MoveIt Simple Controller Manager* and *MoveIt ROS Control Controller Manager* have been covered extensively, this topic has not focused on *Fake* and *Multi* controller managers up to this point. These controllers are described next. - -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: - -.. code-block:: yaml - - controller_list: - - name: fake_arm_controller - type: $(arg fake_execution_type) - joints: - - shoulder_joint - - upperarm_joint - - forearm_joint - - name: fake_gripper_controller - type: $(arg fake_execution_type) - joints: - - gripper - initial: # Define initial robot poses per group - - group: arm - pose: ready - - group: gripper - pose: open - -The ``type`` setting specifies the *fake controller 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. - -Multi Controller Manager -^^^^^^^^^^^^^^^^^^^^^^^^ - -The *MoveIt Multi Controller Manager* can be used when more than one ``ros_control`` node is employed. It works by creating multiple *MoveIt ROS Control Controller Managers*, one for each node. It instantiates them with their respective namespace and takes care of proper delegation. This type of manager can be configured by setting ``moveit_controller_manager`` to ``moveit_ros_control_interface::MoveItMultiControllerManager``: +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: .. code-block:: XML - - -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, and can’t be named ``/``. - -Joints are claimed by each controller during initialization or startup. ROS controllers receive a hardware interface of the type they support (position, velocity, or effort), which they can use to request a *joint handle* for each joint they are configured to control. The act of requesting a joint handle will cause the *ROS Controller Manager* to record that a particular joint was *claimed* as a *resource* by a particular controller. - -For a particular ``ros_control`` node, MoveIt can decide which controllers to start or stop. Since only *managed controllers* (the ones with registered *controller handle allocator* plugins) are handled by MoveIt, it will take care of stopping controllers based on their *claimed resources* if a to-be-started controller needs any of those resources. + + + + +