Tool Changing, End Effector Swapping, and Vacuum Grippers
Many applications require swapping grippers, end effectors, or tools during execution. This document describes our recommended approach to implement these type of applications. This approach can also be used to approximate a vacuum gripper, magnetic gripper, or even a nail gun!
An example showing it put into practice is available in the factory_sim robot configuration in the moveit_pro_example_ws workspace:
- Run
Setup Planning Sceneto populate the initial knowledge of tool location. - Run
Tool Attachment Exampleto perform tool attachment and docking.
Simulation specification for attaching and detaching objects
This section explains how you could implement a tool changing, vacuum gripper, or magnetic gripper 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 "weld site" with the prefix suction_cup_ inside any body tags where you will want to be able to attach the tool, 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>
weld constraints cannot be activated on child body tags and act only on the geom of a parent body.
This can happen when trying to include a mjcf definition instead of writing it directly.
scene.xml
<replicate count="10" sep="-" offset="0 0 0.1">
<body name="part-1" pos="1.1 2.3 5.6" euler="-0.5 -0.5 -0.5">
<include file="part_geometry.xml" />
</body>
</replicate>
<equality>
<weld
name="suction_cup_part-1-0"
body1="part-1-0"
body2="tool"
active="false"
torquescale="5"
/>
<weld
name="suction_cup_part-1-1"
body1="part-1-1"
body2="tool"
...
/>
...
</equality>
part_geometry.xml
<mujoco model="part_geometry">
<body name="my_part"> <!-- This becomes a child body of part-1-* -->
<joint type="free" damping="0.0001" />
<geom type="box" ... />
...
</body>
</mujoco>
In this case, the parent body part-1-* does not have any geometry itself but contains a child body named my_part which contains geometry. When the simulator checks for collisions, part-1-* lacks collision geometry and the weld will never activate. To solve this, remove one of the body tags so the parent directly contains geom tags.
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.
When you send a 1 to the controller, it enables checking if any of your weld constraints involving the body associated with this controller should be activated. The check is whether or not those two bodies are currently in collision: if they are, the weld equality is activated with the current relative pose between the bodies (e.g. it does not "Snap" the two bodies together to be flush).
When you send a 0 to the controller, it (1) disables checking if a weld constraint should be activated if it involves the body associated with this controller, and (2) deactivates any activated weld constraints involving the body associated with the "weld site" for that service.
In this way the GripperActionController commands behave similarly to turning on or off vacuum power - you turn it on first, then make contact with the object.
Additionally, be sure to activate your controllers by listing them in your config.yaml under controllers_active_at_startup
ros2_control:
config:
package: "my_site_config_sim"
path: "config/ros2_control.yaml"
# MoveIt Pro will load and activate these controllers at start up to ensure they are available.
# If not specified, it is up to the user to ensure the appropriate controllers are active and available
# for running the application.
# [Optional, default=[]]
controllers_active_at_startup:
- "joint_trajectory_controller"
- "joint_state_broadcaster"
- "arm_flange_attach_mechanism"
- "tool_holder_attach_mechanism"
Robot modeling of detachable tools
To enable motion planning to be done in the frame of the attached tool's tool tip, we must add it to the robot model when it is attached. 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:
- Move the arm tip to mate the tool at the tool holder.
- 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.
- Bring up tool-specific hardware interfaces (e.g. establish communication channels) and controllers.
- Execute the mechanism that physically detaches the tool from the tool holder. This could also be passive or require electrical activation.
- Update the internal collision model representation to include the new tool.
Detaching a tool would be similar, but in reverse order:
- Move the arm with the attached tool to dock into the tool holder.
- Trigger the mechanism that attaches the tool to the tool holder.
- Bring down tool-specific hardware interfaces and controllers so that those resources are available for other tools.
- Trigger the mechanism that detaches the tool from the arm.
- 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 AttachURDF and DetachURDF Behaviors:
-
AttachURDFexpands the robot model with a URDF object, so that it can be used for collision checking and path planning. The URDF object is attached to the robot at a given link. Once attached to the robot, the URDF object will move with the link it is attached to. If the URDF object contains joints and those joints are reported in the/joint_statestopic, the URDF object collision shapes will update their state accordingly. -
DetachURDFremoves a previously attached URDF object from the robot model. Once detached, the URDF object collision shapes will no longer be part of the robot model.
AttachURDF requires the URDF object to exist in the 'Planning Scene' beforehand, which can be done with the AddURDF Behavior.
The URDF object is given as a URDF file, which describes the kinematics and collision geometry.
Calling AttachURDF basically updates the parent link of the URDF object to be a robot link, instead of the static 'world' frame.
Calling DetachURDF sets the parent link of the URDF object back to the 'world' frame, so it won't move with the robot anymore.
The URDF object will still exist in the 'Planning Scene' after DetachURDF 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 URDF object permanently from the 'Planning Scene', the RemoveURDFFromScene Behavior can be called.
It is important to call AttachURDF and DetachURDF right after physically attaching or detaching a tool to prevent potential collisions or misleading planning failures.
These behaviors modify the planning scene as soon as they are ticked.
Use SetupMTCAttachObject or SetupMTCDetachObject to add an MTC stage that will modify the planning scene, but only apply the changes when their stage of motion is executed by the ExecuteMTCTask behavior.
The deprecated AttachTool and DetachTool behaviors are still available but should be replaced with AttachURDF and DetachURDF respectively. The new behaviors are located in the "Planning Scene" subcategory and provide the same functionality with more general URDF object support.
Current limitations
In progress March 2025:
- 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
AttachURDFBehavior does not support xacro. Only plain URDFs can be attached as objects.