Chapter 12

Make an arm plan, move, touch, and prove the whole task

Turn a perceived object pose into a manipulation result that can be inspected and challenged: model the arm and its surroundings, reject colliding configurations, compare randomized motion plans, add executable timing, combine local Servo alignment with a staged Task Constructor pick-and-place, reason about contact and force, and finish with a twenty-trial acceptance decision instead of a lucky demo.

Before you start

Use joint coordinates, forward and inverse kinematics, transforms, radians, velocity, acceleration, force, torque, and controller limits from the foundation weeks.
Inspect a ROS 2 graph, tf2 tree, URDF, SRDF, ros2_control controller state, action result, timestamps, and saved logs without treating an RViz animation as physical evidence.
Bring forward the framed 3D target, calibration record, uncertainty gates, and rejection reasons from the perception chapter; an untrusted pose must never become an arm command.
Run this chapter with the official MoveIt 2 tutorial robot in simulation or with recorded data. Powered-arm collision, Servo, grasp-force, and contact experiments require a separately approved hardware procedure and are not part of these lessons.

By the end

Explain workspace versus configuration space and prove why a clear tool point does not guarantee that every robot link, payload, or swept state is collision-free.
Connect URDF collision geometry, SRDF planning groups, inverse kinematics, robot state, world objects, attached objects, and the allowed-collision matrix without confusing their responsibilities.
Evaluate an OMPL sampling-based planner over repeated seeds using success, planning time, path length, and clearance rather than selecting one attractive path.
Separate a geometric path from a time-parameterized trajectory, inspect joint limits and timestamps, and verify simulated execution, feedback, cancellation, and terminal result.
Use MoveIt Servo only for bounded local commands and use MoveIt Task Constructor to expose the stages, alternatives, scene transitions, and failures of pick-and-place.
Reason about grasp contact, framed force-torque measurements, friction limits, impedance, and admittance while keeping contact tuning inside simulation or an unpowered physical model.
Run a reproducible twenty-trial manipulation matrix with planted faults, stage-level metrics, linked evidence, and a strict PASS, NEEDS REVISION, or BLOCKED decision.

The field story

Transfer the fragile sample without touching the divider

A simulated six-joint arm must move a foam laboratory vial from the blue tote into a rack across a clear divider. The tool point has an obvious free path, but the elbow can strike the divider, the wrist camera can clip the rack, and a carried vial changes the collision body. The Week 10 target pose and Week 11 confidence record enter this mission as evidence, not permission. Every scene update, candidate configuration, path, trajectory, contact transition, and controller result must remain visible.

Treat the transfer as a sequence of state changes rather than one cinematic motion. The arm begins in a known configuration, receives a framed target, explores collision-free alternatives, time-parameterizes a selected path, approaches locally, closes on the foam vial, attaches it to the planning scene, moves, releases, and retreats. A planner may fail honestly at any stage. The finish line is twenty replayable trials with seeds, cancellation, planted scene and timing faults, stage metrics, and a release decision that never confuses RViz animation with completed physical work.

Why this chapter now: Framed 3D targets and localization beliefs are useful only when the whole arm, environment, timing, contact state, and controller outcome are considered together.
Ignore for now: Do not tune a powered arm, claim force safety, or replace MoveIt planners. Use the official simulated robot and one fragile-sample task.
This unlocks: A complete manipulation baseline supports later demonstrations, policy learning, VLA integration, and acceptance testing.
Proof you will leave with: Provide URDF/SRDF and scene revisions, target and calibration identity, planner seeds, path and trajectory metrics, action feedback, cancellation result, Task Constructor stage outcomes, tactile or force-torque evidence boundaries, planted faults, twenty-trial ledger, and exact launch commands.

Environment contractRepository-supported Node.js 22.13.0 or newer runs the planning-gate starter. The learner-created integrated lab targets ROS 2 Jazzy, Gazebo Harmonic, and compatible MoveIt 2 packages with an official simulated tutorial robot; this repository does not bundle that workspace.

Compatibility boundary: Robot model, SRDF groups, controller names, planning plugins, scene message semantics, and tutorial launch paths must be verified in the active workspace. Examples from another ROS or MoveIt release require adaptation and rerun evidence.
Smoke check: Run node week-12-select-safe-plan.mjs, then launch the pinned simulation and verify model groups, controllers, planning scene, one known-valid plan, cancellation, and terminal result.
Contract reviewed: 2026-07-25
Runtime evidence: The dependency-free starter is executed by repository tests on the supported Node.js baseline. Chapter-specific ROS 2, Gazebo, model, dataset, checkpoint, and hardware environments are learner-created unless the repository supplies an explicit asset; run the smoke check and preserve its versions and output before claiming runtime compatibility.
Drift risk: medium

Today in the field story

One problem, then the next

Place the simulated arm, rack, tote, divider, and vial into one versioned planning scene. The tool point can clear the divider while the elbow collides, so inspect complete link geometry, attached bodies, padding, and swept configurations. Draw the same obstruction in workspace and configuration space. The mission begins by proving why a visually clear endpoint does not make an arm configuration or path acceptable.

Why now: Whole-arm collision reasoning must precede any search for a motion.
Ignore today: Ignore planner tuning; make scene ownership and collision checks correct first.
Unlocks next: A trustworthy validity test for candidate joint configurations.

Understand

Build the physical picture first

A robot pose is one dot on a joint-angle map, and collision checking colours every dot where any modeled body part would overlap something forbidden.

Workspace is the ordinary room around the robot; configuration space, or C-space, is a map whose coordinates are the robot's movable joints. A two-joint arm gets a two-dimensional map with shoulder angle on one axis and elbow angle on the other. One dot on that map describes the whole arm shape. A real box becomes a curved forbidden region in C-space because many different joint pairs make a link touch it.

MoveIt checks modeled bodies, not only the end-effector point. The RobotState supplies joint positions, the URDF supplies collision shapes for the links, and the planning scene supplies world objects and any object attached to the gripper. Self-collision asks whether robot parts intersect one another. Full collision checking also asks whether the padded robot intersects the modeled environment. An attached payload enlarges what moves with the arm and must be checked along the transfer.

The allowed-collision matrix is a narrow exception list, not a way to make a difficult plan succeed. During a grasp, selected fingertip or gripper links may be allowed to contact the intended object; unrelated arm links must not inherit that permission. Every exception needs a physical reason, named link-object pair, bounded task phase, and test showing that other contacts still fail. Clearing collisions globally only hides the model's warning.

A collision-free result is conditional evidence: it means the sampled states passed against the scene and geometry MoveIt had at that time. Thin obstacles can be skipped if path checking is too coarse, and an inaccurate mesh, stale scene, wrong transform, missing cable, loose payload, or unexpected person is absent from that proof. Use conservative geometry and margins, refresh the scene, revalidate after changes, and keep independent hardware safety functions outside the planner.

Words you need

Name each idea precisely

Configuration space: A coordinate space in which one point gives every independent robot joint or base coordinate needed to describe a complete configuration.
Physical example:
The point (shoulder = 30°, elbow = -60°) describes one full shape of a cardboard two-link arm.
C-space obstacle: The set of robot configurations that cause modeled self-collision or collision with an object in the world.
Physical example:
A small block on a desk becomes a broad curved patch of forbidden shoulder-elbow angle pairs.
Collision geometry: Simplified shapes or meshes used for intersection tests, separate from detailed visual artwork.
Physical example:
A gripper may look rounded in RViz but use a few boxes and low-detail meshes for faster conservative checks.
Allowed-collision matrix: A table of specific body or object pairs whose contact the collision checker is instructed to ignore.
Physical example:
Two finger pads may be allowed to touch the cup only while establishing and holding that grasp.
Attached collision object: A formerly separate world object that the scene now treats as rigidly carried by a named robot link.
Physical example:
After grasp verification, the cup moves with the tool and its body must clear the shelf during retreat.

Visual model

See the relationship

Swipe the technical canvas horizontally on a small screen.

Day 78 · Concept — The target is reachable with useful joint margin. Measured anchor: 40 deg margin.

Math, one line at a time

Work through today’s relationship

Prerequisite rescue · optionalConfiguration space, sampling, and trajectory timing

A collision-free pose is not enough; the complete joint path and timing must be feasible.

q: one point in joint configuration spaceUnit: rad or m per joint
Δq: joint change between samplesUnit: rad or m
v = Δq/Δt: joint velocityUnit: rad/s or m/s

A joint moves from 0.2 rad to 0.8 rad, so Δq = 0.6 rad.
If allocated time is 0.3 s, average velocity is 0.6/0.3 = 2 rad/s.
If the limit is 1 rad/s, increase duration to at least 0.6 s and collision-check the interpolated path.

Programmer analogy

A route can pass API validation at its endpoints while failing in the middle; robot trajectories must validate every segment.

A 0.5 rad move with a 2 rad/s limit needs at least how long?

Δt = 0.5/2 = 0.25 s.

Maximum reach is

r_{\max}=l_1+l_2=0.4+0.3=0.7\ \mathrm{m}.

At $q=(0,0)$ the links lie along the positive $x$ axis, so collision must be checked along the whole interval $0\le x\le0.7\ \mathrm{m}$ , not only at the tool.

Reject a pose whose tool point looks clear

A planar arm has links of 0.40 m and 0.30 m. At q = (0°, 0°) both links lie along positive x. A thin obstacle crosses the x-axis from x = 0.32 m to 0.38 m, while the tool is at x = 0.70 m.

Calculate maximum straight reach: 0.40 + 0.30 = 0.70 m, so the tool point lies beyond the obstacle.
Describe link one's occupied centreline at this pose: it spans x = 0.00 m through x = 0.40 m before any thickness or padding is added.
Compare intervals: the obstacle interval 0.32–0.38 m overlaps link one's interval, so the configuration is in collision.
Check the entire RobotState with full collision geometry; do not replace the link test with a tool-point distance check.
Mark the corresponding shoulder-elbow grid cell forbidden and test neighbouring angle pairs to reveal the surrounding C-space obstacle.
Keep the result conditional on modeled thickness, obstacle pose, scene time, and collision-checking resolution; those inputs must travel with the evidence.

Result

The tool is geometrically clear at 0.70 m, yet the pose is invalid because the first link crosses the obstacle between 0.32 m and 0.38 m.

What this proves

A manipulator plan must validate the whole modeled robot and payload, not a dot representing only the tool.

Physical examples

Where this appears in real life

Cardboard arm over an angle grid

Pin two cardboard strips into a planar arm, place an eraser beside the first link, and record shoulder-elbow pairs even when the paper fingertip remains far beyond the eraser.

Look for:

Many poses have a clear fingertip but a colliding forearm or upper arm; those poses form a region rather than one bad point on the joint-angle grid.

Backpack through a doorway

Walk an empty backpack through a doorway, then wear it sideways and repeat slowly without scraping the frame.

Look for:

Your centre can follow the same route while the carried shape changes clearance, just as an attached payload changes the arm's collision envelope.

Hands-on exercise

Make the idea observable

Use the cardboard arm first, then the official MoveIt tutorial robot and planning scene in simulation. Keep every real controller disabled.

Draw a shoulder-versus-elbow grid, place one fixed block near the cardboard arm, and label at least twenty joint pairs as clear or colliding by inspecting both links.
Launch the MoveIt tutorial simulation, record the robot model version, planning frame, planning group, initial RobotState, and exact obstacle ID, shape, dimensions, pose, and frame.
Add a box that blocks one arm link while leaving the end-effector target visually open; request collision contacts and save the reported link-object pair.
Run self-collision and full world-collision checks separately, clearing the previous CollisionResult before each new check so an old answer is not reused.
Add one narrowly justified fingertip-to-object allowed contact, prove that pair changes as expected, and prove an elbow-to-object collision still fails.
Attach a payload in the simulated scene, replay the transfer check, and save before-and-after scene summaries plus the configuration that changed validity.

Observe

The forbidden cardboard cells should resemble regions, and MoveIt should report the modeled link or payload that collides even when the target marker itself appears clear.

Done when

The artifact identifies one self-collision, one world collision, one permitted grasp contact, one still-forbidden contact, and one pose invalidated by attaching the payload.

Build today

Use MoveIt 2 to plan around collision objects, servo toward a target, execute a Task Constructor pick-and-place, and stop safely on contact or invalid state.

Evidence to save

DONE when the learning log explains “Configuration-space obstacles and MoveIt collision checking” in five precise points and a checked example produces the predicted output.

Common mistakes

Catch the wrong mental model

Wrong

Checking only whether the end-effector point is outside every obstacle.

Better

Check full link and payload collision geometry at the start, goal, and sufficiently dense intermediate states, including self-collision and world collision.

Wrong

Disabling a broad group of collisions because the gripper is supposed to touch the object.

Better

Permit only physically intended link-object pairs for the required task phase and verify that unrelated contacts remain detectable.

Wrong

Treating a green RViz plan as proof that the physical workcell is safe.

Better

Record scene freshness, transforms, geometry, padding, payload, and checking resolution, then rely on separately validated hardware safeguards for the real system.

Job connection

How this becomes employable evidence

Qualify a manipulator's collision model, scene-update path, payload attachment, contact exceptions, and path-checking resolution before commissioning a shelf-picking motion near fixtures.

Relevant target roles

Robotics Application / ROS 2 Integration Engineer
Robotics Deployment, Integration & Validation Engineer
Robotics Software Engineer — ROS 2 / AMR

Chapter 12 interview drill

Interview questions: Configuration-space obstacles and MoveIt collision checking

Practise a 60–90 second answer: define the idea, connect it to a physical robot, state assumptions, frames, and units when relevant, then finish with the failure signal or evidence you would inspect.

Primary interview scenario

Explain configuration-space obstacles, then diagnose a plan whose end-effector path looks clear but whose forearm collides; name the model, scene, resolution, and allowed-contact checks you would inspect.

Answer shape: clarify the situation → trace the physical and software path → test the most likely boundaries → name the evidence that would confirm the result.

Technical follow-up questions

Q1Why can a small workspace box create a large forbidden region in a two-joint C-space?

Model interview answer

Many shoulder-elbow combinations place some part of either link through the same box, so each colliding full-arm shape contributes another forbidden point.

Q2What changes in collision checking after a grasped object becomes attached to the tool?

Model interview answer

The object's geometry moves with the attached link and becomes part of the carried collision envelope for subsequent approach, lift, transfer, and retreat states.

Q3What does a collision-free MoveIt result fail to prove by itself?

Model interview answer

It does not prove the model is complete, the scene and transforms are fresh, continuous space was checked finely enough, or independent physical safety functions will stop real hazards.

Chapter references

MoveIt 2 — Planning Scene tutorialThe official distinction between self-collision and full world collision, current RobotState checks, contact reporting, padding, and deliberately scoped entries in the allowed-collision matrix.
MoveIt 2 — Motion Planning conceptsMoveIt constraints, its OMPL integration and randomized planners, request adapters, and the conversion of kinematic paths into joint-limit-aware trajectories.
MoveIt 2 — Realtime Servo tutorialServo command types, ROS and C++ interfaces, joint-limit enforcement, optional smoothing, and collision or singularity velocity scaling for local closed-loop motion.
MoveIt 2 — Pick and Place with MoveIt Task ConstructorThe executable staged pick-and-place structure, planning solvers, scene attachment transitions, task planning, solution inspection, and explicit execution step.
ros2_controllers Kilted — controller and broadcaster indexOfficial entry points for joint trajectory execution and cancellation, parallel gripper control, force-torque broadcasting, wrench transformation, and admittance control.