embodichain.lab.sim.motion.planners

embodichain.lab.sim.motion.planners#

Motion planning stack.

BasePlanner trajectory planners (TOPPRA, trapezoidal, neural, and cuRobo) produce joint trajectories from waypoints. Motion generation composes these backends in embodichain.lab.sim.motion.motion_generator.

Classes

`BasePlannerCfg`	BasePlannerCfg(robot_uid: 'str' = <factory>, planner_type: 'str' = <factory>, sim_instance_id: 'int' = <factory>)
`BasePlanner`	Base class for trajectory planners.
`ToppraPlannerCfg`	Configure the shared CPU/CUDA Warp time parameterizer.
`ToppraPlanner`	Rest-to-rest joint-path timing with differentiable CPU/CUDA Warp kernels.
`TrapezoidalPlanOptions`	Configure trapezoidal or Double-S trajectory generation.
`TrapezoidalPlannerCfg`	Configure the batched joint-space time-profile planner.
`TrapezoidalPlanner`	Plan batched linear joint paths with trapezoidal or Double-S timing.
`TrajectorySampleMethod`	Enumeration for different trajectory sampling methods.
`MovePart`	Enumeration for different robot parts to move.
`MoveType`	Enumeration for different types of movements.
`PlanResult`	Data class representing the result of a motion plan (env-batched).
`PlanState`	Data class representing the state for a motion plan (env-batched).

Base Planner#

class embodichain.lab.sim.motion.planners.BasePlannerCfg[source]#

BasePlannerCfg(robot_uid: ‘str’ = <factory>, planner_type: ‘str’ = <factory>, sim_instance_id: ‘int’ = <factory>)

Attributes:

`robot_uid`	UID of the robot to control.
`sim_instance_id`	ID of the SimulationManager that owns `robot_uid`.

robot_uid: str#: UID of the robot to control. Must correspond to a robot added to the simulation with this UID.

sim_instance_id: int#: ID of the SimulationManager that owns robot_uid. Defaults to 0.

class embodichain.lab.sim.motion.planners.BasePlanner[source]#

Bases: ABC

Base class for trajectory planners.

This class provides common functionality that can be shared across different planner implementations.

Parameters:: cfg (BasePlannerCfg) – Configuration object for the planner.

Methods:

`__init__`(cfg)
`default_plan_options`()	Return backend-default planning options.
`is_satisfied_constraint`(vels, accs, ...[, ...])	Check if the trajectory satisfies velocity and acceleration constraints.
`plan`(target_states[, options])	Execute trajectory planning.
`supports_move_type`(move_type)	Return whether the planner accepts a movement target type directly.
`validate_joint_trajectory`(trajectory, *, ...)	Validate exact joint samples without replacing their path.
`visualize_robot_collision_models`(control_part)	Visualize the robot collision models used by this planner.
`with_collision_world`(options, *, obstacle_poses)	Attach dynamic obstacle poses to backend planning options.
`with_motion_context`(options, *, start_qpos, ...)	Attach MotionGenerator runtime context to backend options.

Attributes:

`collision_world_info`	Return the planner's collision-world contract, if it has one.
`preserve_plan_samples`	Whether callers must retain this planner's returned sample points exactly.
`supported_move_types`	Movement target types accepted directly by this planner.
`supports_collision_world_updates`	Whether per-plan dynamic obstacle poses can update the collision world.
`supports_heterogeneous_waypoints`	Whether one plan may contain an ordered mixture of movement types.
`supports_joint_trajectory_validation`	Whether exact joint samples can be checked against bounds/collisions.
`uses_sparse_joint_waypoints`	Whether joint targets are sparse waypoints owned by the planner.

__init__(cfg)[source]#

property collision_world_info: CollisionWorldInfo | None#: Return the planner’s collision-world contract, if it has one.

default_plan_options()[source]#

Return backend-default planning options.

Return type:: PlanOptions

is_satisfied_constraint(vels, accs, constraints, *, control_part=None)[source]#

Check if the trajectory satisfies velocity and acceleration constraints.

This method checks whether the given velocities and accelerations satisfy the constraints defined in constraints. It allows for some tolerance to account for numerical errors in dense waypoint scenarios.

Parameters:

vels (Tensor) – Velocity tensor (…, DOF) where the last dimension is DOF
accs (Tensor) – Acceleration tensor (…, DOF) where the last dimension is DOF
constraints (dict) – Dictionary containing ‘velocity’ and ‘acceleration’ limits. None velocity resolves robot speed caps; None acceleration uses the planner default.
control_part (str | None) – Control group for resolving unspecified joint limits.

Returns:

True if all constraints are satisfied, False otherwise

Return type:

bool

Note

Allows 10% tolerance for velocity constraints
Allows 25% tolerance for acceleration constraints
Prints exceed information if constraints are violated
Assumes symmetric constraints (velocities and accelerations can be positive or negative)
Supports batch dimension computation, e.g. (B, N, DOF) or (N, DOF)

abstract plan(target_states, options=PlanOptions())[source]#

Execute trajectory planning.

This method must be implemented by subclasses to provide the specific planning algorithm.

Parameters:

target_states (list[PlanState]) – list of PlanState waypoints. Tensor fields carry a leading batch dim B (e.g. qpos is (B, DOF)).

Returns:

An env-batched object containing:

success: torch.Tensor (B,) bool, per-env success
positions: torch.Tensor (B, N, DOF), joint positions
velocities: torch.Tensor (B, N, DOF) or None, joint velocities. Populated by planners that compute dynamics; may be None for planners that do not.
accelerations: torch.Tensor (B, N, DOF) or None, joint accelerations. Populated by planners that compute dynamics; may be None for planners that do not.
dt: torch.Tensor (B, N), per-point time deltas
duration: derived torch.Tensor (B,), total trajectory duration per env

Returning positions without dt raises at PlanResult construction. duration is always derived from dt.sum(dim=1).

Return type:

PlanResult

preserve_plan_samples: bool = False#

Whether callers must retain this planner’s returned sample points exactly.

When True, MotionGenerator returns the planner’s trajectory without resampling, preserving planner-owned samples and derivatives (including collision-checked samples where applicable). When False (the default), the generator may normalize the trajectory to a requested waypoint count.

supported_move_types: frozenset[MoveType] = frozenset({})#

Movement target types accepted directly by this planner.

MotionGenerator uses this declaration to validate targets and determine whether Cartesian targets must first be converted into joint waypoints for a joint-only backend.

supports_collision_world_updates: bool = False#: Whether per-plan dynamic obstacle poses can update the collision world.

supports_heterogeneous_waypoints: bool = False#: Whether one plan may contain an ordered mixture of movement types.

supports_joint_trajectory_validation: bool = False#: Whether exact joint samples can be checked against bounds/collisions.

supports_move_type(move_type)[source]#

Return whether the planner accepts a movement target type directly.

Parameters:: move_type (MoveType) – Movement target type to query.
Return type:: bool
Returns:: True when plan() accepts the target type without MotionGenerator preprocessing.

uses_sparse_joint_waypoints: bool = False#

Whether joint targets are sparse waypoints owned by the planner.

When True, MotionGenerator prepends the observed start_qpos before dispatching a joint target. This lets the planner own both path timing and derivative generation without making every caller materialize the current state as a waypoint. Cartesian preparation also keeps only requested goals unless is_linear explicitly requests a dense Cartesian path.

validate_joint_trajectory(trajectory, *, control_part, obstacle_poses=None)[source]#

Validate exact joint samples without replacing their path.

Backends that implement this contract must evaluate every supplied sample against joint bounds, self-collision, and their configured world collision model. They return a boolean mask with shape (B, T).

Parameters:

trajectory (Tensor) – Simulator-order joint samples with shape (B, T, D).
control_part (str) – Robot control part whose ordered joints form D.
obstacle_poses (Mapping[str, Tensor] | None) – Optional current dynamic-obstacle world poses.

Return type:

Tensor

Returns:

Per-environment, per-sample validity mask.

Raises:

NotImplementedError – Always for the base planner.

visualize_robot_collision_models(control_part, env_id=0)[source]#

Visualize the robot collision models used by this planner.

Planners that support collision avoidance should override this method with their backend-specific visualization.

Parameters:

control_part (str) – Robot control part whose collision models are visualized.
env_id (int) – Simulator environment instance to visualize.

Raises:

NotImplementedError – If the planner does not support collision avoidance.

Return type:

None

with_collision_world(options, *, obstacle_poses)[source]#

Attach dynamic obstacle poses to backend planning options.

The base planner does not consume a collision world. Backends whose collision_world_info enables updates override this method.

Parameters:

options (PlanOptions) – Backend-specific options to enrich.
obstacle_poses (Mapping[str, Tensor]) – Batched world poses keyed by stable obstacle ID.

Return type:

PlanOptions

Returns:

Planning options unchanged for a backend without world updates.

with_motion_context(options, *, start_qpos, control_part)[source]#

Attach MotionGenerator runtime context to backend options.

The base planner fills an absent control group when the options support it. Backends with additional contextual options override this method.

Parameters:

options (PlanOptions) – The backend’s planning options, already constructed (either by the caller or via default_plan_options()).
start_qpos (Tensor | None) – Optional starting joint configuration (B, DOF).
control_part (str | None) – Optional control-part name.

Return type:

PlanOptions

Returns:

The (possibly mutated) planning options carrying the context.

Toppra Planner#

class embodichain.lab.sim.motion.planners.ToppraPlannerCfg[source]#

Configure the shared CPU/CUDA Warp time parameterizer.

Attributes:

`grid_size`	Minimum timing grid size; each spline segment is subdivided uniformly.
`robot_uid`	UID of the robot to control.
`sim_instance_id`	ID of the SimulationManager that owns `robot_uid`.

grid_size: int = 100#: Minimum timing grid size; each spline segment is subdivided uniformly.

robot_uid: str#: UID of the robot to control. Must correspond to a robot added to the simulation with this UID.

sim_instance_id: int#: ID of the SimulationManager that owns robot_uid. Defaults to 0.

class embodichain.lab.sim.motion.planners.ToppraPlanner[source]#

Bases: BasePlanner

Rest-to-rest joint-path timing with differentiable CPU/CUDA Warp kernels.

Uniform not-a-knot cubic geometry is timed under conservative interval bounds. Results preserve the planner’s float32 output contract.

Methods:

`__init__`(cfg)	Validate the timing grid and resolve the configured robot.
`close`()	Complete the planner lifecycle; CPU workers belong to the shared runtime.
`default_plan_options`()	Return a valid time-sampled default when no count is requested.
`is_satisfied_constraint`(vels, accs, ...[, ...])	Check if the trajectory satisfies velocity and acceleration constraints.
`plan`(target_states[, options])	Execute trajectory planning.
`supports_move_type`(move_type)	Return whether the planner accepts a movement target type directly.
`validate_joint_trajectory`(trajectory, *, ...)	Validate exact joint samples without replacing their path.
`visualize_robot_collision_models`(control_part)	Visualize the robot collision models used by this planner.
`with_collision_world`(options, *, obstacle_poses)	Attach dynamic obstacle poses to backend planning options.
`with_motion_context`(options, *, start_qpos, ...)	Attach MotionGenerator runtime context to backend options.

Attributes:

`collision_world_info`	Return the planner's collision-world contract, if it has one.
`preserve_plan_samples`	Whether callers must retain this planner's returned sample points exactly.
`supported_move_types`	Movement target types accepted directly by this planner.
`supports_collision_world_updates`	Whether per-plan dynamic obstacle poses can update the collision world.
`supports_heterogeneous_waypoints`	Whether one plan may contain an ordered mixture of movement types.
`supports_joint_trajectory_validation`	Whether exact joint samples can be checked against bounds/collisions.
`uses_sparse_joint_waypoints`	Whether joint targets are sparse waypoints owned by the planner.

__init__(cfg)[source]#: Validate the timing grid and resolve the configured robot.

close()[source]#

Complete the planner lifecycle; CPU workers belong to the shared runtime.

Return type:: None

property collision_world_info: CollisionWorldInfo | None#: Return the planner’s collision-world contract, if it has one.

default_plan_options()[source]#

Return a valid time-sampled default when no count is requested.

Return type:: ToppraPlanOptions

is_satisfied_constraint(vels, accs, constraints, *, control_part=None)#

Check if the trajectory satisfies velocity and acceleration constraints.

This method checks whether the given velocities and accelerations satisfy the constraints defined in constraints. It allows for some tolerance to account for numerical errors in dense waypoint scenarios.

Parameters:

vels (Tensor) – Velocity tensor (…, DOF) where the last dimension is DOF
accs (Tensor) – Acceleration tensor (…, DOF) where the last dimension is DOF
constraints (dict) – Dictionary containing ‘velocity’ and ‘acceleration’ limits. None velocity resolves robot speed caps; None acceleration uses the planner default.
control_part (str | None) – Control group for resolving unspecified joint limits.

Returns:

True if all constraints are satisfied, False otherwise

Return type:

bool

Note

Allows 10% tolerance for velocity constraints
Allows 25% tolerance for acceleration constraints
Prints exceed information if constraints are violated
Assumes symmetric constraints (velocities and accelerations can be positive or negative)
Supports batch dimension computation, e.g. (B, N, DOF) or (N, DOF)

plan(target_states, options=ToppraPlanOptions(control_part=None, constraints={'velocity': None, 'acceleration': None}, sample_method=<TrajectorySampleMethod.TIME: 'time'>, sample_interval=0.01))[source]#

Execute trajectory planning.

Parameters:

target_states (list[PlanState]) – list of PlanState waypoints. Tensor fields carry a leading batch dim B: qpos is (B, DOF).
options (ToppraPlanOptions) – ToppraPlanOptions with constraints and sampling.

Return type:

PlanResult

Returns:

PlanResult containing the planned trajectory details. All tensor fields are env-batched with leading dim B: success (B,), positions/velocities/accelerations (B, N, DOF), dt (B, N), duration (B,).

preserve_plan_samples: bool = False#

Whether callers must retain this planner’s returned sample points exactly.

When True, MotionGenerator returns the planner’s trajectory without resampling, preserving planner-owned samples and derivatives (including collision-checked samples where applicable). When False (the default), the generator may normalize the trajectory to a requested waypoint count.

supported_move_types: frozenset[MoveType] = frozenset({MoveType.JOINT_MOVE})#

Movement target types accepted directly by this planner.

MotionGenerator uses this declaration to validate targets and determine whether Cartesian targets must first be converted into joint waypoints for a joint-only backend.

supports_collision_world_updates: bool = False#: Whether per-plan dynamic obstacle poses can update the collision world.

supports_heterogeneous_waypoints: bool = False#: Whether one plan may contain an ordered mixture of movement types.

supports_joint_trajectory_validation: bool = False#: Whether exact joint samples can be checked against bounds/collisions.

supports_move_type(move_type)#

Return whether the planner accepts a movement target type directly.

Parameters:: move_type (MoveType) – Movement target type to query.
Return type:: bool
Returns:: True when plan() accepts the target type without MotionGenerator preprocessing.

uses_sparse_joint_waypoints: bool = False#

Whether joint targets are sparse waypoints owned by the planner.

When True, MotionGenerator prepends the observed start_qpos before dispatching a joint target. This lets the planner own both path timing and derivative generation without making every caller materialize the current state as a waypoint. Cartesian preparation also keeps only requested goals unless is_linear explicitly requests a dense Cartesian path.

validate_joint_trajectory(trajectory, *, control_part, obstacle_poses=None)#

Validate exact joint samples without replacing their path.

Backends that implement this contract must evaluate every supplied sample against joint bounds, self-collision, and their configured world collision model. They return a boolean mask with shape (B, T).

Parameters:

trajectory (Tensor) – Simulator-order joint samples with shape (B, T, D).
control_part (str) – Robot control part whose ordered joints form D.
obstacle_poses (Mapping[str, Tensor] | None) – Optional current dynamic-obstacle world poses.

Return type:

Tensor

Returns:

Per-environment, per-sample validity mask.

Raises:

NotImplementedError – Always for the base planner.

visualize_robot_collision_models(control_part, env_id=0)#

Visualize the robot collision models used by this planner.

Planners that support collision avoidance should override this method with their backend-specific visualization.

Parameters:

control_part (str) – Robot control part whose collision models are visualized.
env_id (int) – Simulator environment instance to visualize.

Raises:

NotImplementedError – If the planner does not support collision avoidance.

Return type:

None

with_collision_world(options, *, obstacle_poses)#

Attach dynamic obstacle poses to backend planning options.

The base planner does not consume a collision world. Backends whose collision_world_info enables updates override this method.

Parameters:

options (PlanOptions) – Backend-specific options to enrich.
obstacle_poses (Mapping[str, Tensor]) – Batched world poses keyed by stable obstacle ID.

Return type:

PlanOptions

Returns:

Planning options unchanged for a backend without world updates.

with_motion_context(options, *, start_qpos, control_part)#

Attach MotionGenerator runtime context to backend options.

The base planner fills an absent control group when the options support it. Backends with additional contextual options override this method.

Parameters:

options (PlanOptions) – The backend’s planning options, already constructed (either by the caller or via default_plan_options()).
start_qpos (Tensor | None) – Optional starting joint configuration (B, DOF).
control_part (str | None) – Optional control-part name.

Return type:

PlanOptions

Returns:

The (possibly mutated) planning options carrying the context.

Trapezoidal Planner#

The trapezoidal planner applies either acceleration-limited trapezoidal timing or jerk-limited Double-S timing to batched, piecewise-linear joint paths. Use TrapezoidalPlanOptions.minimum_duration to slow a trajectory without changing its path or violating derivative limits. For densely interpolated straight paths, set stop_at_waypoints=False to remove redundant same-direction interior points while preserving real corners. backend="auto" selects Warp profile construction and sampling for CUDA float32 inputs and retains the Torch reference implementation for CPU or float64. The Warp path builds trapezoidal or Double-S phases in parallel per batch segment, then evaluates scalar samples and composes all joints. Both backends avoid a dense sample-by-segment lookup tensor: Torch uses batched searchsorted, while Warp performs one binary lookup per batch sample before parallel joint composition. Torch also gathers profile coefficients directly by selected phase, avoiding full sample-by-phase copies of position, velocity, acceleration, and jerk data. All-stationary batches use a hold fast path and skip constraint projection, profile construction, segment lookup, and backend dispatch. Through MotionGenerator, the planner consumes start_qpos plus sparse joint goals without generic pre-interpolation. Its native sample grid, velocities, accelerations, timing, and constraint report are preserved.

class embodichain.lab.sim.motion.planners.TrapezoidalPlanOptions[source]#

Configure trapezoidal or Double-S trajectory generation.

Parameters:

profile (Literal['trapezoidal', 'double_s']) – Scalar time profile used for every linear joint-path segment.
constraints (dict) – Scalar or per-joint velocity, acceleration, and jerk limits. None velocity resolves robot speed caps; None acceleration uses the planner default. Explicit speeds are capped by robot limits. Jerk is required by double_s.
sample_method (TrajectorySampleMethod) – Fixed output quantity or approximately fixed time step.
sample_interval (float | int) – Output count for QUANTITY or seconds for TIME.
minimum_duration (float | None) – Optional lower bound on each environment trajectory’s duration. Slower trajectories retain the same path and limits.
stop_at_waypoints (bool) – Whether every supplied waypoint is a rest point. When false, redundant points on straight, same-direction runs are removed. Blended reversals still retain their exact waypoint and rest state.
collinearity_tolerance (float) – Cosine tolerance relative to the first edge of each retained straight run during waypoint compression.
blend_tolerance (float) – Non-negative corner deviation used for quintic blends. Positive values require stop_at_waypoints=False.
backend (Literal['auto', 'torch', 'warp']) – Profile-construction and sampling backend. auto selects Warp for CUDA float32 trajectories and Torch otherwise.

Attributes:

control_part

Control group whose joint order matches the trajectory columns.

control_part: str | None = None#: Control group whose joint order matches the trajectory columns.

class embodichain.lab.sim.motion.planners.TrapezoidalPlannerCfg[source]#

Configure the batched joint-space time-profile planner.

Attributes:

`robot_uid`	UID of the robot to control.
`sim_instance_id`	ID of the SimulationManager that owns `robot_uid`.

robot_uid: str#: UID of the robot to control. Must correspond to a robot added to the simulation with this UID.

sim_instance_id: int#: ID of the SimulationManager that owns robot_uid. Defaults to 0.

class embodichain.lab.sim.motion.planners.TrapezoidalPlanner[source]#

Bases: BasePlanner

Plan batched linear joint paths with trapezoidal or Double-S timing.

Methods:

`__init__`(cfg)
`default_plan_options`()	Return backend-default planning options.
`is_satisfied_constraint`(vels, accs, ...[, ...])	Check if the trajectory satisfies velocity and acceleration constraints.
`plan`(target_states[, options])	Generate one batched joint trajectory through all target states.
`supports_move_type`(move_type)	Return whether the planner accepts a movement target type directly.
`validate_joint_trajectory`(trajectory, *, ...)	Validate exact joint samples without replacing their path.
`visualize_robot_collision_models`(control_part)	Visualize the robot collision models used by this planner.
`with_collision_world`(options, *, obstacle_poses)	Attach dynamic obstacle poses to backend planning options.
`with_motion_context`(options, *, start_qpos, ...)	Attach MotionGenerator runtime context to backend options.

Attributes:

`collision_world_info`	Return the planner's collision-world contract, if it has one.
`preserve_plan_samples`	Whether callers must retain this planner's returned sample points exactly.
`supported_move_types`	Movement target types accepted directly by this planner.
`supports_collision_world_updates`	Whether per-plan dynamic obstacle poses can update the collision world.
`supports_heterogeneous_waypoints`	Whether one plan may contain an ordered mixture of movement types.
`supports_joint_trajectory_validation`	Whether exact joint samples can be checked against bounds/collisions.
`uses_sparse_joint_waypoints`	Whether joint targets are sparse waypoints owned by the planner.

__init__(cfg)#

property collision_world_info: CollisionWorldInfo | None#: Return the planner’s collision-world contract, if it has one.

default_plan_options()[source]#

Return backend-default planning options.

Return type:: TrapezoidalPlanOptions

is_satisfied_constraint(vels, accs, constraints, *, control_part=None)#

Check if the trajectory satisfies velocity and acceleration constraints.

This method checks whether the given velocities and accelerations satisfy the constraints defined in constraints. It allows for some tolerance to account for numerical errors in dense waypoint scenarios.

Parameters:

vels (Tensor) – Velocity tensor (…, DOF) where the last dimension is DOF
accs (Tensor) – Acceleration tensor (…, DOF) where the last dimension is DOF
constraints (dict) – Dictionary containing ‘velocity’ and ‘acceleration’ limits. None velocity resolves robot speed caps; None acceleration uses the planner default.
control_part (str | None) – Control group for resolving unspecified joint limits.

Returns:

True if all constraints are satisfied, False otherwise

Return type:

bool

Note

Allows 10% tolerance for velocity constraints
Allows 25% tolerance for acceleration constraints
Prints exceed information if constraints are violated
Assumes symmetric constraints (velocities and accelerations can be positive or negative)
Supports batch dimension computation, e.g. (B, N, DOF) or (N, DOF)

plan(target_states, options=TrapezoidalPlanOptions(profile='trapezoidal', control_part=None, constraints={'velocity': None, 'acceleration': None, 'jerk': 2.0}, sample_method=<TrajectorySampleMethod.QUANTITY: 'quantity'>, sample_interval=100, minimum_duration=None, stop_at_waypoints=True, collinearity_tolerance=1e-05, blend_tolerance=0.0, backend='auto'))[source]#

Generate one batched joint trajectory through all target states.

Parameters:

target_states (list[PlanState]) – Joint waypoints with qpos shape (B, DOF).
options (TrapezoidalPlanOptions) – Time-profile, derivative limits, and sampling configuration.

Return type:

PlanResult

Returns:

Batched positions, velocities, accelerations, and explicit timing.

Raises:

ValueError – If fewer than two valid joint waypoints are supplied, or a joint with a zero velocity cap is requested to move.

preserve_plan_samples: bool = True#

Whether callers must retain this planner’s returned sample points exactly.

When True, MotionGenerator returns the planner’s trajectory without resampling, preserving planner-owned samples and derivatives (including collision-checked samples where applicable). When False (the default), the generator may normalize the trajectory to a requested waypoint count.

supported_move_types: frozenset[MoveType] = frozenset({MoveType.JOINT_MOVE})#

Movement target types accepted directly by this planner.

MotionGenerator uses this declaration to validate targets and determine whether Cartesian targets must first be converted into joint waypoints for a joint-only backend.

supports_collision_world_updates: bool = False#: Whether per-plan dynamic obstacle poses can update the collision world.

supports_heterogeneous_waypoints: bool = False#: Whether one plan may contain an ordered mixture of movement types.

supports_joint_trajectory_validation: bool = False#: Whether exact joint samples can be checked against bounds/collisions.

supports_move_type(move_type)#

Return whether the planner accepts a movement target type directly.

Parameters:: move_type (MoveType) – Movement target type to query.
Return type:: bool
Returns:: True when plan() accepts the target type without MotionGenerator preprocessing.

uses_sparse_joint_waypoints: bool = True#

Whether joint targets are sparse waypoints owned by the planner.

When True, MotionGenerator prepends the observed start_qpos before dispatching a joint target. This lets the planner own both path timing and derivative generation without making every caller materialize the current state as a waypoint. Cartesian preparation also keeps only requested goals unless is_linear explicitly requests a dense Cartesian path.

validate_joint_trajectory(trajectory, *, control_part, obstacle_poses=None)#

Validate exact joint samples without replacing their path.

Backends that implement this contract must evaluate every supplied sample against joint bounds, self-collision, and their configured world collision model. They return a boolean mask with shape (B, T).

Parameters:

trajectory (Tensor) – Simulator-order joint samples with shape (B, T, D).
control_part (str) – Robot control part whose ordered joints form D.
obstacle_poses (Mapping[str, Tensor] | None) – Optional current dynamic-obstacle world poses.

Return type:

Tensor

Returns:

Per-environment, per-sample validity mask.

Raises:

NotImplementedError – Always for the base planner.

visualize_robot_collision_models(control_part, env_id=0)#

Visualize the robot collision models used by this planner.

Planners that support collision avoidance should override this method with their backend-specific visualization.

Parameters:

control_part (str) – Robot control part whose collision models are visualized.
env_id (int) – Simulator environment instance to visualize.

Raises:

NotImplementedError – If the planner does not support collision avoidance.

Return type:

None

with_collision_world(options, *, obstacle_poses)#

Attach dynamic obstacle poses to backend planning options.

The base planner does not consume a collision world. Backends whose collision_world_info enables updates override this method.

Parameters:

options (PlanOptions) – Backend-specific options to enrich.
obstacle_poses (Mapping[str, Tensor]) – Batched world poses keyed by stable obstacle ID.

Return type:

PlanOptions

Returns:

Planning options unchanged for a backend without world updates.

with_motion_context(options, *, start_qpos, control_part)#

Attach MotionGenerator runtime context to backend options.

The base planner fills an absent control group when the options support it. Backends with additional contextual options override this method.

Parameters:

options (PlanOptions) – The backend’s planning options, already constructed (either by the caller or via default_plan_options()).
start_qpos (Tensor | None) – Optional starting joint configuration (B, DOF).
control_part (str | None) – Optional control-part name.

Return type:

PlanOptions

Returns:

The (possibly mutated) planning options carrying the context.

Example#

Select the Double-S profile when jerk continuity is required while retaining the same MotionGenerator entry point:

generator = MotionGenerator(
    MotionGenCfg(
        planner_cfg=TrapezoidalPlannerCfg(robot_uid=robot.uid),
    )
)
result = generator.generate(
    [PlanState.from_qpos(goal)],
    MotionGenOptions(
        start_qpos=start,
        plan_opts=TrapezoidalPlanOptions(
            profile="double_s",
            constraints={
                "velocity": 0.5,
                "acceleration": 1.0,
                "jerk": 3.0,
            },
            sample_interval=200,
            stop_at_waypoints=False,
            backend="auto",
        )
    ),
)

The complete batched simulation tutorial is scripts/tutorials/sim/planner/trapezoidal_planner.py. Pass --backend torch or --backend warp to compare implementations. For repeatable timing and memory measurements, run scripts/benchmark/motion_generation/trapezoidal_planner.py.

Utilities#

class embodichain.lab.sim.motion.planners.TrajectorySampleMethod[source]#

Bases: Enum

Enumeration for different trajectory sampling methods.

This enum defines various methods for sampling trajectories, providing meaningful names for different sampling strategies.

Attributes:

`DISTANCE`	Sample based on distance intervals.
`QUANTITY`	Sample based on a specified number of points.
`TIME`	Sample based on time intervals.

Methods:

from_str(value)

DISTANCE = 'distance'#: Sample based on distance intervals.

QUANTITY = 'quantity'#: Sample based on a specified number of points.

TIME = 'time'#: Sample based on time intervals.

classmethod from_str(value)[source]#

Return type:: TrajectorySampleMethod

class embodichain.lab.sim.motion.planners.MovePart[source]#

Bases: Enum

Enumeration for different robot parts to move.

Defines robot part selection for motion planning.

LEFT#

left arm or end-effector.

Type:: int

RIGHT#

right arm or end-effector.

Type:: int

BOTH#

both arms or end-effectors.

Type:: int

TORSO#

torso for humanoid robot.

Type:: int

ALL#

all joints of the robot (joint control only).

Type:: int

Attributes:

`ALL`
`BOTH`
`LEFT`
`RIGHT`
`TORSO`

ALL = 4#

BOTH = 2#

LEFT = 0#

RIGHT = 1#

TORSO = 3#

class embodichain.lab.sim.motion.planners.MoveType[source]#

Bases: Enum

Enumeration for different types of movements.

Defines movement types for robot planning.

TOOL#

Tool open or close.

Type:: int

EEF_MOVE#

Move end-effector to target pose (IK + trajectory).

Type:: int

JOINT_MOVE#

Move joints to target angles (trajectory planning).

Type:: int

SYNC#

Synchronized left/right arm movement (dual-arm robots).

Type:: int

PAUSE#

Pause for specified duration (see PlanState.pause_seconds).

Type:: int

Attributes:

`EEF_MOVE`
`JOINT_MOVE`
`PAUSE`
`SYNC`
`TOOL`

EEF_MOVE = 1#

JOINT_MOVE = 2#

PAUSE = 4#

SYNC = 3#

TOOL = 0#

class embodichain.lab.sim.motion.planners.PlanResult[source]#

Bases: object

Data class representing the result of a motion plan (env-batched).

A result that contains joint positions must also contain per-sample dt. Per-environment duration is derived from those intervals. Failed plans may omit all trajectory fields by leaving positions as None.

Methods:

`__init__`([success, xpos_list, positions, ...])
`is_all_success`()	Return True only when every env succeeded.

Attributes:

`accelerations`	Joint accelerations, shape `(B, N, DOF)`.
`constraint_report`	Optional derivative diagnostics, invalidated when the trajectory changes.
`dt`	Per-env time deltas, shape `(B, N)`.
`duration`	Return per-environment duration derived from `dt`.
`positions`	Joint positions, shape `(B, N, DOF)`.
`success`	Per-env success, shape `(B,)` bool tensor (or scalar bool).
`velocities`	Joint velocities, shape `(B, N, DOF)`.
`xpos_list`	End-effector poses, shape `(B, N, 4, 4)`.

__init__(success=False, xpos_list=None, positions=None, velocities=None, accelerations=None, dt=None, constraint_report=None)#

accelerations: Tensor | None = None#: Joint accelerations, shape (B, N, DOF).

constraint_report: dict[str, Tensor] | None = None#

Optional derivative diagnostics, invalidated when the trajectory changes.

Peak fields describe sampled derivatives (or analytic phase jerk). Blended joint plans also provide conservative *_upper_bound_per_joint fields; their within_limits is based on these continuous bounds. Trapezoidal acceleration jumps are not jerk constrained and have an infinite jerk bound. TrapezoidalPlanner reports *_limit summaries in joint units as the maximum configured limit over joints; use per-joint utilization fields to assess heterogeneous limits.

dt: Tensor | None = None#: Per-env time deltas, shape (B, N).

property duration: Tensor | None#: Return per-environment duration derived from dt.

is_all_success()[source]#

Return True only when every env succeeded.

Return type:: bool

positions: Tensor | None = None#: Joint positions, shape (B, N, DOF).

success: bool | Tensor = False#: Per-env success, shape (B,) bool tensor (or scalar bool).

velocities: Tensor | None = None#: Joint velocities, shape (B, N, DOF).

xpos_list: Tensor | None = None#: End-effector poses, shape (B, N, 4, 4).

class embodichain.lab.sim.motion.planners.PlanState[source]#

Bases: object

Data class representing the state for a motion plan (env-batched).

Tensor fields carry a leading batch dim B: qpos:(B, DOF), xpos:(B, 4, 4). Enum/scalar fields are shared across B (vectorized envs share the same task skeleton).

Methods:

`__init__`([move_type, move_part, xpos, qpos, ...])
`from_qpos`(qpos, *[, move_type, move_part])	Create a PlanState from batched joint positions `(B, DOF)`.
`from_xpos`(xpos, *[, move_type, move_part])	Create a PlanState from batched end-effector poses `(B, 4, 4)`.
`single`(*[, qpos, xpos, move_type, move_part])	B=1 convenience constructor: unsqueezes a single-env qpos/xpos.

Attributes:

`is_open`	For `MoveType.TOOL`, indicates whether to open (`True`) or close (`False`) the tool.
`is_world_coordinate`	`True` if the target pose is in world coordinates, `False` if relative to the current pose.
`move_part`	Robot part that should move.
`move_type`	Type of movement used by the plan.
`pause_seconds`	Duration of a pause when `move_type` is `MoveType.PAUSE`.
`qacc`	Target joint accelerations for `MoveType.JOINT_MOVE` with shape `(B, DOF)`.
`qpos`	Target joint angles for `MoveType.JOINT_MOVE` with shape `(B, DOF)`.
`qvel`	Target joint velocities for `MoveType.JOINT_MOVE` with shape `(B, DOF)`.
`xpos`	Target TCP pose (Bx4x4) for `MoveType.EEF_MOVE`.

__init__(move_type=MoveType.JOINT_MOVE, move_part=MovePart.LEFT, xpos=None, qpos=None, qvel=None, qacc=None, is_open=True, is_world_coordinate=True, pause_seconds=0.0)#

classmethod from_qpos(qpos, *, move_type=MoveType.JOINT_MOVE, move_part=MovePart.LEFT, **kwargs)[source]#

Create a PlanState from batched joint positions (B, DOF).

Return type:: PlanState

classmethod from_xpos(xpos, *, move_type=MoveType.EEF_MOVE, move_part=MovePart.LEFT, **kwargs)[source]#

Create a PlanState from batched end-effector poses (B, 4, 4).

Return type:: PlanState

is_open: bool = True#: For MoveType.TOOL, indicates whether to open (True) or close (False) the tool.

is_world_coordinate: bool = True#: True if the target pose is in world coordinates, False if relative to the current pose.

move_part: MovePart = 0#: Robot part that should move.

move_type: MoveType = 2#: Type of movement used by the plan.

pause_seconds: float = 0.0#: Duration of a pause when move_type is MoveType.PAUSE.

qacc: Tensor | None = None#: Target joint accelerations for MoveType.JOINT_MOVE with shape (B, DOF).

qpos: Tensor | None = None#: Target joint angles for MoveType.JOINT_MOVE with shape (B, DOF).

qvel: Tensor | None = None#: Target joint velocities for MoveType.JOINT_MOVE with shape (B, DOF).

classmethod single(*, qpos=None, xpos=None, move_type=MoveType.JOINT_MOVE, move_part=MovePart.LEFT, **kwargs)[source]#

B=1 convenience constructor: unsqueezes a single-env qpos/xpos.

Already-batched tensors (2D qpos / 3D xpos) pass through unchanged (idempotent).

Return type:: PlanState

xpos: Tensor | None = None#: Target TCP pose (Bx4x4) for MoveType.EEF_MOVE.

Joint path retiming#

TOPPRA uses a single Warp calculation chain on CPU and CUDA. Its configuration accepts grid_size and the shared robot/manager fields; the removed backend, max_workers and mp_context fields are no longer accepted. CPU inference uses shared threads without creating planner-owned processes.

Re-parameterize existing joint paths with the same differentiable timing solve. The helper preserves float32/float64 input precision, supports valid per-row waypoint prefixes, and returns explicit timing with isolated failures.

embodichain.lab.sim.motion.planners.toppra_planner.retime_joint_paths

Re-parameterize already-planned joint paths under dynamic limits.

embodichain.lab.sim.motion.planners.toppra_planner.retime_joint_paths(positions, *, constraints, path_lengths=None, sample_method=TrajectorySampleMethod.QUANTITY, sample_interval=None, grid_size=100, device=None)[source]#

Re-parameterize already-planned joint paths under dynamic limits.

This is the time parameterization of ToppraPlanner, exposed for planners that produce geometry without executable timing. A closed-loop policy rollout, for example, emits one joint sample per control step on a nominal dt that no dynamic limit informed; feeding those samples here replaces that timing with a solved one that respects the limits.

Warp handles both CPU and CUDA inputs. Sufficiently large CPU inference batches share a bounded thread pool; small requests and gradient recording run inline. Interval constraints enforce limits between grid points. Floating outputs retain float32/float64 input precision; other input dtypes are promoted to float32 before solving.

The path geometry is resampled along the fitted spline, so poses derived from the input samples must be recomputed from the returned positions.

Parameters:

positions (Tensor) – Planned joint paths of shape (B, N, DOF).
constraints (Mapping[str, float | Sequence[float] | Tensor]) – velocity and acceleration limits, each a scalar, one value per joint, or signed (DOF, 2) bounds, matching ToppraPlanOptions.
path_lengths (Tensor | None) – Optional valid prefix length for each batch row. This excludes rectangular tail padding from spline fitting while preserving explicit repeated endpoints inside each prefix.
sample_method (TrajectorySampleMethod) – Fixed output quantity or approximately fixed time step.
sample_interval (float | int | None) – Output count for QUANTITY or seconds for TIME. None keeps the input sample count and is only meaningful for QUANTITY, which leaves array shapes unchanged while the timing behind them changes.
grid_size (int) – Minimum parameterization grid size, as for ToppraPlannerCfg.
device (device | None) – Device for the assembled tensors; defaults to the input’s.

Return type:

PlanResult

Returns:

PlanResult whose success is per-env and false where the path could not be parameterized.

Raises:

ValueError – If positions is not a (B, N, DOF) tensor, path_lengths is malformed, the constraints omit velocity or acceleration, or TIME sampling is requested without an explicit interval in seconds.

embodichain.lab.sim.motion.planners

Contents

embodichain.lab.sim.motion.planners#

Base Planner#

Toppra Planner#

Trapezoidal Planner#

Example#

Utilities#

Joint path retiming#