embodichain.lab.sim.planners

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

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.

PlanState

Data class representing the state for a motion plan.

Base Planner#

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

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

is_satisfied_constraint(vels, accs, constraints)

Check if the trajectory satisfies velocity and acceleration constraints.

plan(target_states[, options])

Execute trajectory planning.
__init__(cfg)[source]#
is_satisfied_constraint(vels, accs, constraints)[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

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 dictionaries containing target states

Returns:

An object containing:

  • success: bool, whether planning succeeded

  • positions: torch.Tensor (N, DOF), joint positions along trajectory

  • velocities: torch.Tensor (N, DOF), joint velocities along trajectory

  • accelerations: torch.Tensor (N, DOF), joint accelerations along trajectory

  • times: torch.Tensor (N,), time stamps for each point

  • duration: float, total trajectory duration

  • error_msg: Optional error message if planning failed

Return type:

PlanResult

Toppra Planner#

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

ToppraPlannerCfg(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.ToppraPlanner[source]#

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)[source]#

Initialize the TOPPRA trajectory planner.

References

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))[source]#

Execute trajectory planning.

Parameters:

  • target_states (list[PlanState]) – List of dictionaries containing target states

  • cfg – ToppraPlanOptions

Return type:

PlanResult

Returns:

PlanResult containing the planned trajectory details.

Motion Generator#

class embodichain.lab.sim.planners.MotionGenCfg[source]#

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[source]#

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)[source]#
estimate_trajectory_sample_count(xpos_list=None, qpos_list=None, step_size=0.01, angle_step=0.03490658503988659, control_part=None)[source]#

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 (Union[Tensor, list[Tensor], None]) – Tensor of 4x4 transformation matrices, shape [B, N, 4, 4] or [N, 4, 4]

  • qpos_list (Union[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))[source]#

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))[source]#
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 (Optional[str]) – 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 (Optional[Tensor]) – List of end-effector poses (torch.Tensor of shape [N, 4, 4]) to interpolate through. Required if control_part is an end-effector control part.

  • qpos_list (Optional[Tensor]) – List of joint positions (torch.Tensor of shape [N, DOF]) to interpolate through. 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: Tensor of interpolated joint positions along the trajectory, shape [M, DOF]

  • feasible_pose_targets: Tensor of corresponding end-effector poses for the interpolated joint positions, shape [M, 4, 4]. This is useful for verifying the interpolation results and can be None if not applicable.

Return type:

Tuple containing

plot_trajectory(positions, vels=None, accs=None)[source]#

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 (Optional[Tensor]) – Velocity tensor (N, DOF) or (B, N, DOF), optional

  • accs (Optional[Tensor]) – 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)[source]#

Register a new planner type.

Return type:

None

Utilities#

class embodichain.lab.sim.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.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.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.planners.PlanResult[source]#

Bases: object

Data class representing the result of a motion plan.

Methods:

__init__([success, xpos_list, positions, ...])

Attributes:

accelerations

Joint accelerations along trajectory with shape (N, DOF).

dt

Time duration between each point with shape (N,).

duration

Total trajectory duration in seconds.

positions

Joint positions along trajectory with shape (N, DOF).

success

Whether planning succeeded.

velocities

Joint velocities along trajectory with shape (N, DOF).

xpos_list

End-effector poses along trajectory with shape (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 along trajectory with shape (N, DOF).

dt: Tensor | None = None#

Time duration between each point with shape (N,).

duration: float | Tensor = 0.0#

Total trajectory duration in seconds.

positions: Tensor | None = None#

Joint positions along trajectory with shape (N, DOF).

success: bool | Tensor = False#

Whether planning succeeded.

velocities: Tensor | None = None#

Joint velocities along trajectory with shape (N, DOF).

xpos_list: Tensor | None = None#

End-effector poses along trajectory with shape (N, 4, 4).

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

Bases: object

Data class representing the state for a motion plan.

Methods:

__init__([move_type, move_part, xpos, qpos, ...])

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

qpos

Target joint angles for MoveType.JOINT_MOVE with shape (DOF,).

qvel

Target joint velocities for MoveType.JOINT_MOVE with shape (DOF,).

xpos

Target TCP pose (4x4 matrix) 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)#
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 (DOF,).

qpos: Tensor | None = None#

Target joint angles for MoveType.JOINT_MOVE with shape (DOF,).

qvel: Tensor | None = None#

Target joint velocities for MoveType.JOINT_MOVE with shape (DOF,).

xpos: Tensor | None = None#

Target TCP pose (4x4 matrix) for MoveType.EEF_MOVE.

Contents