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_ROOT to another asset root.

Example used#

Example

Role in this tutorial

Pick-and-Place State Machine

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.

Franka Panda robot stacking the cyan cube on the orange cube, then unstacking it.

Model the task as bounded phases#

One transfer separates approach, grasp, carry, release, and settling into phases with one guarded outgoing transition each:

Approach contains approach_pick and descend_pick. Grasp contains grasp. Carry contains lift, approach_place, and descend_place. Release contains release and retreat. Settle contains settle and done.

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:

The loop observes the robot and cubes, chooses the next task phase, moves the arm and gripper, advances physics, 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.