Generate manipulator motion from trajectories and targets#
Move a Franka Panda robot with two complementary motion-generation workflows. First, time-parameterize a known sequence of joint poses. Then, use cuMotion RMPflow to recompute joint commands from a Cartesian target and measured articulation state.
Learning objectives#
After completing this tutorial, you can:
Build complete joint poses without assuming articulation degree-of-freedom (DOF) order.
Convert joint-space waypoints into a velocity- and acceleration-limited trajectory.
Express a Cartesian goal as a named tool-frame state.
Reset and advance cuMotion RMPflow from measured robot state.
Choose between planned joint motion and reactive Cartesian control.
Prerequisites#
Complete Install and run.
Run the commands in this tutorial from the repository root.
Use a graphical desktop for interactive runs. The bounded tests use offscreen rendering.
Allow the examples to resolve the Franka Panda asset, or set
ISAACSIM_ASSET_ROOTto another asset root.
Examples used#
Example |
Role in this tutorial |
|---|---|
Executes a time-parameterized joint-space trajectory. |
|
Uses RMPflow to reactively follow a Cartesian target. |
Each example page lists its source directory, requirements, and runner commands.
Choose a motion representation#
Workflow |
Use it when |
Controller behavior |
|---|---|---|
Joint trajectory |
You know the joint-space waypoints and limits before execution. |
Samples a fixed, time-parameterized path. The example refreshes measured state when a cycle restarts. |
Cartesian target |
You know the desired tool pose and may update it while the robot moves. |
RMPflow recomputes a joint command from the target and current measured state on every physics step. |
Neither representation is universally preferable. The task-level goal, available constraints, and required response to changing state determine which one to use.
Generate a joint trajectory#
Resolve complete poses by name#
The Franka articulation reports its complete DOF order. Build each pose by joint name so the motion does not depend on where the seven arm and two finger joints appear in that array:
def _make_joint_pose(dof_names: list[str], arm_positions: tuple[float, ...]) -> np.ndarray:
"""Build one named Franka pose in articulation order.
Args:
dof_names: Ordered degree-of-freedom names.
arm_positions: Positions for the seven arm joints.
Returns:
Joint positions in articulation order.
"""
expected_names = (*_ARM_JOINTS, *_FINGER_JOINTS)
missing = [name for name in expected_names if name not in dof_names]
if missing:
raise RuntimeError(f"Franka articulation is missing joints: {', '.join(missing)}")
positions_by_name = dict(zip(_ARM_JOINTS, arm_positions, strict=True))
positions_by_name.update(dict.fromkeys(_FINGER_JOINTS, _FINGER_POSITION))
return np.asarray([positions_by_name[name] for name in dof_names], dtype=np.float32)
Time-parameterize the waypoints#
The example constructs a Path through pose A, pose B, and pose A again. It then computes a minimal-time joint
trajectory within the velocity and acceleration limits assigned to each DOF:
def _make_trajectory(pose_a: np.ndarray, pose_b: np.ndarray, dof_names: list[str]) -> mg.Trajectory:
"""Time-parameterize a two-pose Franka round trip.
Args:
pose_a: Starting joint pose.
pose_b: Opposite joint pose.
dof_names: Ordered degree-of-freedom names.
Returns:
Minimal-time trajectory from pose A to pose B and back.
"""
maximum_velocities = np.asarray([0.55 if name in _ARM_JOINTS else 0.08 for name in dof_names], dtype=np.float32)
maximum_accelerations = np.asarray([1.2 if name in _ARM_JOINTS else 0.20 for name in dof_names], dtype=np.float32)
waypoints = np.asarray([pose_a, pose_b, pose_a], dtype=np.float32)
# The follower consumes CPU robot state, so do not inherit Warp's CUDA default on GPU workers.
path = mg.Path(wp.array(waypoints, dtype=wp.float32, device="cpu"))
return path.to_minimal_time_joint_trajectory(
max_velocities=maximum_velocities,
max_accelerations=maximum_accelerations,
robot_joint_space=dof_names,
active_joints=dof_names,
)
Specifying the CPU device prevents Warp’s default CUDA device from moving these small control arrays to the GPU.
Follow the trajectory#
TrajectoryFollower samples the trajectory at simulation time. The example expands each named result into the
articulation’s complete DOF order, applies it, advances OvPhysX, and publishes the measured transforms:
def advance_simulation() -> None:
nonlocal completed_cycles
nonlocal cycle_start_state
nonlocal simulation_complete
nonlocal trajectory_step
simulation_time = min(trajectory_step * _PHYSICS_DT, trajectory.duration)
# `TrajectoryFollower` samples an open-loop trajectory, so the state is refreshed only when a cycle resets.
desired = follower.forward(cycle_start_state, None, simulation_time)
if desired is None or desired.joints is None or desired.joints.positions is None:
raise RuntimeError("TrajectoryFollower returned no Franka joint command.")
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.")
trajectory_step += 1
if trajectory_step >= trajectory_step_count:
completed_cycles += 1
print(f"Completed Franka trajectory cycle {completed_cycles}.")
if not visible:
simulation_complete = True
return
trajectory_step = 0
measured_positions = articulation.get_dof_positions().numpy()[0].copy()
cycle_start_state = make_robot_state(measured_positions)
if not follower.reset(cycle_start_state, None, 0.0):
raise RuntimeError("TrajectoryFollower could not restart the Franka trajectory.")
This is open-loop trajectory sampling within a cycle: the follower uses the cycle’s initial state and simulated time. The physics engine still determines the measured motion, which the test uses to verify that Franka reaches both poses and returns.
Follow a Cartesian target#
Express named target and measured states#
RMPflow needs a target for a named site and the robot’s measured joint state. The Follow Target example names the
panda_leftfingertip tool frame in both the loaded robot configuration and the setpoint:
def _make_setpoint(robot_site_space: list[str], target_position: np.ndarray) -> mg.RobotState:
"""Build the target position requested from RMPflow.
Args:
robot_site_space: Site names exposed by the robot description.
target_position: Requested world-space tool position.
Returns:
Named site-position setpoint.
"""
return mg.RobotState(
sites=mg.SpatialState.from_name(
spatial_space=robot_site_space,
positions=([TOOL_FRAME], wp.array([target_position], dtype=wp.float32, device="cpu")),
)
)
def _make_estimated_state(robot: ArticulationEntity) -> mg.RobotState:
"""Build the measured joint state used to reset RMPflow.
Args:
robot: Simulated Franka articulation.
Returns:
Named measured joint state.
"""
robot_joint_space = list(robot.dof_names)
return mg.RobotState(
joints=mg.JointState.from_name(
robot_joint_space=robot_joint_space,
positions=(robot_joint_space, robot.get_dof_positions()),
velocities=(robot_joint_space, robot.get_dof_velocities()),
)
)
Names keep the site and joint values attached to their spaces, so the motion-generation API can map controller output to the corresponding articulation DOFs.
Initialize cuMotion RMPflow#
Load the tuned Franka description, create the world interface, and initialize the controller from measured state:
# Configure RMPflow with the robot model, tool frames, and world transform.
robot_config = load_cumotion_supported_robot("franka")
robot_site_space = list(robot_config.robot_description.tool_frame_names())
if TOOL_FRAME not in robot_site_space:
raise RuntimeError(f"Franka cuMotion configuration does not expose tool frame {TOOL_FRAME!r}.")
world_interface = CumotionWorldInterface(device="cpu")
world_interface.update_world_to_robot_root_transforms(poses=(robot_positions, robot_orientations_wxyz))
controller = RmpFlowController(
cumotion_robot=robot_config,
cumotion_world_interface=world_interface,
robot_joint_space=robot_joint_space,
robot_site_space=robot_site_space,
tool_frame=TOOL_FRAME,
)
setpoint = _make_setpoint(robot_site_space, TARGET_POSITION)
if not controller.reset(_make_estimated_state(robot), setpoint, 0.0):
raise RuntimeError("RmpFlowController failed to reset from the measured Franka state.")
The world-to-robot-root transform tells RMPflow how to interpret the world-space Cartesian goal. Resetting from the measured articulation state prevents the controller from starting with an assumed joint configuration.
Close the feedback loop#
Each fixed step reads measured positions and velocities, evaluates RMPflow, maps the named result into the complete articulation target, advances physics, and publishes measured transforms:
def _advance_simulation(
robot: ArticulationEntity,
manager: PhysicsManager,
controller: RmpFlowController,
setpoint: mg.RobotState,
simulation_time: float,
) -> float:
"""Apply one RMPflow command, advance physics, and publish its transforms to OVStage.
Args:
robot: Simulated Franka articulation.
manager: Active physics manager.
controller: Initialized RMPflow controller.
setpoint: Desired robot state for this step.
simulation_time: Current simulation time in seconds.
Returns:
Simulation time after the step.
"""
desired_state = controller.forward(_make_estimated_state(robot), setpoint, simulation_time)
if desired_state is None or desired_state.joints is None or desired_state.joints.positions is None:
raise RuntimeError("RmpFlowController did not produce joint position targets.")
position_targets = robot.get_dof_positions().numpy().copy()
position_targets[:, desired_state.joints.position_indices.numpy()] = desired_state.joints.positions.numpy()
robot.set_dof_position_targets(position_targets)
manager.step()
if not manager.publish_transforms_to_stage():
raise RuntimeError("Physics transform publication failed.")
return simulation_time + TIME_STEP
The interactive run updates the target along a bounded path. The test holds it fixed, then computes tool position from measured joint state and forward kinematics rather than treating the command itself as evidence of success.
Run the workflows#
Run the joint trajectory:
source/examples/example.sh run motion_generation.follow_trajectory
.\source\examples\example.bat run motion_generation.follow_trajectory
Run the Cartesian target follower:
source/examples/example.sh run motion_generation.follow_target
.\source\examples\example.bat run motion_generation.follow_target
Press Escape to close either viewport.
Verify the workflows#
Run the tests for both examples:
source/examples/example.sh test motion_generation.follow_trajectory
source/examples/example.sh test motion_generation.follow_target
.\source\examples\example.bat test motion_generation.follow_trajectory
.\source\examples\example.bat test motion_generation.follow_target
The Follow Trajectory test.py checks both configured poses, measured articulation travel, the return to pose A,
and changing rendered frames. The Follow Target test.py checks measured tool-position error and confirms that the
rendered frame changes as Franka moves.
Tradeoffs#
A joint trajectory gives explicit timing and predictable interpolation, but it does not react to a moving Cartesian goal within a cycle.
RMPflow reacts to a new Cartesian target and measured articulation state, but the target must remain reachable and the controller must have an accurate robot and world model.
The Follow Target example tracks an unobstructed point. Register relevant collision geometry with the world interface before using the same pattern around obstacles.
Change the trajectory poses or limits to study timing and tracking error. Change TARGET_POSITION or the
interactive target amplitudes to explore Franka’s reachable workspace, then rerun the corresponding test.
Next steps#
Continue with Compose a pick-and-place state machine to choose Cartesian targets and gripper commands from measured task state.