MotionGenerator

MotionGenerator#

MotionGenerator turns batched joint or end-effector goals into timed trajectories. Choose a planner backend, then select one of two strategies for each request:

`MotionGenOptions.strategy`	Behavior	Required inputs
`"motion_gen"` (default)	Run the configured planner.	Targets and backend options; provide the start state explicitly in the examples below.
`"ik_interp"`	Solve EEF waypoints sequentially with IK, or interpolate joint waypoints directly.	`start_qpos`, `sample_count`, and `interpolation_dt`.

The strategy selects who generates the timed trajectory. motion_gen can also interpolate a path and solve IK before planning; it still invokes the backend. Unsupported target types raise an error instead of switching strategy.

Plan a trajectory#

These examples assume sim already contains a robot with an "arm" control part and a configured IK solver. See the simulation tutorial for scene setup. B is the number of robot instances and DOF is the controlled joint count.

Initialize a backend#

from embodichain.lab.sim.motion.motion_generator import (
    MotionGenCfg,
    MotionGenOptions,
    MotionGenerator,
)
from embodichain.lab.sim.motion.planners import (
    PlanState,
    ToppraPlannerCfg,
    ToppraPlanOptions,
    TrajectorySampleMethod,
)

motion_gen = MotionGenerator(
    MotionGenCfg(
        planner_cfg=ToppraPlannerCfg(
            robot_uid=robot.uid,
            sim_instance_id=sim.instance_id,
        ),
    )
)
plan_opts = ToppraPlanOptions(
    constraints={"velocity": 0.2, "acceleration": 0.5},
    sample_method=TrajectorySampleMethod.TIME,
    sample_interval=0.01,
)
start_qpos = robot.get_qpos(name="arm")  # (B, DOF)

Joint goals#

Supply both the start and goal when calling TOPPRA without pre-interpolation. Choose a goal within the robot’s joint limits and reachable workspace.

goal_qpos = start_qpos.clone()
goal_qpos[:, 0] += 0.1

result = motion_gen.generate(
    [PlanState.from_qpos(start_qpos), PlanState.from_qpos(goal_qpos)],
    MotionGenOptions(
        strategy="motion_gen",
        start_qpos=start_qpos,
        control_part="arm",
        plan_opts=plan_opts,
    ),
)
assert result.is_all_success()

Cartesian goals#

For a joint-only backend such as TOPPRA, enable pre-interpolation to convert Cartesian waypoints through the robot’s IK solver. Native Cartesian backends receive the original targets directly.

Pre-interpolation solves the sampled poses in order, using each solution as the next IK seed. A failed required pose fails that environment’s trajectory; the generator never drops it to report a shorter path as successful. The backend then applies its own path and timing behavior, so these input samples do not guarantee an exact Cartesian line in the final output.

goal_pose = robot.compute_fk(qpos=start_qpos, name="arm", to_matrix=True)
goal_pose = goal_pose.clone()  # (B, 4, 4), arena-local TCP pose
goal_pose[:, 2, 3] += 0.02

result = motion_gen.generate(
    [PlanState.from_xpos(goal_pose)],
    MotionGenOptions(
        strategy="motion_gen",
        start_qpos=start_qpos,
        control_part="arm",
        plan_opts=plan_opts,
        is_interpolate=True,
        is_linear=True,
        interpolate_position_step=0.002,
    ),
)

Deterministic interpolation#

Use explicit sampling and timing to generate a path without invoking the configured planner. EEF targets use the previous IK solution as the next seed; joint targets are interpolated directly.

result = motion_gen.generate(
    [PlanState.from_qpos(goal_qpos)],
    MotionGenOptions(
        strategy="ik_interp",
        start_qpos=start_qpos,
        control_part="arm",
        sample_count=50,
        interpolation_dt=0.02,
    ),
)

Interpolation alone does not enforce velocity or acceleration limits or check collisions. Passing plan_opts, velocity_limit, or acceleration_limit with ik_interp raises an error. Use a suitable planner when those constraints are required.

To preserve a supplied Cartesian path, use ik_interp with preserve_cartesian_samples=True and provide exactly sample_count - 1 EEF poses after the start. Each pose is solved with IK and retained at the explicit interpolation_dt; no joint-space resampling follows. is_linear=True in this strategy requires those explicit samples. The caller defines their geometry, including whether they form a line.

motion_gen with preserve_cartesian_samples=True is unsupported and raises an error: the current backend contract cannot guarantee exact Cartesian samples while also applying backend timing and path constraints.

Inputs and results#

PlanState.from_qpos() takes (B, DOF) tensors and PlanState.from_xpos() takes (B, 4, 4) tensors. For a single environment, PlanState.single() also accepts unbatched inputs. All waypoints must use the same batch size as the robot.

generate() returns a PlanResult:

Field	Shape	Meaning
`success`	`(B,)`	Per-environment planning status; `is_all_success()` checks every row.
`positions`	`(B, N, DOF)`	Joint position samples; `generate()` requires a trajectory from the backend.
`velocities`, `accelerations`	`(B, N, DOF)` when present	Joint derivatives. Missing velocities are derived from the final timed positions.
`dt`	`(B, N)`	Arrival intervals, required whenever positions are present.
`duration`	`(B,)`	Derived as `dt.sum(dim=1)`.
`constraint_report`	mapping or `None`	Backend diagnostics, retained only while the reported trajectory is unchanged.

When sample_count changes a backend’s output grid, the generator resamples in time, preserves each row’s duration, recomputes velocities, and invalidates accelerations and constraint diagnostics. If the backend supports joint trajectory validation, the final samples are checked again against the resolved collision scene, and a failed check fails that environment. This sampled check does not certify continuous motion between samples or enforce derivative limits. Backends may preserve native samples; see TrapezoidalPlanner and cuRobo for their controls.

Failed rows hold start_qpos with zero velocity when it is supplied, except for backends that explicitly preserve failed rollout positions, such as NeuralPlanner. Always check success before execution.

Execute in simulation#

Planning returns a trajectory; playback applies it on the simulator’s fixed control clock. With sim in explicit-update mode:

from embodichain.lab.sim.motion.execution import (
    JointTrajectoryPlaybackCfg,
    play_joint_trajectory,
)

assert result.is_all_success()
play_joint_trajectory(
    sim,
    robot,
    positions=result.positions,
    dt=result.dt,
    joint_ids=robot.get_joint_ids("arm"),
    cfg=JointTrajectoryPlaybackCfg(joint_command_mode="position_velocity"),
)

Playback defaults to one command per physics step. An explicit control_dt must be an integer multiple of physics_dt; playback retimes the trajectory and recomputes velocity targets on that grid without changing the physics period. Position-only playback is the default; the example opts into velocity targets, with zero velocity at the first, terminal, and padded hold samples.

Atomic Skills and Gym experts own their execution cadence and holds. See Atomic actions and the expert data tutorial for those workflows. Teleporting through samples, as the cuRobo visualization demo does, shows a path but does not measure physical tracking.

For a physical position-only versus velocity-target comparison, run examples/sim/motion/trajectory_velocity_tracking.py. For complete options and helper methods such as estimate_trajectory_sample_count(), see the MotionGenerator API.