Tool Changing

Many applications require swapping grippers or tools during execution. This document describes our recommended approach to implement these type of applications.

An example showing it put into practice is available in the fanuc_sim robot configuration in the moveit_pro_example_ws workspace:

1. Run Setup Planning Scene to populate the initial knowledge of tool location.
2. Run Tool Attachment Example to perform tool attachment and docking.

Robot modeling

Our recommended approach is to have robot URDFs and tool URDFs in separate files, for instance:

- user_workspace
  - description
    - robot.urdf
    - one_tool.urdf
    - another_tool.urdf

The SRDF and other robot configuration files should not include any gripper-specific groups or links either. Tool-specific information should go into the tool URDFs, therefore fully decoupling robot and tool description files.

Runtime tool attachments

The general approach when attaching a new tool to a robot arm is:

1. Move the arm tip to mate the tool at the tool holder.
2. Execute the mechanism that physically attaches the tool to the arm. This is highly dependent on your tool changing hardware. It may be passively activated by arm motion, or require activating an IO signal, etc.
3. Bring up tool-specific hardware interfaces (e.g. establish communication channels) and controllers.
4. Execute the mechanism that physically detaches the tool from the tool holder. This could also be passive or require electrical activation.
5. Update the internal collision model representation to include the new tool.

Detaching a tool would be similar, but in reverse order:

1. Move the arm with the attached tool to dock into the tool holder.
2. Trigger the mechanism that attaches the tool to the tool holder.
3. Bring down tool-specific hardware interfaces and controllers so that those resources are available for other tools.
4. Trigger the mechanism that detaches the tool from the arm.
5. Update the internal collision model to not include the tool.

Steps 2 to 4 are highly dependent on the specific tool changing hardware and may even require dedicated controllers. MoveIt Pro implements mechanisms to achieve those steps in simulation (see section below), but it is up to the user to implement them for their specific hardware.

Step 5 (update the internal collision model) can be achieved with the AttachTool and DetachTool Behaviors:

• AttachTool expands the robot model with a tool, so that it can be used for collision checking and path planning. The tool is attached to the robot at a given link. Once attached to the robot, the tool will move with the link it is attached to. If the tool contains joints and those joints are reported in the /joint_states topic, the tool collision shapes will update their state accordingly.

• DetachTool removes a previously attached tool from the robot model. Once detached, the tool collision shapes will no longer be part of the robot model.

AttachTool requires the tool to exist in the 'Planning Scene' beforehand, which can be done with the AddToolToPlanningScene Behavior. The tool is given as a URDF file, which describes the kinematics and collision geometry. Calling AttachTool basically updates the parent link of the tool to be a robot link, instead of the static 'world' frame. Calling DetachTool sets the parent link of the tool back to the 'world' frame, so it won't move with the robot anymore.

The tool will still exist in the 'Planning Scene' after DetachTool is called. This is to prevent collision with it on subsequent motion planning requests, for instance if the tool is docked in a tool holder. To remove the tool permanently from the 'Planning Scene', the RemoveToolFromPlanningScene Behavior can be called.

It is important to call AttachTool and DetachTool right after physically attaching or detaching a tool to prevent potential collisions or misleading planning failures.

Tool Changing in a digital twin

This section explains how you could implement a tool changing mechanism in simulation.

The simulator in MoveIt Pro allows enabling or disabling Mujoco 'weld' constraints at runtime, via controllers. A 'weld' constraint allows attaching any two bodies so that they move together. Therefore you can simulate the attaching mechanism of a physical tool holder via weld constraints in simulation.

To do that, define a weld constraint in the Mujoco model for the two bodies that you want to attach or detach at runtime:

<equality>
    <weld name="tool_holder_constraint" body1="tool_holder" body2="tool"/>
</equality>

Note that you will also need to do this for the tool and arm flange, which also need to attach / detach at runtime.

<equality>
    <weld name="arm_tool_constraint" body1="arm_flange" body2="tool"/>
</equality>

MoveIt Pro digital twin can create controllers to enable / disable Mujoco weld constraints at runtime. To do so, define a site with the prefix suction_cup_ at the bodies where the tool will be attached, e.g.:

<body name="arm_flange">
    <geom ... margin="0.01" gap="0.01" />
    <site name="suction_cup_arm_flange" />
</body>
<body name="tool_holder">
    <geom ... margin="0.01" gap="0.01" />
    <site name="suction_cup_tool_holder" />
</body>

The arguments margin and gap control the activation distance (see Mujoco's documentation).

Then create controllers for those site in your ros2_control.yaml file, e.g.:

controller_manager:
  ros__parameters:
    # Used to activate / deactivate the mechanism that attaches the tool to the arm flange.
    arm_flange_attach_mechanism:
      type: position_controllers/GripperActionController

    # Used to activate / deactivate the mechanism that attaches the tool to the tool holder
    tool_holder_mechanism:
      type: position_controllers/GripperActionController

arm_flange_attach_mechanism:
  ros__parameters:
    default: true
    joint: suction_cup_arm_flange # Should match the Mujoco site name.
    allow_stalling: true

tool_holder_attach_mechanism:
  ros__parameters:
    default: true
    joint: suction_cup_tool_holder # Should match the Mujoco site name.
    allow_stalling: true

These controllers are of type GripperActionController, which expose a /gripper_cmd action that can be set to 1 or 0 to activate / deactivate the weld constraint. MoveIt Pro includes a MoveGripperAction Behavior that can be used to interface with these controllers.

Current limitations

• Cartesian Teleoperation modes (IMarker, Pose Jog) have very limited support for multiple tools. You can't teleoperate tool-specific frames, only the arm tip link.
• Some Behaviors involving kinematics have limited support for links that are not part of the planning group, i.e. they won't work for tool links. A workaround is to transform targets given in a tool frame to the arm tip frame before calling those Behaviors.
• The AttachTool Behavior does not support xacro. Only plain URDFs can be attached as tools.