Version: 9

Interacting with the Planning Scene

An integral part of planning motions in MoveIt Pro involves the concept of a Planning Scene. The Planning Scene is the source of truth of the environment for the robot to plan motions in. More information on the Planning Scene can be found here.

In MoveIt Pro, there are multiple Behaviors that interact with the Planning Scene. These Behaviors can add collision objects to the Planning Scene, introspect the Planning Scene, attach objects to the robot in the Planning Scene, and much more. This guide covers the common operations: adding, moving, removing, attaching, and setting collision rules for objects. Each is available two ways: as a Behavior that updates the live Planning Scene, and as a MoveIt Task Constructor (MTC) stage that applies the change as part of a planned motion.

MoveIt Pro 9.4

The Behaviors for interacting with the Planning Scene were reworked in MoveIt Pro 9.4. Most of the collision-object Behaviors in this guide require version 9.4 or later; the Behaviors they replace are covered under Migrating from the legacy Behaviors.

Key concepts

A collision object is geometry that the robot must reason about while planning.

World objects and attached objects. A collision object is either free in the world or attached to a robot link, never both. An attached object moves with the link it is bound to. Each object is identified by a plain-text id (for example box), and you are responsible for keeping ids unique. To remove or re-pose an object that is attached, detach it first.
The live scene versus a supplied scene. Every scene-modifying Behavior, and SetCollisionRule, exposes an optional bidirectional planning_scene port. Left unwired, the Behavior reads and updates the live Planning Scene through the /get_planning_scene and /apply_planning_scene services, which is the common case. Wired to a blackboard variable, the Behavior reads the scene from that variable, applies the change in memory, and writes the result back to the same variable without calling any service, so you can build up a scene and plan against it before committing it.
The Allowed Collision Matrix (ACM). The ACM records which pairs of entities are allowed to collide. SetCollisionRule edits it so that, for example, the gripper fingers may collide with a grasped part. See the Planning Scene concept page for background.
Two ways to apply a change. Most operations come in two forms: a Behavior that updates the live Planning Scene, and a SetupMTC* Behavior that folds the change into an MTC motion plan as one of its stages.

Introspecting the Planning Scene

The following Behaviors read the Planning Scene and report information about it without modifying it.

GetCurrentPlanningScene reads the live scene into a blackboard variable.
IsCollisionObjectInPlanningScene checks whether an object with a given id is present.
IsAnyObjectAttached checks whether any object is attached to the robot.
IsObjectAttachedTo checks whether a given object is attached to a given link.
WhichObjectIsAttached reports which object is attached to a link.

Modifying the Planning Scene

These Behaviors change the live Planning Scene the moment they tick. Each of the add, move, remove, attach, detach, and collision-rule Behaviors accepts the optional planning_scene port described in Key concepts; the examples below leave it unwired to operate on the live scene.

Add a collision object

AddCollisionBox builds the geometry and applies it to the scene in one step. Build the pose first, then add the box:

<Control ID="Sequence">
  <Action ID="CreatePoseStamped" reference_frame="world" position_xyz="0.5;0.0;0.1" pose_stamped="{box_pose}" />
  <Action ID="AddCollisionBox" object_id="box" dimensions="0.3;0.3;0.3" pose="{box_pose}" />
</Control>

AddCollisionCylinder (height, radius), AddCollisionSphere (radius), and AddCollisionMesh (mesh_file_path, scale) work the same way for their shape. To apply a moveit_msgs/CollisionObject you have already built, use AddCollisionObject with its collision_object port.

By default, adding an object whose id already exists in the world fails. Set overwrite="true" to replace the existing world object instead. overwrite never affects an attached object: if the id is currently attached to the robot, the add fails regardless, and you must detach the object first.

Move a collision object

MoveCollisionObject updates the reference pose of an existing object by id. All of the object's shapes follow the new pose, so multi-shape objects move as a unit.

<Action ID="MoveCollisionObject" object_id="box" pose="{new_pose}" />

The move fails if no object with that id exists or if the id is attached to the robot. When operating on a supplied scene, the new pose must be in the same frame as the existing object.

Remove a collision object

RemoveCollisionObject removes a world object by id.

<Action ID="RemoveCollisionObject" object_id="box" />

Removing an id that is not present is a no-op that returns success, so this Behavior is safe to run at the start of an Objective to clear leftovers. It fails only if object_id is empty or if the id is currently attached to the robot; detach the object first in that case.

Attach and detach objects

AttachObject binds an existing world object to a robot link so it travels with that link. Use the optional allowed_collision_links port to let nearby links, such as the gripper fingers, collide with the object.

<Action ID="AttachObject" object_id="box" link_name="grasp_link" allowed_collision_links="left_finger;right_finger" />

By default the object keeps its current pose relative to the link. Wire the optional relative_transform port (the transform of the object frame in the link frame) to attach the object at a chosen pose instead; if the object is already attached to that link, it is re-posed to the new transform. Build the transform with CreateTransform, whose transform output wires directly to relative_transform.

DetachObject returns an attached object to the world at its current pose:

<Action ID="DetachObject" object_id="box" />

Detaching an id that is not attached is a no-op that returns success. Objects added through the tool-changing workflow are managed by AttachURDF and DetachURDF instead; see Tool Changing.

Allow or forbid collisions

SetCollisionRule edits the ACM to allow or forbid collisions between two entities, without an MTC task.

<Action ID="SetCollisionRule" name_a="manipulator" name_b="table" allow_collision="true" />

Each of name_a and name_b is resolved as a planning group, a collision object id, or a robot link; a planning group expands to its links that carry collision geometry. The names are not directional, so the order does not matter. A name that matches more than one of these categories, or matches nothing, fails the Behavior, because silently doing nothing is not acceptable for a collision-rule change. Keep your planning groups, collision objects, and links from sharing names to avoid this.

Plan against a scene without changing the live scene

Wiring the planning_scene port lets you build up a scene in memory and plan against it before committing anything to the robot. Point GetCurrentPlanningScene and the scene-modifying Behaviors at the same blackboard variable:

<Control ID="Sequence">
  <Action ID="GetCurrentPlanningScene" planning_scene_msg="{scene}" />
  <Action ID="AddCollisionBox" object_id="probe" dimensions="0.1;0.1;0.1" pose="{probe_pose}" planning_scene="{scene}" />
</Control>

{scene} now holds the modified scene while the live scene is untouched. Pass {scene} to any Behavior that takes a planning-scene input, such as ComputeInverseKinematics or ValidateTrajectory (which read it from their planning_scene_msg port), to evaluate the change before applying it.

Other planning-scene Behaviors

ResetPlanningSceneObjects removes all collision and attached objects.
UpdatePlanningSceneService updates the collision octomap from a point cloud.

It is also possible to add keepout zones and to save and load a Planning Scene through the UI; see Keepout Zones.

Modifying the Planning Scene during MTC planning

The SetupMTC* Behaviors add scene changes as stages within a MoveIt Task Constructor task, so each change becomes part of the motion the task plans. Use them when a change belongs at a specific point in a multi-step motion rather than up front, such as attaching a part to the gripper at the moment it is grasped so the rest of the pick is planned with the part attached.

<Control ID="Sequence">
  <Action ID="InitializeMTCTask" task_id="pick" task="{mtc_task}" />
  <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
  <Action ID="SetupMTCSetCollisionRule" name_a="gripper" name_b="box" allow_collision="true" task="{mtc_task}" />
  <Action ID="SetupMTCAttachObjectByID" object_id="box" link_name="grasp_link" task="{mtc_task}" />
  <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
</Control>

Each operation from the previous section has a matching MTC stage:

SetupMTCAddCollisionBox, SetupMTCAddCollisionCylinder, SetupMTCAddCollisionSphere, and SetupMTCAddCollisionMesh add a shape, with the same shape ports as their direct counterparts plus an overwrite port.
SetupMTCRemoveCollisionObject removes an object by id; removing an absent id is a no-op, and removing an attached id fails planning.
SetupMTCAttachObjectByID and SetupMTCDetachObjectByID attach or detach an object, and both require link_name. For attach it is the target link; for detach it is required because the stages are built before the task runs, so the runtime scene cannot be queried to discover the current parent link. SetupMTCAttachObjectByID also accepts the optional allowed_collision_links and relative_transform ports.
SetupMTCSetCollisionRule edits the ACM. name_a and name_b follow the same resolution rules as SetCollisionRule, resolved against the task's planning scene at plan time.

Migrating from the legacy Behaviors

The ModifyObjectInPlanningScene, AddVirtualObjectToPlanningScene, CreateCollisionObjectFromSolidPrimitive, CreateSolidPrimitiveBox, and RemoveCollisionObjectFromPlanningScene Behaviors, along with the legacy MTC stages SetupMTCUpdateGroupCollisionRule, SetupMTCUpdateObjectCollisionRule, SetupMTCIgnoreCollisionsBetweenObjects, SetupMTCAttachObject, and SetupMTCDetachObject, are deprecated in favor of the Behaviors above (MoveIt Pro 9.4 and later). They still run, but will be removed in a future major version. See the collision object migration guide for the full mapping and side-by-side examples.