embodichain.lab.sim.planners

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
MotionGenCfg	MotionGenCfg(planner_cfg: embodichain.lab.sim.planners.base_planner.BasePlannerCfg = <factory>)
MotionGenerator	Unified motion generator for robot trajectory planning.
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.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.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)
is_satisfied_constraint(vels, accs, constraints)	Check if the trajectory satisfies velocity and acceleration constraints.
plan(target_states[, options])	Execute trajectory planning.

__init__(cfg)

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)

abstractmethod 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: torch.Tensor (B,), total trajectory duration per env

Return type:

PlanResult

Toppra Planner

class embodichain.lab.sim.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.planners.ToppraPlanner

Bases: BasePlanner

Methods:

__init__(cfg)	Initialize the TOPPRA trajectory planner.
is_satisfied_constraint(vels, accs, constraints)	Check if the trajectory satisfies velocity and acceleration constraints.
plan(target_states[, options])	Execute trajectory planning.

__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

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

Motion Generator

class embodichain.lab.sim.planners.MotionGenCfg

MotionGenCfg(planner_cfg: embodichain.lab.sim.planners.base_planner.BasePlannerCfg = <factory>)

Attributes:

planner_cfg

Configuration for the underlying planner.

planner_cfg: BasePlannerCfg

Configuration for the underlying planner. Must include ‘planner_type’ attribute to specify which planner to use, and any additional parameters required by that planner.

class embodichain.lab.sim.planners.MotionGenerator

Bases: object

Unified motion generator for robot trajectory planning.

This class provides a unified interface for trajectory planning with and without collision checking.

Parameters:

cfg (MotionGenCfg) – Configuration object for motion generation, must include ‘planner_cfg’ attribute

Methods:

__init__(cfg)
estimate_trajectory_sample_count([...])	Estimate the number of trajectory sampling points required.
generate(target_states[, options])	Generate motion with given options.
interpolate_trajectory([control_part, ...])	Interpolate trajectory based on provided waypoints.
plot_trajectory(positions[, vels, accs])	Plot trajectory data.
register_planner_type(name, planner_class, ...)	Register a new planner type.

__init__(cfg)

estimate_trajectory_sample_count(xpos_list=None, qpos_list=None, step_size=0.01, angle_step=0.03490658503988659, control_part=None)

Estimate the number of trajectory sampling points required.

This function estimates the total number of sampling points needed to generate a trajectory based on the given waypoints and sampling parameters. Supports parallel computation for batched input trajectories.

Parameters:

• xpos_list (Tensor | list[Tensor] | None) – Tensor of 4x4 transformation matrices, shape [B, N, 4, 4] or [N, 4, 4]

• qpos_list (Tensor | list[Tensor] | None) – Tensor of joint positions, shape [B, N, D] or [N, D] (optional)

• step_size (float | Tensor) – Maximum allowed distance between points (meters). Float or Tensor [B]

• angle_step (float | Tensor) – Maximum allowed angular difference between points (radians). Float or Tensor [B]

Returns:

Estimated number of sampling points per trajectory, shape [B]

(or scalar tensor if single trajectory)

Return type:

torch.Tensor

generate(target_states, options=MotionGenOptions(start_qpos=None, control_part=None, plan_opts=None, is_interpolate=False, interpolate_nums=10, is_linear=False, interpolate_position_step=0.002, interpolate_angle_step=0.03490658503988659))

Generate motion with given options.

This method generates a smooth trajectory using the selected planner that satisfies constraints and perform pre-interpolation if specified in the options.

Parameters:

• target_states (List[PlanState]) – List[PlanState].

• options (MotionGenOptions) – MotionGenOptions.

Return type:

PlanResult

Returns:

PlanResult containing the planned trajectory details.

interpolate_trajectory(control_part=None, xpos_list=None, qpos_list=None, options=MotionGenOptions(start_qpos=None, control_part=None, plan_opts=None, is_interpolate=False, interpolate_nums=10, is_linear=False, interpolate_position_step=0.002, interpolate_angle_step=0.03490658503988659))

Interpolate trajectory based on provided waypoints.

This method performs interpolation on the provided waypoints to generate a smoother trajectory. It supports both Cartesian (end-effector) and joint space interpolation based on the control part and options specified.

Parameters:

• control_part (str | None) – Name of the robot part to control, e.g. ‘left_arm’. Must correspond to a valid control part defined in the robot’s configuration.

• xpos_list (Tensor | None) – End-effector poses, shape (B, N, 4, 4) or (N, 4, 4). Required if control_part is an end-effector control part.

• qpos_list (Tensor | None) – Joint positions, shape (B, N, DOF) or (N, DOF). Required if control_part is a joint control part.

• options (MotionGenOptions) – MotionGenOptions containing interpolation settings such as step size and whether to use linear interpolation.

Returns:

• interpolate_qpos_list: Interpolated joint positions, shape (B, M, DOF).

• feasible_pose_targets: Corresponding end-effector poses, shape (B, M, 4, 4), or None if not applicable.

Return type:

Tuple containing

plot_trajectory(positions, vels=None, accs=None)

Plot trajectory data.

This method visualizes the trajectory by plotting position, velocity, and acceleration curves for each joint over time. It also displays the constraint limits for reference. Supports plotting batched trajectories.

Parameters:

• positions (Tensor) – Position tensor (N, DOF) or (B, N, DOF)

• vels (Tensor | None) – Velocity tensor (N, DOF) or (B, N, DOF), optional

• accs (Tensor | None) – Acceleration tensor (N, DOF) or (B, N, DOF), optional

Return type:

None

Note

• Creates a multi-subplot figure (position, and optional velocity/acceleration)

• Shows constraint limits as dashed lines

• If input is (B, N, DOF), plots elements separately per batch sequence.

• Requires matplotlib to be installed

classmethod register_planner_type(name, planner_class, planner_cfg_class)

Register a new planner type.

Return type:

None

Utilities

class embodichain.lab.sim.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.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.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.planners.PlanResult

Bases: object

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

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	Per-env total duration, shape (B,).
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, duration=0.0)

accelerations: Tensor | None = None

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

dt: Tensor | None = None

Per-env time deltas, shape (B, N).

duration: float | Tensor = 0.0

Per-env total duration, shape (B,).

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