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

Today in the field story

One problem, then the next

Build the transfer as explicit stages: current state, open, connect, approach, close, attach, lift, move, lower, open, detach, and retreat. Use Task Constructor to retain failed alternatives and Servo only for a bounded local correction with fresh commands and cancellation. When close is commanded, do not mark the vial attached until the mission’s declared evidence permits that scene transition.

Why now: A multi-step task needs visible alternatives and state transitions, not one opaque planner call.
Ignore today: Ignore autonomous recovery invention; expose each stage and its declared failure result.
Unlocks next: A complete pick-and-place graph ready for contact reasoning and acceptance trials.

Understand

Build the physical picture first

Servo is a careful stream of local nudges, while Task Constructor is a visible recipe whose stages carry the robot and object from one valid scene state to the next.

MoveIt Servo accepts frequent local commands expressed as joint velocity, end-effector velocity, or desired end-effector pose. It is useful for teleoperation, visual alignment, and other closed-loop corrections where a new measurement can adjust the next small motion. It is not a global task planner: Servo does not choose which object to pick, invent a grasp sequence, or prove that a distant route through clutter exists.

A Servo command needs a declared command frame, scale, rate, timestamp or freshness policy, and halt behavior. MoveIt Servo can enforce joint position and velocity limits and can optionally smooth motion and scale velocity near modeled collisions or singularities. Those features must be configured and tested; disabled collision checking, stale scenes, wrong command frames, or delayed commands still matter. Software scaling is useful risk reduction, not a safety-rated stop.

MoveIt Task Constructor, or MTC, exposes a complex manipulation job as connected stages. Generators create possible states such as grasp poses, propagators extend a state forward or backward, and connectors find motion between state sets. Serial containers require an end-to-end chain; parallel containers represent alternatives. Each stage passes interface states and may produce zero, one, or many solutions, so the failed stage and rejected alternatives become inspectable evidence.

A pick-and-place task usually includes current state, open gripper, move to pick, generate grasp candidates, solve grasp IK, approach, allow intended object-hand contact, close, attach, lift, connect to the place region, lower, open, detach, forbid contact again, and retreat. Planning and execution are separate calls. Scene transitions must follow verified physical meaning: a close command is not proof of grasp, and a planned task solution is not proof that its controllers executed every subtrajectory.

Words you need

Name each idea precisely

MoveIt Servo: A MoveIt component that turns repeated local joint, Cartesian-velocity, or Cartesian-pose commands into bounded arm motion.
Physical example:
A camera sends small tool corrections that centre a gripper over a peg without replanning the entire workcell route each update.
Command frame: The coordinate frame whose axes give the direction and orientation meaning of a Servo command.
Physical example:
Positive x may mean forward from the camera or outward from the tool, producing different motion if the frame is wrong.
Singularity: A robot configuration where some Cartesian motion directions become poorly conditioned or require extremely large joint rates.
Physical example:
Near a fully stretched simple arm, a small sideways tool command may demand large opposing joint motion.
MTC stage: One named planning operation that consumes, produces, or connects interface states inside a manipulation task.
Physical example:
A GenerateGraspPose stage proposes candidates while a later ComputeIK wrapper rejects those with no valid arm configuration.
Interface state: The robot and planning-scene state passed between Task Constructor stages with properties needed by later stages.
Physical example:
After attachment, the next stage receives a scene in which the cup moves with the tool instead of remaining on the table.
Task solution: A complete compatible chain of stage results that can be inspected and then explicitly submitted for execution.
Physical example:
One solution connects the selected grasp, lift, transfer, place, and retreat trajectories while other candidate chains remain rejected.

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.

Repeated Cartesian increments imply

v=\Delta x\,f=(0.002\ \mathrm{m})(50\ \mathrm{Hz})=0.10\ \mathrm{m/s}.

A stale interval of $0.200\ \mathrm{s}$ spans

N=f t=50(0.200)=10

update opportunities.

Bound a Servo stream and place it inside a staged task

A simulated final-alignment loop publishes tool-x increments of 0.002 m at 50 Hz. The application declares a 0.10 m/s command limit and stops accepting a command after 0.20 s without a fresh observation.

Calculate the implied steady speed if every increment is applied: 0.002 m × 50 /s = 0.10 m/s, exactly the declared command limit.
Calculate how many update opportunities fit inside the freshness window: 50 /s × 0.20 s = 10 cycles.
Do not replay the last increment for those ten cycles; check freshness before each update and begin the configured halt as soon as the observation contract expires.
Express the increment in the named tool or camera frame, transform it at the correct time, and test that reversing the frame cannot silently reverse physical direction.
Use MTC for the global approach and grasp-candidate stages, allow Servo only for the bounded final alignment state, then halt it before gripper close and scene attachment.
Advance to attach and lift only when the simulated grasp-verification condition succeeds; otherwise preserve the object as world geometry and return a named task failure.

Result

The local stream reaches at most 0.10 m/s, has a ten-cycle-long 0.20 s freshness interval, and is contained inside a task whose grasp and scene transitions remain explicit.

What this proves

Command arithmetic, freshness, frames, and stage ownership keep a helpful local Servo loop from becoming an unbounded substitute for task planning.

Physical examples

Where this appears in real life

Remote-control nudges versus a delivery route

Use arrow cards to nudge a toy car the final few centimetres into a drawn parking box, then compare that with writing the whole route around furniture before movement begins.

Look for:

Local nudges correct nearby alignment but do not decide the global route or destination; Servo and a task planner solve different scales of the problem.

Pick-and-place recipe cards

Arrange cards labeled approach, close, verify, attach, lift, transfer, lower, open, verify release, detach, and retreat; remove or reorder one card.

Look for:

A missing verification or early attachment creates an impossible scene story even when every individual motion card looks reasonable.

Hands-on exercise

Make the idea observable

Use the official MoveIt Servo and Task Constructor examples with the tutorial robot in simulation. Start with conservative scaling and keep hardware discovery disabled.

Draw and then implement an MTC graph that names current state, open, approach, grasp generation, IK, close, attach, lift, transfer, place, open, detach, and retreat stages.
Run nominal planning, inspect every candidate and failed stage, and save the complete solution graph before calling execution in simulation.
Insert one impossible grasp orientation, one blocked approach, and one unreachable place pose; verify that each fault stops at a different named stage with a specific reason.
Add a Servo final-alignment segment with explicit command frame, input type, scaling, update rate, freshness window, collision checking, singularity thresholds, and halt messages.
Test a stale-command stream, wrong-frame command, near-singularity command, and approaching-obstacle command; capture output status and scaled or halted joint behavior for each.
Execute one simulated complete task, joining MTC stage logs, Servo input and status, scene attachment evidence, controller results, and final object pose under one trial ID.

Observe

The staged task should expose where candidates disappear, while Servo should alter or halt local motion at configured limits without pretending to solve failed global stages.

Done when

One repeatable simulation produces a complete pick-and-place trace plus four bounded Servo or stage failures, with no scene attachment before grasp verification.

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 a comparison table for “MoveIt Servo and Task Constructor pick-and-place” contains the test condition, metric, result, and justified engineering decision.

Common mistakes

Catch the wrong mental model

Wrong

Using Servo as if repeated local commands guarantee a globally feasible route through clutter.

Better

Use a global planner or staged task for route and task feasibility; reserve Servo for bounded local corrections with fresh feedback and explicit halt behavior.

Wrong

Assuming collision or singularity scaling is active and safety certified because Servo supports those features.

Better

Inspect the actual parameters and status, test every boundary, keep the planning scene fresh, and retain independently validated protective stops for hardware.

Wrong

Attaching the object immediately after the close-gripper stage starts.

Better

Wait for the declared simulated or hardware grasp evidence, then perform one traceable attach transition; otherwise fail the task with the object still in the world.

Job connection

How this becomes employable evidence

Combine a camera-guided local alignment loop with a staged industrial pick task, expose Servo status and MTC stage failures to operators, and qualify frames, timeouts, collision scaling, scene transitions, and controller results.

Relevant target roles

Robotics Application / ROS 2 Integration Engineer
Robotics Software Engineer — ROS 2 / AMR
Robot HMI / Control & Monitoring Engineer
Robotics Deployment, Integration & Validation Engineer

Chapter 12 interview drill

Interview questions: MoveIt Servo and Task Constructor pick-and-place

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

Contrast MoveIt Servo with global motion planning, then sketch a Task Constructor pick-and-place and identify where freshness, singularity, collision, grasp verification, attachment, and execution results belong.

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

Q1Which problem does MoveIt Servo solve that Task Constructor does not solve in the same way?

Model interview answer

Servo turns frequent local feedback commands into bounded responsive motion, while Task Constructor searches and connects explicit stages of a larger manipulation task.

Q2Why must a Servo command name its frame and freshness rule?

Model interview answer

Axes have different physical meanings in different frames, and an old command can keep moving the arm after the observation or operator intent that created it is no longer valid.

Q3What should happen when no grasp candidate can pass the MTC IK stage?

Model interview answer

The task should return an inspectable stage failure, keep the object as world geometry, avoid gripper or attachment claims, and choose an explicit retry or rejection policy.