MoveIt 2 for manipulation: pick-and-place, constraints, dual-arm

The ROS 2 track's MoveIt lesson covers setup. This one covers what teams actually do with MoveIt for manipulation — pick-and-place, attaching grasped objects, constraint-aware planning, dual-arm coordination. The patterns that turn "MoveIt works on the demo arm" into "MoveIt picks parts off a conveyor belt for 12 hours."

The pick-and-place pipeline

Real pick-and-place is more than "plan to grasp, plan to drop." MoveIt formalizes the steps:

1. Approach: move from the current pose to a pre-grasp pose (~10 cm above the grasp). Standard motion plan.
2. Grasp descent: Cartesian linear motion from pre-grasp to grasp pose. Slow, straight-line, no replanning.
3. Close gripper: command the gripper joint(s) to closed.
4. Attach object: tell MoveIt the object is now part of the robot. The planner now treats the object as a permanently attached link.
5. Lift: Cartesian motion straight up.
6. Transit: full motion plan to the placement pre-place pose.
7. Place descent: Cartesian to the place pose.
8. Open gripper.
9. Detach object: tell MoveIt it's no longer part of the robot.
10. Retreat: Cartesian back up.

Each step has a different requirement. Phase 1, 6 are full motion planning. Phases 2, 5, 7, 10 are Cartesian (straight-line). Phases 3, 8 are gripper commands. Phases 4, 9 manipulate the planning scene state.

Attached collision objects (the killer feature)

When the robot grabs a part, the part becomes part of the robot for collision-checking purposes. MoveIt handles this with AttachedCollisionObject messages:

from moveit_msgs.msg import CollisionObject, AttachedCollisionObject
from shape_msgs.msg import SolidPrimitive

attached = AttachedCollisionObject()
attached.link_name = 'panda_hand'
attached.object.id = 'grasped_box'
attached.object.primitives = [box_primitive]
attached.object.primitive_poses = [grasp_pose_in_hand_frame]
attached.object.operation = CollisionObject.ADD

planning_scene_pub.publish(...)

Now MoveIt plans paths that account for the part: don't bash it into the table, don't squeeze it through a doorway it doesn't fit through. After release, switch operation to REMOVE.

Cartesian path computation

For the approach/lift/transit moves, you don't want a free-form motion planner that might curl the arm strangely. You want a straight line in task space:

waypoints = [pre_grasp_pose, grasp_pose]   # straight line
fraction = move_group.compute_cartesian_path(
    waypoints,
    eef_step=0.01,        # interpolate every 1 cm
    jump_threshold=0.0,    # disallow joint-space jumps
)
if fraction < 0.95:
    raise RuntimeError(f'Cartesian path only {fraction:.0%} feasible')

compute_cartesian_path returns the fraction of the trajectory that worked. Fall back to a free motion plan if the Cartesian path can't reach the goal (often it can't due to singularities or joint limits).

Constraints — "do this, but also stay within these rules"

MoveIt's constraint system: orientation, position, joint, and visibility constraints. Useful patterns:

• Keep upright: gripper-z aligned with world-z while transporting a glass of water. OrientationConstraint with tight tolerance on roll/pitch, free yaw.
• Stay within a box: end-effector inside a workspace volume. PositionConstraint.
• Avoid certain joint configurations: keep elbow up. JointConstraint.
• Visibility: keep the wrist camera looking at a target. VisibilityConstraint.

Constraint planning is slower than free planning. Use it sparingly — for the segments where it actually matters.

Dual-arm coordination

Two arms in the same MoveIt instance. Two patterns:

Pattern 1: independent arms, separate planning groups

Each arm is a separate planning group. Coordinate at the application layer: plan arm A, execute, plan arm B, execute. Easy to implement; can't take advantage of synchronized motions.

Pattern 2: combined planning group

Define a planning group that includes both arms (e.g., 7+7=14 joints). Plan the joint state of both arms simultaneously. Slower but produces coordinated motions. Used for handoff tasks.

For handoff specifically, use a relative pose constraint: arm B's gripper must stay 5 cm in front of arm A's gripper while both move. MoveIt's constraint planner handles this with the right setup.

Grasp generation integration

MoveIt's moveit_msgs/Grasp packages a grasp candidate with everything needed to execute:

msg = Grasp()
msg.grasp_pose = grasp_pose_stamped
msg.pre_grasp_approach.direction.vector.z = -1.0   # approach from above
msg.pre_grasp_approach.min_distance = 0.05
msg.pre_grasp_approach.desired_distance = 0.10
msg.post_grasp_retreat.direction.vector.z = 1.0
msg.pre_grasp_posture = open_gripper_state
msg.grasp_posture = closed_gripper_state
msg.grasp_quality = 0.92

Pass a list of these to pick(); MoveIt iterates through ranked candidates until one is reachable. This integrates cleanly with deep grasp networks (Contact-GraspNet, AnyGrasp) that output grasp poses + confidences.

Common production-grade tweaks

• IKFast for the arm: 1000× faster IK than KDL. Worth the half-day generation.
• Decimated collision meshes: 100-face simplified versions instead of 100k-face visual meshes. Collision-checking goes 10× faster with no perceivable safety loss.
• Pre-computed roadmaps: for repeated tasks (always pick from this region, place in this region), pre-compute a probabilistic roadmap; planning becomes O(1).
• Joint-acceleration limits: tune in joint_limits.yaml per-arm. Default values are often too aggressive for new hardware; loosen them and re-time the trajectory.
• Servo for the last centimeter: switch from MoveIt to MoveIt Servo for fine positioning at the contact. Faster and smoother than re-planning.

Failure modes and recoveries

• Grasp pose unreachable: try the next candidate from the grasp planner. Have at least 5 candidates queued.
• Cartesian path partial: fall back to free planning to the goal, accept a curved path.
• Object detached at lift: F/T sensor or wrist camera detects unexpected force change. Halt; retry.
• Trajectory tracking error too large: controller couldn't follow the planned path. Lower joint accel limits; replan.

Production pick-and-place stacks include retry logic for each. A 5-attempts-with-different-grasp wrapper turns 70% single-shot success into 95% effective success.

The 5-day workflow for a new robot

1. Day 1: URDF, ros2_control, MoveIt Setup Assistant. Default planner.
2. Day 2: Bring up RViz, plan to a fixed pose, execute on real hardware. Tune joint limits.
3. Day 3: Add static collision scene (table, fixtures). Test pick-and-place with hardcoded grasps on a single known part.
4. Day 4: Add perception — depth camera + grasp network. Pipeline from "see object" to "execute grasp."
5. Day 5: Add retries, tactile verification, and the safety wrapper. Run for an hour. Measure success rate.

Five days from URDF to a working pick-and-place demo. That's the bar production teams hit.

Exercise

Run moveit2_tutorials's pick-and-place example on a Panda in simulation. It works out of the box. Then break it: change the object's mass to make it heavy enough that gripper close fails. Add the failure detection. Add a retry. Now you're past tutorial-level; that's where every team spends weeks.

Next

Trajectory generation — the time-optimal and jerk-limited motion profiles that turn a planned path into a smooth, safe execution.