embodichain.lab.sim.utility

Helper utilities for simulation state conversion, mesh/geometry handling, configuration transforms, keyboard interaction, and action/solver adaptation.

Overview

This package contains helper utilities for simulation state conversion, mesh/geometry handling, configuration transforms, keyboard interaction, and action/solver adaptation.

Submodules

action_utils	Simulation trajectory adaptation and compatibility exports.
atom_action_utils
cfg_utils
import_utils
io_utils
keyboard_utils
sim_utils
mesh_utils
solver_utils
tensor

Action Utilities

Simulation trajectory adaptation and compatibility exports.

Pure trajectory computations are owned by embodichain.compute.trajectory.

Functions:

compute_pose_offset_related_to_first(full_pose)	Compute pose offset relative to the first pose.
get_trajectory_object_offset_qpos(...[, device])	warp trajectory according to object pose offset
interpolate_with_distance(trajectory, interp_num)	Interpolate batched keyframes while preserving every keyframe boundary.
interpolate_with_nums(trajectory, interp_nums)	Interpolate each segment with its requested number of intervals.
resample_with_distance(trajectory, interp_num)	Resample a batched path at uniform cumulative-distance positions.
sort_and_padding_key_frame(trajectory, ...)	Sort keyframes and pad missing trajectory endpoints.
warp_trajectory_qpos(trajectory, ...[, device])	Apply interpolated keyframe offsets to batches of a joint trajectory.

embodichain.lab.sim.utility.action_utils.compute_pose_offset_related_to_first(full_pose)

Compute pose offset relative to the first pose.

Parameters:

full_pose (torch.Tensor) – The full pose tensor of shape (N, 4, 4).

Returns:

The pose offset tensor of shape (N, 4, 4).

Return type:

torch.Tensor

embodichain.lab.sim.utility.action_utils.get_trajectory_object_offset_qpos(trajectory, key_indices, key_obj_indices, obj_offset, solver, base_xpos, device=device(type='cuda'))

warp trajectory according to object pose offset

Parameters:

• trajectory (torch.Tensor) – raw trajectory. [n_waypoint, dof] of float, joint positions.

• key_indices (torch.Tensor) – key frame waypoint indices. [n_keyframe,] of int.

• key_obj_indices (torch.Tensor) – key frame belong to which object index. [n_keyframe,] of int.

• obj_offset (torch.Tensor) – each object pose offset. [obj_num, n_batch, 4, 4] of float.

• solver (BaseSolver) – robot kinematic solver.

• base_xpos (torch.Tensor) – solver root link pose in world coordinate. [4, 4] of float.

• device (str, optional) – torch tensor device. Defaults to “cuda”.

Returns:

warped trajectory. [n_batch, n_waypoint, dof] of float.

Return type:

torch.Tensor

embodichain.lab.sim.utility.action_utils.interpolate_with_distance(trajectory, interp_num, device=device(type='cuda'))

Interpolate batched keyframes while preserving every keyframe boundary.

Each input point is treated as a required keyframe. The output is generated segment by segment: every segment receives at least one interval, and any remaining intervals are distributed by Euclidean segment length for each batch independently. Segment endpoints are copied directly from the input, so intermediate keyframes occur as exact emitted samples.

Attention

interp_num must be at least the number of input keyframes. Use resample_with_distance() when input points are optional dense path samples that may be downsampled.

Parameters:

• trajectory (Tensor) – Keyframe tensor with shape (B, N, M).

• interp_num (int) – Target number of samples T.

• device (device | str) – Device on which to perform interpolation.

Return type:

Tensor

Returns:

Interpolated trajectories with shape (B, T, M).

Raises:

ValueError – If trajectory is not three-dimensional, contains no keyframes for a non-empty output, or interp_num cannot hold all keyframes.

embodichain.lab.sim.utility.action_utils.interpolate_with_nums(trajectory, interp_nums, device=device(type='cuda'))

Interpolate each segment with its requested number of intervals.

The first keyframe is emitted once. Each positive count appends that many evenly spaced samples ending at the next keyframe. A zero count appends the next keyframe directly, so every original boundary is retained.

Parameters:

• trajectory (Tensor) – Keyframe tensor with shape (B, N, M).

• interp_nums (Tensor | Sequence[int]) – Non-negative interval counts with shape (N - 1,).

• device (device | str) – Device on which to perform interpolation.

Return type:

Tensor

Returns:

A tensor containing the interpolated trajectories. For non-empty input, its sample count is 1 + sum(max(count, 1) for count in interp_nums).

Raises:

ValueError – If the count shape is invalid or a count is negative.

embodichain.lab.sim.utility.action_utils.resample_with_distance(trajectory, interp_num, device=device(type='cuda'))

Resample a batched path at uniform cumulative-distance positions.

Unlike interpolate_with_distance(), interior input samples are not required output points, so this function supports both upsampling and downsampling. It is intended for dense planner paths rather than required waypoint sequences.

Parameters:

• trajectory (Tensor) – Path tensor with shape (B, N, M).

• interp_num (int) – Target number of samples T.

• device (device | str) – Device on which to perform interpolation.

Return type:

Tensor

Returns:

Resampled trajectories with shape (B, T, M).

Raises:

ValueError – If trajectory is not three-dimensional, contains no points for a non-empty output, or interp_num is negative.

embodichain.lab.sim.utility.action_utils.sort_and_padding_key_frame(trajectory, key_indices, key_frames_batch)

Sort keyframes and pad missing trajectory endpoints.

Parameters:

• trajectory (np.ndarray) – Original trajectory, shaped (N, DOF).

• key_indices (np.ndarray) – Non-empty array of keyframe waypoint indices.

• key_frames_batch (np.ndarray) – Target keyframes, shaped (B, K, DOF).

Returns:

padded and sorted key frame indices. [n_keyframe_new,] of int. key_frames_ascending (np.ndarray): padded and sorted batch key frames. [n_batch, n_keyframe_new, dof] of float.

Return type:

key_indices_ascending (np.ndarray)

embodichain.lab.sim.utility.action_utils.warp_trajectory_qpos(trajectory, key_indices, key_frames_batch, device='cuda')

Apply interpolated keyframe offsets to batches of a joint trajectory.

Parameters:

• trajectory (torch.Tensor) – raw trajectory. [n_waypoint, dof] of float.

• key_indices (torch.Tensor) – key frame waypoint indices. [n_keyframe,] of int.

• key_frames_batch (torch.Tensor) – batch key frames. [n_batch, n_keyframe, dof] of float.

• device (str, optional) – torch tensor device. Defaults to “cuda”.

Returns:

warped trajectory. [n_batch, n_waypoint, dof] of float.

Return type:

torch.Tensor

Atomic Action Utilities

Functions:

draw_axis(env, pose)	Draw an axis marker in the simulation for debugging/visualization.
extract_drive_calls(code_str)	Extract all drive() function calls from a code string.
finalize_actions(select_qpos_traj, ...)	Format trajectory data into action format.
find_nearest_valid_pose(env, select_arm, pose)	Find the nearest valid pose using reachability validation.
get_arm_states(env, robot_name)	Get the current state of the specified robot arm.
get_qpos(env, is_left, select_arm, pose, ...)	Solve inverse kinematics to get joint positions for a given pose.
plan_gripper_trajectory(env, is_left, ...)	Plan a gripper trajectory (opening or closing) and append to trajectory lists.
plan_trajectory(env, select_arm, qpos_list, ...)	Plan a trajectory between joint positions and append to trajectory lists.

embodichain.lab.sim.utility.atom_action_utils.draw_axis(env, pose)

Draw an axis marker in the simulation for debugging/visualization.

Parameters:

• env – The simulation environment.

• pose – The pose (4x4 matrix) where to draw the axis.

embodichain.lab.sim.utility.atom_action_utils.extract_drive_calls(code_str)

Extract all drive() function calls from a code string.

Parameters:

code_str (str) – Python code string to parse.

Return type:

List[str]

Returns:

List of code blocks containing drive() calls.

embodichain.lab.sim.utility.atom_action_utils.finalize_actions(select_qpos_traj, ee_state_list_select)

Format trajectory data into action format.

Parameters:

• select_qpos_traj – List of joint positions.

• ee_state_list_select – List of gripper states.

Returns:

Formatted actions array with joint positions and gripper states.

Return type:

np.ndarray

embodichain.lab.sim.utility.atom_action_utils.find_nearest_valid_pose(env, select_arm, pose, xpos_resolution=0.1)

Find the nearest valid pose using reachability validation.

Parameters:

• env – The simulation environment.

• select_arm – Arm identifier (“left_arm” or “right_arm”).

• pose – Target pose (4x4 matrix).

• xpos_resolution – Resolution for reachability checking.

Returns:

The nearest valid pose (4x4 matrix).

Return type:

torch.Tensor

embodichain.lab.sim.utility.atom_action_utils.get_arm_states(env, robot_name)

Get the current state of the specified robot arm.

Parameters:

• env – The simulation environment.

• robot_name – Name of the robot arm (should contain “left” or “right”).

Returns:

• is_left: bool, whether this is the left arm

• select_arm: str, arm identifier (“left_arm” or “right_arm”)

• current_qpos: Current joint positions

• current_pose: Current end-effector pose (4x4 matrix)

• current_gripper_state: Current gripper state

Return type:

Tuple of (is_left, select_arm, current_qpos, current_pose, current_gripper_state)

embodichain.lab.sim.utility.atom_action_utils.get_qpos(env, is_left, select_arm, pose, qpos_seed, force_valid=False, name='')

Solve inverse kinematics to get joint positions for a given pose.

Parameters:

• env – The simulation environment.

• is_left – bool, whether this is the left arm.

• select_arm – Arm identifier (“left_arm” or “right_arm”).

• pose – Target end-effector pose (4x4 matrix).

• qpos_seed – Seed joint positions for IK solving.

• force_valid – If True, use nearest valid pose if IK fails.

• name – Name for logging purposes.

Returns:

Joint positions (qpos) corresponding to the target pose.

embodichain.lab.sim.utility.atom_action_utils.plan_gripper_trajectory(env, is_left, sample_num, execute_open, select_arm_current_qpos, select_qpos_traj, ee_state_list_select)

Plan a gripper trajectory (opening or closing) and append to trajectory lists.

Parameters:

• env – The simulation environment.

• is_left – bool, whether this is the left arm.

• sample_num – Number of samples for gripper motion.

• execute_open – bool, True for opening, False for closing.

• select_arm_current_qpos – Current joint positions.

• select_qpos_traj – List to append joint positions to (modified in-place).

• ee_state_list_select – List to append gripper states to (modified in-place).

embodichain.lab.sim.utility.atom_action_utils.plan_trajectory(env, select_arm, qpos_list, sample_num, select_arm_current_gripper_state, select_qpos_traj, ee_state_list_select)

Plan a trajectory between joint positions and append to trajectory lists.

Parameters:

• env – The simulation environment.

• select_arm – Arm identifier (“left_arm” or “right_arm”).

• qpos_list – List of joint positions to plan between.

• sample_num – Number of samples for trajectory interpolation.

• select_arm_current_gripper_state – Current gripper state.

• select_qpos_traj – List to append planned joint positions to (modified in-place).

• ee_state_list_select – List to append gripper states to (modified in-place).

Configuration Utilities

Functions:

merge_robot_cfg(base_cfg, override_cfg_dict)	Merge current robot configuration with overriding values from a dictionary.
merge_solver_cfg(default, provided)	Merge provided solver configuration into the default solver config.

embodichain.lab.sim.utility.cfg_utils.merge_robot_cfg(base_cfg, override_cfg_dict)

Merge current robot configuration with overriding values from a dictionary.

Parameters:

• base_cfg (RobotCfg) – The base robot configuration.

• override_cfg_dict (dict[str, any]) – Dictionary of overriding configuration values.

Returns:

The merged robot configuration.

Return type:

RobotCfg

embodichain.lab.sim.utility.cfg_utils.merge_solver_cfg(default, provided)

Merge provided solver configuration into the default solver config.

Rules: - For each arm key in provided, if the key exists in default, update fields provided. - If a provided value is a dict, update attributes on the SolverCfg-like object (or dict) by setting keys. - Primitive values or arrays/lists replace the target value. - Unknown keys in provided create new entries in the result.

Return type:

dict[str, SolverCfg]

Import Utilities

Functions:

lazy_import_casadi()	Lazily import casadi and return the module.
lazy_import_pink()	Lazily import pin-pink and return its components.
lazy_import_pinocchio()	Lazily import pinocchio and return the module.
lazy_import_pinocchio_casadi()	Lazily import pinocchio.casadi and return the module.
lazy_import_pytorch_kinematics()	Lazily import pytorch_kinematics and return the module.

embodichain.lab.sim.utility.import_utils.lazy_import_casadi()

Lazily import casadi and return the module.

Returns:

The casadi module if available.

Return type:

module

Raises:

ImportError – If the module is not installed.

embodichain.lab.sim.utility.import_utils.lazy_import_pink()

Lazily import pin-pink and return its components.

Returns:

The solve_ik, Configuration, and FrameTask components.

Return type:

tuple

Raises:

ImportError – If the module is not installed.

embodichain.lab.sim.utility.import_utils.lazy_import_pinocchio()

Lazily import pinocchio and return the module.

Returns:

The pinocchio module if available.

Return type:

module

Raises:

ImportError – If the module is not installed.

embodichain.lab.sim.utility.import_utils.lazy_import_pinocchio_casadi()

Lazily import pinocchio.casadi and return the module.

Returns:

The pinocchio.casadi module if available.

Return type:

module

Raises:

ImportError – If the module is not installed.

embodichain.lab.sim.utility.import_utils.lazy_import_pytorch_kinematics()

Lazily import pytorch_kinematics and return the module.

Returns:

The pytorch_kinematics module if available.

Return type:

module

Raises:

ImportError – If the module is not installed.

I/O Utilities

Functions:

suppress_stdout_stderr()

A context manager that redirects stdout and stderr to devnull

embodichain.lab.sim.utility.io_utils.suppress_stdout_stderr()

A context manager that redirects stdout and stderr to devnull

Keyboard Utilities

Functions:

run_keyboard_control_for_camera(sensor[, ...])	Run keyboard control loop for camera pose adjustment.
run_keyboard_control_for_light(light[, ...])	Run keyboard control loop for light adjustment.

embodichain.lab.sim.utility.keyboard_utils.run_keyboard_control_for_camera(sensor, trans_step=0.01, rot_step=1.0, vis_pose=False)

Run keyboard control loop for camera pose adjustment.

Parameters:

• sensor (Camera | str) – Camera sensor or name of the camera to control.

• trans_step (float, optional) – Translation step size. Defaults to 0.01.

• rot_step (float, optional) – Rotation step size in degrees. Defaults to 1.0.

• vis_pose (bool, optional) – Whether to visualize the camera pose in axis form. Defaults to False.

Return type:

None

embodichain.lab.sim.utility.keyboard_utils.run_keyboard_control_for_light(light, trans_step=0.01, intensity_step=1.0, falloff_step=1.0, color_step=0.05, vis_pose=False)

Run keyboard control loop for light adjustment.

Parameters:

• light (Light | str) – Light object or name of the light to control.

• trans_step (float, optional) – Translation step size. Defaults to 0.01.

• intensity_step (float, optional) – Intensity adjustment step. Defaults to 0.1.

• falloff_step (float, optional) – Falloff/radius adjustment step. Defaults to 0.1.

• color_step (float, optional) – Color channel adjustment step. Defaults to 0.05.

• vis_pose (bool, optional) – Whether to visualize the light position with a marker. Defaults to False.

Return type:

None

Simulation Utils

Functions:

create_cube(envs, size[, uid])	Create cube objects in the specified environments or arenas.
create_sphere(envs, radius[, resolution, uid])	Create sphere objects in the specified environments or arenas.
get_dexsim_arena_num()	Get the number of arenas in the default dexsim world.
get_dexsim_arenas()	Get all arenas in the default dexsim world.
get_dexsim_drive_type(drive_type)	Get the dexsim drive type from a string.
is_rt_enabled()	Check if Ray Tracing rendering backend is enabled in the default dexsim world.
load_mesh_objects_from_cfg(cfg, env_list[, ...])	Load mesh objects from configuration.
set_dexsim_articulation_cfg(arts, cfg)	Set articulation configuration for a list of dexsim articulations.

embodichain.lab.sim.utility.sim_utils.create_cube(envs, size, uid='cube')

Create cube objects in the specified environments or arenas.

Parameters:

• envs (List[Union[Env, Arena]]) – List of environments or arenas to create cubes in.

• size (List[float]) – Size of the cube as [length, width, height] in meters.

• uid (str, optional) – Unique identifier for the cube objects. Defaults to “cube”.

Returns:

List of created cube mesh objects.

Return type:

List[MeshObject]

embodichain.lab.sim.utility.sim_utils.create_sphere(envs, radius, resolution=20, uid='sphere')

Create sphere objects in the specified environments or arenas.

Parameters:

• envs (List[Union[Env, Arena]]) – List of environments or arenas to create spheres in.

• radius (float) – Radius of the sphere in meters.

• resolution (int, optional) – Resolution of the sphere mesh. Defaults to 20.

• uid (str, optional) – Unique identifier for the sphere objects. Defaults to “sphere”.

Returns:

List of created sphere mesh objects.

Return type:

List[MeshObject]

embodichain.lab.sim.utility.sim_utils.get_dexsim_arena_num()

Get the number of arenas in the default dexsim world.

Returns:

The number of arenas in the default world, or 0 if no world is found.

Return type:

int

embodichain.lab.sim.utility.sim_utils.get_dexsim_arenas()

Get all arenas in the default dexsim world.

Returns:

A list of arenas in the default world, or an empty list if no world is found.

Return type:

List[dexsim.environment.Arena]

embodichain.lab.sim.utility.sim_utils.get_dexsim_drive_type(drive_type)

Get the dexsim drive type from a string.

Parameters:

drive_type (str) – The drive type as a string.

Returns:

The corresponding DriveType enum.

Return type:

DriveType

embodichain.lab.sim.utility.sim_utils.is_rt_enabled()

Check if Ray Tracing rendering backend is enabled in the default dexsim world.

Returns:

True if Ray Tracing rendering is enabled, False otherwise.

Return type:

bool

embodichain.lab.sim.utility.sim_utils.load_mesh_objects_from_cfg(cfg, env_list, cache_dir=None)

Load mesh objects from configuration.

Parameters:

• cfg (RigidObjectCfg) – Configuration for the rigid object.

• env_list (List[Arena]) – List of arenas to load the objects into.

cache_dir (str | None, optional): Directory for caching convex decomposition files. Defaults to None :returns: List of loaded mesh objects. :rtype: List[MeshObject]

embodichain.lab.sim.utility.sim_utils.set_dexsim_articulation_cfg(arts, cfg)

Set articulation configuration for a list of dexsim articulations.

Parameters:

• arts (List[Articulation]) – List of dexsim articulations to configure.

• cfg (ArticulationCfg) – Configuration object containing articulation settings.

Return type:

None

Mesh Utils

Functions:

export_articulation_mesh(articulation[, ...])

Export a combined mesh from all links of one or more articulations to a mesh file format.

embodichain.lab.sim.utility.mesh_utils.export_articulation_mesh(articulation, output_path='./articulation.obj', link_names=None, base_xpos=None, base_link_name=None, **kwargs)

Export a combined mesh from all links of one or more articulations to a mesh file format.

This function retrieves the link geometries and poses from the given articulation(s), transforms each link mesh to its world pose, merges them into a single mesh, and exports the result to the specified file path. The export format is inferred from the file extension (e.g., .obj, .ply, .stl, .glb, .gltf).

Parameters:

• articulation (dexsim.engine.Articulation or list) – The articulation object or list of articulations.

• output_path (str) – The output file path including the file name and extension. Supported extensions: .obj, .ply, .stl, .glb, .gltf.

• link_names (list[str] or dict[Any, list[str]] | None) – Specify which links to export. If None, export all links.

• base_xpos (np.ndarray | None) – 4x4 homogeneous transformation matrix. All meshes will be transformed into this base pose coordinate system.

• base_link_name (str | None) – If specified, use the pose of this link as the base pose. The link will be searched from all link_names of all articulations.

Returns:

The combined Open3D mesh object of all articulations.

Return type:

o3d.geometry.TriangleMesh

Solver Utilities

Functions:

build_reduced_pinocchio_robot(entire_robot, ...)	Build a reduced robot model by locking all joints except those specified.
compute_pinocchio_fk(pin_module, robot, ...)	Compute forward kinematics using Pinocchio for the specified end-effector.
create_pk_chain(urdf_path, device, **kwargs)	Factory method to create a pk.SerialChain object from a URDF file.
create_pk_serial_chain([urdf_path, device, ...])	Factory method to create a pk.SerialChain object from a URDF file.
validate_iteration_params(pos_eps, rot_eps, ...)	Validate iteration parameters for IK solvers.

embodichain.lab.sim.utility.solver_utils.build_reduced_pinocchio_robot(entire_robot, joint_names)

Build a reduced robot model by locking all joints except those specified.

This utility function creates a reduced Pinocchio robot model by locking all joints except those in the provided joint_names list and the ‘universe’ joint.

Parameters:

• entire_robot (RobotWrapper) – The full Pinocchio RobotWrapper model.

• joint_names (List[str]) – List of joint names to keep unlocked in the reduced model.

Returns:

The reduced robot model with specified joints unlocked.

Return type:

pin.RobotWrapper

embodichain.lab.sim.utility.solver_utils.compute_pinocchio_fk(pin_module, robot, qpos, end_link_name, tcp_xpos)

Compute forward kinematics using Pinocchio for the specified end-effector.

This utility function computes FK using the Pinocchio library and applies the TCP transformation.

Parameters:

• pin_module (Any) – The imported pinocchio module.

• robot (RobotWrapper) – The Pinocchio RobotWrapper model.

• qpos (Union[Tensor, ndarray]) – Joint positions, shape should be (nq,).

• end_link_name (str) – The name of the end-effector link.

• tcp_xpos (ndarray) – The TCP transformation matrix (4x4).

Returns:

The resulting end-effector pose as a (4, 4) homogeneous

transformation matrix.

Return type:

np.ndarray

Raises:

ValueError – If qpos shape is not (nq,) or if the end_link_name is not found.

embodichain.lab.sim.utility.solver_utils.create_pk_chain(urdf_path, device, **kwargs)

Factory method to create a pk.SerialChain object from a URDF file.

Parameters:

• urdf_path (str) – Path to the URDF file.

• end_link_name (str) – Name of the end-effector link.

root_link_name (str | None): Name of the root link. If None, the chain starts from the base.

device (torch.device): The device to which the chain will be moved. is_serial (bool): Whether the chain is serial or not.

Returns:

The created serial chain object.

Return type:

pk.SerialChain

embodichain.lab.sim.utility.solver_utils.create_pk_serial_chain(urdf_path=None, device=None, end_link_name=None, root_link_name=None, chain=None, **kwargs)

Factory method to create a pk.SerialChain object from a URDF file.

Parameters:

• urdf_path (str) – Path to the URDF file.

• end_link_name (str) – Name of the end-effector link.

root_link_name (str | None): Name of the root link. If None, the chain starts from the base.

device (torch.device): The device to which the chain will be moved. is_serial (bool): Whether the chain is serial or not.

Returns:

The created serial chain object.

Return type:

pk.SerialChain

embodichain.lab.sim.utility.solver_utils.validate_iteration_params(pos_eps, rot_eps, max_iterations, dt, damp, num_samples)

Validate iteration parameters for IK solvers.

This helper validates common iteration parameters used by IK solvers. Returns True if all parameters are valid, False otherwise, and logs warnings for invalid parameters.

Parameters:

• pos_eps (float) – Position convergence threshold, must be positive.

• rot_eps (float) – Rotation convergence threshold, must be positive.

• max_iterations (int) – Maximum number of iterations, must be positive.

• dt (float) – Time step size, must be positive.

• damp (float) – Damping factor, must be non-negative.

• num_samples (int) – Number of samples, must be positive.

Returns:

True if all parameters are valid, False otherwise.

Return type:

bool

Tensor Utilities

Functions:

to_tensor(arr[, dtype, device])

Convert input to torch.Tensor with specified dtype and device.

embodichain.lab.sim.utility.tensor.to_tensor(arr, dtype=torch.float32, device=None)

Convert input to torch.Tensor with specified dtype and device.

Supports torch.Tensor, np.ndarray, and list.

Parameters:

• arr (Union[torch.Tensor, np.ndarray, list]) – Input array.

• dtype (torch.dtype, optional) – Desired tensor dtype. Defaults to torch.float32.

• device (torch.device, optional) – Desired device. If None, uses current device.

Returns:

Converted tensor.

Return type:

torch.Tensor