Configuring a Mobile Base Robot

This page describes the design and configuration of mobile base support in MoveIt Pro.

MoveIt supports planning for floating and planar joints by internally representing the dimensions as a set of variables. For instance, a mobile planar joint is represented by a 2D position x and y and a rotation theta. MoveIt implements simple diff drive and holonomic drive joint models which include interpolation and basic drive kinematics, enabling path planning and kinematic sampling primarily for short-range purposes and for coordinated motions of arm and mobile base. The controller execution is enabled by running ros2_control’s JointTrajectoryController (JTC) on the planar joint variables, and forwarding velocity commands to a base drive-specific controller.

The image below shows how mobile base support is enabled by the different nodes:

../../../_images/mobile_base_design.png

This tutorial describes the following necessary configuration steps: - Configure the planar joint in MoveIt to enable base trajectory planning and execution - Configure ros2_control’s base drive controller for executing drive-specific motions - Configure ros2_control’s JointTrajectoryController to track MoveIt’s trajectories by chaining velocity commands to the base drive controller - Broadcast multi-DOF joint states to MoveIt Pro’s UI for odometry visualization

1. Configure the Planar Base Joint for MoveIt

MoveIt requires that a base joint type is configured in the robot model for path planning and kinematics. Since this joint representation is specific to MoveIt, we only add it as a virtual joint to the SRDF. The parent frame of the base is odom which is published as TF transform by the controller (see ros2_control’s diff_drive_controller) or the hardware driver. This example configures a diff drive position joint for the Stretch RE1 robot.

<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="position" type="planar" parent_frame="odom" child_link="base_link" />
<joint_property joint_name="position" property_name="motion_model" value="diff_drive" />
<joint_property joint_name="position" property_name="min_translational_distance" value="0.01" />

For trajectory execution and ros2_control support, we are also configuring MoveIt to represent the base waypoints using planar joint variables instead of multi-DOF joint states by setting trajectory_execution.control_multi_dof_joint_variables to true.

2. Configure the base drive controller

The ros2_controllers repository provides controller implementations for different mobile base drive types. The drive controller typically takes base velocity commands as Twist messages which are in turn processed as wheel controls. Depending on the type, the controller may also compute drive dynamics and broadcast Odometry to a dedicated odom topic or TF.

The diff drive of the Stretch RE1 is actuated by two continuous wheel joints (here, left wheel in the URDF):

<joint
  name="joint_right_wheel"
  type="continuous">
  <origin
    xyz="-0.00300000000000034 -0.15765 0.0508000000000004"
    rpy="-1.57079632679489 -1.11022302462516E-16 7.28583859910277E-17" />
  <parent
    link="base_link" />
  <child
    link="link_right_wheel" />
  <axis
    xyz="0 0 1" />
</joint>

The yaml snippet below shows the ros2_control configuration of the Stretch, binding a DiffDriveController to both wheel joints (from stretch.ros2_control.yaml). Take note of the extra parameters for wheel measurement, these are important for computing the correct wheel controls and Odometry. The Odometry publisher must be enabled for TF (enable_odom_tf: true) and the odom_frame_id and base_frame_id parameters must match the parent and child frames of MoveIt’s base position joint, as configured in the previous step.

controller_manager:
  ros__parameters:

    diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

# Controller configuration for HelloRobot Stretch RE1
# See https://docs.hello-robot.com/0.2/stretch-hardware-guides/docs/hardware_guide_re1/#base
diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["joint_left_wheel"]
    right_wheel_names: ["joint_right_wheel"]

    wheel_separation: 0.3 # estimate! (340mm - 2x20mm wheel width)
    wheels_per_side: 1
    wheel_radius: 0.0508

    wheel_separation_multiplier: 1.0
    left_wheel_radius_multiplier: 1.0
    right_wheel_radius_multiplier: 1.0

    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_link
    pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]

    open_loop: true
    enable_odom_tf: true

    cmd_vel_timeout: 0.5

    # Velocity and acceleration limits, should match MoveIt's default_joint_limits.yaml
    # Whenever a min_* is unspecified, default to -max_*
    linear.x.has_velocity_limits: true
    linear.x.has_acceleration_limits: true
    linear.x.has_jerk_limits: false
    linear.x.max_velocity: 0.42
    linear.x.max_acceleration: 0.2
    linear.x.max_jerk: 0.0

    angular.z.has_velocity_limits: true
    angular.z.has_acceleration_limits: true
    angular.z.has_jerk_limits: false
    angular.z.max_velocity: 0.42
    angular.z.max_acceleration: 0.2
    angular.z.max_jerk: 0.0

The DiffDriveController setup can be run on ros2_control’s Mock Components, simulator integrations, or the real hardware if ros2_control is supported by the hardware driver.

3. Configure ros2_control’s JointTrajectoryController

The JointTrajectoryController (JTC) is usually responsible for executing motions for the joints of the robot kinematics. With mobile bases, we configure a JointTrajectoryController to track the Cartesian planar joint trajectory and produce velocities that can be forwarded to the base drive controller, here DiffDriveController.

Since MoveIt’s planar joint is configured in the SRDF and floating joints are not supported by ros2_control, the base drive representation of MoveIt is unavailable for claiming joint resources in the JTC. Instead, we configure the simple joint variables for the planar joint as individual joints inside the URDF’s ros2_control tag. That way they are only visible to ros2_control and don’t cause any issues with TF or visualization backends. See below for how the stretch_base.ros2_control.xacro includes x, y, and theta position joints, and note that they are completely detached from the kinematic chain.

<ros2_control name="${name}" type="system">

    <!-- ... -->

    <!-- Virtual position joint for supporting MoveIt's planar joint type in joint trajectories -->
    <joint name="position/x">
        <param name="initial_position">0.0</param>
        <command_interface name="velocity" />
        <state_interface name="position"/>
        <state_interface name="velocity"/>
    </joint>
    <joint name="position/y">
        <param name="initial_position">0.0</param>
        <command_interface name="velocity" />
        <state_interface name="position"/>
        <state_interface name="velocity"/>
    </joint>
    <joint name="position/theta">
        <param name="initial_position">0.0</param>
        <command_interface name="velocity" />
        <state_interface name="position"/>
        <state_interface name="velocity"/>
    </joint>

    <!-- ... -->

</ros2_control>

Using those joints, we can now configure the JTC and directly use it for cascading velocity commands to the DiffDriveController:

stretch_base_controller:
  type: joint_trajectory_controller/JointTrajectoryController

  stretch_base_controller:
    ros__parameters:
      command_interfaces:
        - velocity
      state_interfaces:
        - position
        - velocity
      open_loop_control: true # disables PID
      joints:
        - position/x
        - position/y
        - position/theta
      command_joints:
        - diff_drive_controller/x
        - diff_drive_controller/y
        - diff_drive_controller/theta

The joints parameter claims the joint variables to use for controls, the command_joints parameter opts for writing joint commands to the interfaces exported by the DiffDriveController, the command_interfaces specifies that the JTC should calculate velocity commands. The allow_integration_in_goal_trajectories is needed for mocking velocity commands. The example configuration is also running the JTC in open loop mode, which means that no feedback and no PID controller is used for adjusting the motion during execution. With mobile bases, the feedback would have to be provided by updating the position joint variables from the the odometry signal of the base drive.

4. Publish multi-DOF joint states to the UI

The Web UI uses a joint state subscription for updating the kinematic state of the robot. For mobile bases, MoveIt Pro depends on MultiDOFJointStates topic at /multi_dof_joint_states. A node for converting and republishing the odometry signal as MultiDOFJointStates is provided with the repub_odometry_mdof_joint_states.py script.