embodichain.lab.sim.motion.planners

Motion planning stack.

BasePlanner trajectory planners (TOPPRA, neural, 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>)
BasePlanner	Base class for trajectory planners.
ToppraPlannerCfg	ToppraPlannerCfg(robot_uid: 'str' = <factory>, planner_type: 'str' = <factory>, max_workers: 'int | None' = <factory>, mp_context: 'str | None' = <factory>)
ToppraPlanner	Time-optimal joint-space planner backed by TOPPRA.
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

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

Attributes:

robot_uid

UID of the robot to control.

robot_uid: str

UID of the robot to control. Must correspond to a robot added to the simulation with this UID.

class embodichain.lab.sim.motion.planners.BasePlanner

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, constraints)	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.
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_joint_trajectory_validation	Whether exact joint samples can be checked against bounds/collisions.

__init__(cfg)

property collision_world_info: CollisionWorldInfo | None

Return the planner’s collision-world contract, if it has one.

default_plan_options()

Return backend-default planning options.

Return type:

PlanOptions

is_satisfied_constraint(vels, accs, constraints)

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

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())

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 collision-checked samples. 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_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.

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.

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 has no context fields and therefore returns options unchanged. Backends with 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

ToppraPlannerCfg(robot_uid: ‘str’ = <factory>, planner_type: ‘str’ = <factory>, max_workers: ‘int | None’ = <factory>, mp_context: ‘str | None’ = <factory>)

Attributes:

max_workers	Worker process count for the batched fan-out.
mp_context	Multiprocessing start method for the batched fan-out.
robot_uid	UID of the robot to control.

max_workers: int | None

Worker process count for the batched fan-out. None => min(cpu_count()//2, B).

mp_context: str | None

Multiprocessing start method for the batched fan-out.

None (default) auto-selects based on the simulation device: 'fork' on CPU and 'spawn' on GPU. 'fork' is faster — workers inherit the parent’s already-loaded modules, so pool startup is near-instant — and is safe here because the TOPPRA worker (_toppra_solve_one_env()) is pure numpy/scipy and never touches the parent’s Vulkan/Warp/CUDA context or render threads; _worker_init clears the inherited atexit registry and installs prctl(PR_SET_PDEATHSIG) so workers are reaped when the parent dies (incl. the os._exit path). 'spawn' is the safer choice when the parent has initialized CUDA physics (sim_device='cuda') — fork-after-CUDA-init is the officially unsupported case — or if fork deadlocks are observed, at the cost of re-importing modules per worker.

robot_uid: str

UID of the robot to control. Must correspond to a robot added to the simulation with this UID.

class embodichain.lab.sim.motion.planners.ToppraPlanner

Bases: BasePlanner

Time-optimal joint-space planner backed by TOPPRA.

Methods:

__init__(cfg)	Initialize the TOPPRA trajectory planner.
close()	Release TOPPRA worker processes owned by this planner.
default_plan_options()	Return backend-default planning options.
is_satisfied_constraint(vels, accs, constraints)	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.
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_joint_trajectory_validation	Whether exact joint samples can be checked against bounds/collisions.

__init__(cfg)

Initialize the TOPPRA trajectory planner.

References

• TOPPRA: Time-Optimal Path Parameterization for Robotic Systems (hungpham2511/toppra)

Parameters:

cfg (ToppraPlannerCfg) – Configuration object containing ToppraPlanner settings

close()

Release TOPPRA worker processes owned by this planner.

Return type:

None

property collision_world_info: CollisionWorldInfo | None

Return the planner’s collision-world contract, if it has one.

default_plan_options()

Return backend-default planning options.

Return type:

ToppraPlanOptions

is_satisfied_constraint(vels, accs, constraints)

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

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(constraints={'velocity': 0.2, 'acceleration': 0.5}, sample_method=<TrajectorySampleMethod.QUANTITY: 'quantity'>, sample_interval=0.01))

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 collision-checked samples. 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_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.

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.

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 has no context fields and therefore returns options unchanged. Backends with 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.

Utilities

class embodichain.lab.sim.motion.planners.TrajectorySampleMethod

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)

Return type:

TrajectorySampleMethod

class embodichain.lab.sim.motion.planners.MovePart

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

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

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).
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)

accelerations: Tensor | None = None

Joint accelerations, shape (B, N, DOF).

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()

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

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)

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)

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)

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.