Compose a pick-and-place state machine#
Compose motion-generation primitives into a measured task workflow. This tutorial uses a finite-state machine, cuMotion RMPflow, and measured robot and cube state to stack a cube and move it to a second destination with a Franka Panda robot.
Learning objectives#
After completing this tutorial, you can:
Divide a physical manipulation task into bounded phases.
Advance phases from measured convergence, grasp, carry, release, and settling conditions.
Convert a task-level grasp point into a Cartesian RMPflow target.
Combine arm and gripper commands without coupling task logic to the physics loop.
Validate a manipulation task from object, robot, and rendered state.
Prerequisites#
Complete Generate manipulator motion from trajectories and targets.
Run the commands in this tutorial from the repository root.
Use a graphical desktop for the interactive run. The bounded test uses offscreen rendering.
Allow the example to resolve the Franka Panda asset, or set
ISAACSIM_ASSET_ROOTto another asset root.
Example used#
Example |
Role in this tutorial |
|---|---|
Combines motion policies, explicit task states, and transition checks. |
The example page lists its source directory, requirements, and runner commands.
Build and run the example#
Build the example once to install its pinned transitions dependency, then run it:
source/examples/example.sh build motion_generation.pick_place_state_machine
source/examples/example.sh run motion_generation.pick_place_state_machine
.\source\examples\example.bat build motion_generation.pick_place_state_machine
.\source\examples\example.bat run motion_generation.pick_place_state_machine
The viewport shows the robot place the cyan cube on the orange cube, then move it to a new position on the ground. Press Escape after both transfers finish.
Model the task as bounded phases#
One transfer separates approach, grasp, carry, release, and settling into phases with one guarded outgoing transition each:
Each physical action contains the exact phase names used by state_machine.py.#
The transition graph keeps the order, guard, and timeout for every active phase in one declarative structure:
# (source, destination, guard, timeout_seconds)
_GRAPH = (
("approach_pick", "descend_pick", "_motion_complete", 20.0),
("descend_pick", "grasp", "_motion_complete", 10.0),
("grasp", "lift", "_grasp_complete", 6.0),
("lift", "approach_place", "_lift_complete", 10.0),
("approach_place", "descend_place", "_motion_complete", 20.0),
("descend_place", "release", "_motion_complete", 10.0),
("release", "retreat", "_release_complete", 6.0),
("retreat", "settle", "_motion_complete", 10.0),
("settle", "done", "_settle_complete", 8.0),
)
A timeout turns stalled motion into a clear failure. Without a bound, an unreachable target, lost grasp, or invalid controller command could leave the example running indefinitely.
Advance from measured guards#
The state machine receives task measurements instead of physics or controller objects:
@dataclass(frozen=True, slots=True)
class GuardInput:
"""Measurements used to decide whether the current phase is complete."""
position_error: float
orientation_error: float
finger_positions: tuple[float, float]
object_height: float
grasp_distance: float
placement_error: float
object_linear_speed: float
object_angular_speed: float
On each physics step, tick rejects invalid time or measurements, enforces the current phase timeout, detects a
lost carried cube, and evaluates at most one transition:
def tick(self, status: GuardInput, time: float) -> bool:
"""Evaluate one bounded transition from one physics-step measurement.
Args:
status: Measurements used by the current phase guard.
time: Current simulation time in seconds.
Returns:
Whether the machine changed phases.
"""
if self.is_done:
return False
values = (
time,
status.position_error,
status.orientation_error,
*status.finger_positions,
status.object_height,
status.grasp_distance,
status.placement_error,
status.object_linear_speed,
status.object_angular_speed,
)
if not all(math.isfinite(float(value)) for value in values) or time < self._last_time:
raise RuntimeError(f"Transfer {self._transfer_index + 1} {self.phase}: invalid measurement.")
elapsed = time - self._phase_started
if elapsed >= _PHASE_TIMEOUTS[self.phase]:
raise RuntimeError(f"Transfer {self._transfer_index + 1} {self.phase}: timed out at {elapsed:.2f} s.")
if self.phase in _CARRY_PHASES and status.grasp_distance > _CARRY_DISTANCE:
if self._carry_lost_since is None:
self._carry_lost_since = time
if time - self._carry_lost_since >= _CARRY_LOSS_DWELL:
raise RuntimeError(f"Transfer {self._transfer_index + 1} {self.phase}: lost the carried cube.")
else:
self._carry_lost_since = None
changed = bool(self._machine.events["tick"].trigger(self, status=status, time=float(time)))
self._last_time = float(time)
return changed
Guards combine several kinds of evidence:
Motion phases require position and orientation error to stay within tolerance.
Grasp requires converged motion, plausible finger separation, proximity to the cube, and a gripper dwell.
Lift requires both tool convergence and measured cube rise.
Carry phases fail if the cube remains too far from the grasp point.
Settle requires placement error, linear speed, and angular speed to remain below thresholds.
The shared stability dwell prevents one favorable sample from advancing a phase. A failed sample restarts the dwell period.
Connect task state to motion control#
The simulation loop observes the robot and cubes, asks the state machine for the current grasp target and gripper decision, advances RMPflow and physics once, and repeats:
Observe, choose, command, and simulate once per physics step.#
main.py converts measured state into GuardInput, resets RMPflow when the phase target changes, combines the
arm result with the state machine’s finger state, and applies the complete articulation commands:
target, hand_target = _target_state(
site_space, np.asarray(fsm.target_position, dtype=np.float64)
)
orientation_dot = abs(float(np.dot(hand_orientation, _GRASP_ORIENTATION)))
phase_changed = fsm.tick(
GuardInput(
position_error=float(np.linalg.norm(hand_position - hand_target)),
orientation_error=2.0 * float(np.arccos(np.clip(orientation_dot, 0.0, 1.0))),
finger_positions=tuple(float(value) for value in joint_positions[finger_indices]),
object_height=float(moving_position[2]),
grasp_distance=float(np.linalg.norm(grasp_position - moving_position)),
placement_error=float(np.linalg.norm(moving_position - place_position)),
object_linear_speed=float(
np.linalg.norm(block_linear_velocities.numpy()[_MOVING_BLOCK])
),
object_angular_speed=float(
np.linalg.norm(block_angular_velocities.numpy()[_MOVING_BLOCK])
),
),
simulation_time,
)
if phase_changed:
target, _ = _target_state(site_space, np.asarray(fsm.target_position))
if controller_phase is None or phase_changed:
if not controller.reset(estimated, target, simulation_time):
raise RuntimeError(f"cuMotion RMPflow reset failed in {fsm.phase}.")
controller_phase = fsm.phase
arm_state = controller.forward(estimated, target, simulation_time)
desired = mg.combine_robot_states(arm_state, finger_states[fsm.close_fingers])
if desired is None or desired.joints is None:
raise RuntimeError("Could not combine RMPflow and finger RobotState commands.")
joints = desired.joints
for values, indices, setter in (
(joints.positions, joints.position_indices, articulation.set_dof_position_targets),
(joints.velocities, joints.velocity_indices, articulation.set_dof_velocity_targets),
(joints.efforts, joints.effort_indices, articulation.set_dof_efforts),
):
if values is not None:
setter(_full_command(values, indices, len(dof_names)))
physics_manager.step()
if not physics_manager.publish_transforms_to_stage():
raise RuntimeError("Physics transform publication failed.")
simulation_time += _PHYSICS_DT
State-machine target positions refer to the pinch center, while RMPflow controls panda_hand. _target_state
applies a fixed hand-to-grasp offset and grasp orientation to keep this frame conversion in one place.
Only the ground is registered as a cuMotion planning obstacle. The cubes still collide through OvPhysX, which makes the grasp and stack physical, but RMPflow does not plan collision-free motion around the cubes themselves.
Reuse one transfer for stack and unstack#
Before each transfer, main.py measures the moving cube and calls fsm.reset with new world-space pick and
place positions. The first destination is one cube height above the support cube. The second is a ground position.
The phase graph and guards do not change.
This separation keeps reusable task policy in state_machine.py and
scene-specific observation and actuation in main.py. To change the final
placement, update _UNSTACK_DESTINATION. Keep its Z coordinate at half the
cube size for a ground placement, and update the corresponding expected
position in test.py.
Verify the workflow#
Run the example tests:
source/examples/example.sh test motion_generation.pick_place_state_machine
.\source\examples\example.bat test motion_generation.pick_place_state_machine
The test.py file attaches test-specific observers to the bounded
stack-and-unstack task. It verifies grasp, lift, carry, release, settling,
support-cube stability, final placement, and changing rendered frames
independently of the state machine’s completion messages.
Tradeoffs#
A finite-state machine makes phases, failure bounds, and recovery points explicit, but the number of transitions grows as task branching increases.
Finger position and cube motion provide a portable grasp signal, but they cannot report contact force. Add a force or contact sensor when the task needs force thresholds.
Fixed grasp offsets are concise for one known object. Use object-relative grasp generation for varied shapes and poses.
Register all collision geometry that the motion policy must avoid; physics collision alone does not make the planned path collision-free.
Next steps#
Extend the task with an explicit recovery state for a lost grasp, or use Derive sensor readings from physics state to add measurement-driven checks without coupling them to the task graph.