embodichain.lab.sim.motion.solvers

Inverse-kinematics solvers sharing the BaseSolver interface.

Provides analytic (SRS, OPW, UR, FEP), numerical (Pinocchio, Pink, Differential), and learning-based (PyTorch, NeuralIK) solvers, one per robot control part.

Overview

Inverse-kinematics solvers for robot control parts. Every solver implements the BaseSolver interface (forward kinematics, IK, Jacobian, TCP, and joint limits) and is constructed from a SolverCfg subclass whose init_solver() factory produces the runtime instance inside RobotCfg. A robot may carry one solver per control part. All solvers share a pytorch_kinematics serial chain for FK and Jacobian computation, with torch.compile used by the base FK path. The differentiable solver uses eager Tensor FK with standard autograd for its full unrolled graph.

Available implementations: analytic/closed-form (SRS, OPW, UR, FEP), numerical (Pinocchio, Pink with null-space posture tasks, Differential), learning-based (PytorchSolver, NeuralIKSolver).

Classes

SolverCfg	Configuration for the kinematic solver used in the robot simulation.
BaseSolver	Base interface shared by robot forward and inverse kinematics solvers.
SRSSolverCfg	Configuration for SRS inverse kinematics controller.
SRSSolver	SRS inverse kinematics (IK) controller.
OPWSolverCfg	Configuration for OPW inverse kinematics controller.
OPWSolver	OPW inverse kinematics (IK) controller.
URSolverCfg	URSolverCfg(class_type: 'str' = <factory>, urdf_path: 'str | None' = <factory>, joint_names: 'list[str] | None' = <factory>, end_link_name: 'str' = <factory>, root_link_name: 'str' = <factory>, tcp: 'torch.Tensor | np.ndarray' = <factory>, ik_nearest_weight: 'List[float] | None' = <factory>, user_qpos_limits: 'List[float] | None' = <factory>, ur_type: 'str' = <factory>, d1: 'float' = <factory>, a2: 'float' = <factory>, a3: 'float' = <factory>, d4: 'float' = <factory>, d5: 'float' = <factory>, d6: 'float' = <factory>, alpha1: 'float' = <factory>, alpha4: 'float' = <factory>, alpha5: 'float' = <factory>)
URSolver
PytorchSolverCfg	Configuration for the pytorch kinematics solver used in the robot simulation.
PytorchSolver
FEPSolverCfg	Geometric IK tolerances and optional sampled redundancy optimization.
FEPSolver	Geometric IK for the Franka axis layout, with up to eight branches.
PinocchioSolverCfg	PinocchioSolverCfg(class_type: 'str' = <factory>, urdf_path: 'str | None' = <factory>, joint_names: 'list[str] | None' = <factory>, end_link_name: 'str' = <factory>, root_link_name: 'str' = <factory>, tcp: 'torch.Tensor | np.ndarray' = <factory>, ik_nearest_weight: 'List[float] | None' = <factory>, user_qpos_limits: 'List[float] | None' = <factory>, mesh_path: 'str' = <factory>, pos_eps: 'float' = <factory>, rot_eps: 'float' = <factory>, max_iterations: 'int' = <factory>, dt: 'float' = <factory>, damp: 'float' = <factory>, is_only_position_constraint: 'bool' = <factory>, num_samples: 'int' = <factory>)
PinocchioSolver
PinkSolverCfg	Configure the Pink task-space IK solver.
PinkSolver	Iterative task-space IK with adaptive damping and convergence checks.
DifferentialSolverCfg	Configuration for differential inverse kinematics controller.
DifferentialSolver	Differential inverse kinematics (IK) controller.
NeuralIKSolverCfg	Configuration for the neural network IK solver.
NeuralIKSolver	IK solver using a trained neural network policy.

Base Solver

Robot.compute_batch_ik(..., continuous=True) checks BaseSolver.supports_continuous_batch_ik before generating any candidates. The capability defaults to False; OPW opts in and implements the protected _select_continuous_ik_path hook. Supporting solvers accept get_ik(return_all_solutions=True) and return validity (M, K) and joint candidates (M, K, DOF). Selection consumes (B, N, K, DOF) candidates, (B, N, K) validity, and an initial (B, DOF) seed, then returns (B, N) validity and (B, N, DOF) joint positions. Other solvers retain ordinary batch IK and reject continuous requests before solving.

class embodichain.lab.sim.motion.solvers.SolverCfg

Configuration for the kinematic solver used in the robot simulation.

Attributes:

class_type	The class type of the solver to be used.
end_link_name	The name of the end-effector link for the solver.
ik_nearest_weight	Weights for the inverse kinematics nearest calculation.
joint_names	List of joint names for the solver.
root_link_name	The name of the root/base link for the solver.
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.
user_qpos_limits	User defined Joint position limits [2, DOF] for the solver.

Methods:

from_dict(init_dict)

Initialize the concrete solver configuration from a dictionary.

class_type: str

The class type of the solver to be used.

end_link_name: str

The name of the end-effector link for the solver.

This defines the target link for forward/inverse kinematics calculations. Must match a link name in the URDF file.

classmethod from_dict(init_dict)

Initialize the concrete solver configuration from a dictionary.

The concrete config receives all recognized dataclass init fields in its constructor so initialization and __post_init__ observe the final inputs exactly once. Legacy unannotated config attributes are applied afterward. Unknown fields preserve the historical behavior: they are ignored with a warning.

Return type:

SolverCfg

ik_nearest_weight: Optional[List[float]]

Weights for the inverse kinematics nearest calculation.

The weights influence how the solver prioritizes closeness to the seed position when multiple solutions are available.

joint_names: list[str] | None

List of joint names for the solver.

If None, all joints in the URDF will be used. If specified, only these named joints will be included in the kinematic chain.

root_link_name: str

The name of the root/base link for the solver.

This defines the starting point of the kinematic chain. Must match a link name in the URDF file.

tcp: Tensor | ndarray

The tool center point (TCP) position as a 4x4 homogeneous matrix.

This represents the position and orientation of the tool in the robot’s end-effector frame. It must be a finite SE(3) rigid transform; affine transforms are not supported.

urdf_path: str | None

The file path to the URDF model of the robot.

user_qpos_limits: Optional[List[float]]

User defined Joint position limits [2, DOF] for the solver. If not provided (None), this value will replace by joint limits defined in urdf when solver init from robot. If provided, the solver will use the intersection of user defined limits and urdf limits as the final joint limits.

class embodichain.lab.sim.motion.solvers.BaseSolver

Bases: object

Base interface shared by robot forward and inverse kinematics solvers.

Methods:

__init__([cfg, device])	Initializes the kinematics solver with a robot model.
get_default_qpos_seed()	Get the feasibility-safe default IK seed: the joint-range midpoint.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_fk_batch(qpos)	Compute chain-root FK while preserving arbitrary leading batch axes.
get_ik(target_pose[, qpos_seed, num_samples])	Computes the inverse kinematics for a given target pose.
get_ik_batch(target_xpos[, qpos_seed])	Compute nearest-solution IK with a uniform batch result contract.
get_ik_nearest_weight()	Gets the inverse kinematics nearest weight.
get_jacobian(qpos[, locations, jac_type])	Compute the Jacobian matrix for the given joint positions.
get_qpos_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
prepare_buffers(max_batch)	Reserve reusable analytic IK scratch capacity.
set_ik_nearest_weight(ik_weight[, joint_ids])	Sets the inverse kinematics nearest weight.
set_qpos_limits(lower_qpos_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Sets the TCP position with the given 4x4 homogeneous matrix.
update_with_robot_limit(robot_qpos_limits)	Intersect solver joint limits with the robot's effective qpos limits.

Attributes:

supports_continuous_batch_ik

Whether all-candidate IK and temporally continuous selection are supported.

__init__(cfg=None, device=None, **kwargs)

Initializes the kinematics solver with a robot model.

Parameters:

• cfg (SolverCfg) – The configuration for the solver.

• device (str or torch.device, optional) – The device to run the solver on. Defaults to “cuda” if available, otherwise “cpu”.

• **kwargs – Additional keyword arguments for customization.

get_default_qpos_seed()

Get the feasibility-safe default IK seed: the joint-range midpoint.

A zero configuration violates the joint limits of some robots (for example Franka FR3, whose joints 4 and 6 exclude zero), which wastes a multi-start slot, biases nearest-solution selection toward the limits, and can start iterative solvers from an infeasible configuration. The midpoint is inside the limits by construction and maximises the distance to both bounds.

Returns:

Default joint seed with shape (dof,) on the solver device.

Return type:

torch.Tensor

Raises:

ValueError – If the solver joint limits are not initialized.

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_fk_batch(qpos)

Compute chain-root FK while preserving arbitrary leading batch axes.

Parameters:

qpos (Tensor) – Joint positions with shape (..., dof) on the solver device.

Return type:

Tensor

Returns:

TCP transforms with shape (..., 4, 4).

abstract get_ik(target_pose, qpos_seed=None, num_samples=None, **kwargs)

Computes the inverse kinematics for a given target pose.

This method generates random joint configurations within the specified limits, including the provided qpos_seed, and attempts to find valid inverse kinematics solutions. It then identifies the joint position that is closest to the qpos_seed.

Parameters:

• target_pose (torch.Tensor) – The target pose represented as a 4x4 transformation matrix.

• qpos_seed (torch.Tensor | None) – The initial joint positions used as a seed.

• num_samples (int | None) – The number of random joint seeds to generate.

• **kwargs – Additional keyword arguments for customization.

Returns:

• success (torch.Tensor): Boolean tensor indicating IK solution validity for each environment, shape (num_envs,).

• target_joints (torch.Tensor): Computed target joint positions, shape (num_envs, num_joints).

Return type:

Tuple[torch.Tensor, torch.Tensor]

get_ik_batch(target_xpos, qpos_seed=None)

Compute nearest-solution IK with a uniform batch result contract.

Parameters:

• target_xpos (Tensor) – Chain-root TCP transforms with shape (..., 4, 4).

• qpos_seed (Tensor | None) – Optional seeds with matching leading axes and dof joints.

Return type:

tuple[Tensor, Tensor]

Returns:

Boolean success (...) and joint positions (..., dof). No candidate axis is exposed; concrete get_ik APIs stay unchanged.

get_ik_nearest_weight()

Gets the inverse kinematics nearest weight.

Returns:

A numpy array representing the nearest weights for inverse kinematics.

Return type:

np.ndarray

get_jacobian(qpos, locations=None, jac_type='full')

Compute the Jacobian matrix for the given joint positions.

Parameters:

• qpos (torch.Tensor) – The joint positions. Shape: (dof,) or (batch_size, dof).

• locations (torch.Tensor | np.ndarray | None) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str) – ‘full’, ‘trans’, or ‘rot’ for full, translational, or rotational Jacobian. Defaults to ‘full’.

Returns:

The Jacobian matrix. Shape:

• (batch_size, 6, dof) for ‘full’

• (batch_size, 3, dof) for ‘trans’ or ‘rot’

Return type:

torch.Tensor

get_qpos_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

• lower_qpos_limits (List[float]): The current lower limits for each joint.

• upper_qpos_limits (List[float]): The current upper limits for each joint.

Return type:

dict

get_tcp()

Returns the current TCP position.

Returns:

The current TCP position.

Return type:

np.ndarray

Raises:

ValueError – If the TCP position has not been set.

prepare_buffers(max_batch)

Reserve reusable analytic IK scratch capacity.

Parameters:

max_batch (int) – Maximum flattened target count, including IK seeds. Storage is allocated lazily by analytic backends and grows when necessary. Returned FK/IK tensors remain owned by the caller.

Return type:

None

set_ik_nearest_weight(ik_weight, joint_ids=None)

Sets the inverse kinematics nearest weight.

Parameters:

• ik_weight (np.ndarray) – A numpy array representing the nearest weights for inverse kinematics.

• joint_ids (np.ndarray, optional) – A numpy array representing the indices of the joints to which the weights apply. If None, defaults to all joint indices.

Returns:

True if the weights are set successfully, False otherwise.

Return type:

bool

set_qpos_limits(lower_qpos_limits, upper_qpos_limits)

Sets the upper and lower joint position limits.

Parameters:

• lower_qpos_limits (List[float]) – A list of lower limits for each joint.

• upper_qpos_limits (List[float]) – A list of upper limits for each joint.

Returns:

True if limits are successfully set, False if the input is invalid.

Return type:

bool

set_tcp(xpos)

Sets the TCP position with the given 4x4 homogeneous matrix.

Parameters:

xpos (np.ndarray) – The 4x4 homogeneous matrix to be set as the TCP position.

Raises:

• ValueError – If the input is not a finite 4x4 SE(3) transform.

• numpy.linalg.LinAlgError – If the transform is singular.

Return type:

None

property supports_continuous_batch_ik: bool

Whether all-candidate IK and temporally continuous selection are supported.

Supporting solvers return candidate validity shaped (M, K) and joint positions shaped (M, K, DOF) from get_ik(return_all_solutions=True). They must also implement _select_continuous_ik_path().

update_with_robot_limit(robot_qpos_limits)

Intersect solver joint limits with the robot’s effective qpos limits.

Robot-side articulation limits are the hard physical bound. Solver-specific limits from SolverCfg.user_qpos_limits may be even tighter for planning. The final solver limits must satisfy both constraints.

Parameters:

robot_qpos_limits (torch.Tensor) – [DOF, 2] tensor of joint limits from the robot data.

PyTorch Solver

PytorchSolver uses batched damped least squares. Set PytorchSolverCfg.is_only_position_constraint=True for underactuated arms such as SO101: the objective and convergence test constrain only TCP position. Targets remain root-relative 4x4 poses, but their rotation is ignored, including when a translated or rotated TCP is configured. Runtime set_tcp() and set_iteration_params() updates preserve this behavior. The default False continues to require both position and rotation convergence.

class embodichain.lab.sim.motion.solvers.PytorchSolverCfg

Configuration for the pytorch kinematics solver used in the robot simulation.

This configuration includes properties related to the solver setup, such as the URDF path, the end link name, and the root link name, along with the Tool Center Point (TCP).

Attributes:

class_type	The class type of the solver to be used.
damp	Diagonal DLS regularization; zero retains the PK numerical floor of 1e-9.
dt	Time step for numerical integration
enable_seed_selection	Retrieve multi-start seeds from a precomputed FK database.
end_link_name	The name of the end-effector link for the solver.
ik_nearest_weight	Weights for the inverse kinematics nearest calculation.
ik_solution_selection	How to collapse multi-seed IK candidates into one solution.
is_only_position_constraint	Constrain TCP position only, ignoring target orientation in DLS and convergence.
joint_names	List of joint names for the solver.
max_iterations	Maximum number of iterations for the solver
num_samples	Number of samples to generate different joint seeds for IK iterations.
pos_eps	Tolerance for convergence for position
root_link_name	The name of the root/base link for the solver.
rot_eps	Tolerance for convergence for rotation
seed_db_size	Number of joint configurations stored in the seed-selection database.
seed_rot_scale	Metres-per-radian weight of the rotation block in the seed-retrieval pose metric.
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.
user_qpos_limits	User defined Joint position limits [2, DOF] for the solver.

Methods:

init_solver([device])

Initialize the solver with the configuration.

class_type: str

The class type of the solver to be used.

damp: float

Diagonal DLS regularization; zero retains the PK numerical floor of 1e-9.

dt: float

Time step for numerical integration

enable_seed_selection: bool

Retrieve multi-start seeds from a precomputed FK database.

When enabled, the random slots of the multi-start seed batch are replaced by database configurations whose flange poses are nearest to each target (re-ranked by the predicted joint-space correction). Slot 0 still holds the caller-provided seed. The database is built lazily on the first get_ik call and rebuilt automatically when joint limits change.

end_link_name: str

The name of the end-effector link for the solver.

This defines the target link for forward/inverse kinematics calculations. Must match a link name in the URDF file.

ik_nearest_weight: list[float] | None

Weights for the inverse kinematics nearest calculation.

The weights influence how the solver prioritizes closeness to the seed position when multiple solutions are available.

ik_solution_selection: str

How to collapse multi-seed IK candidates into one solution.

"nearest" (default) keeps the successful candidate closest to the caller seed under ik_nearest_weight. "manipulability" re-ranks the successful candidates by their Yoshikawa manipulability (embodichain.compute.kinematics.yoshikawa_manipulability()) and returns the best-conditioned posture instead of the first/nearest one.

init_solver(device=device(type='cpu'), **kwargs)

Initialize the solver with the configuration.

Parameters:

• device (torch.device) – The device to use for the solver. Defaults to CPU.

• **kwargs – Additional keyword arguments that may be used for solver initialization.

Returns:

An initialized solver instance.

Return type:

PytorchSolver

is_only_position_constraint: bool

Constrain TCP position only, ignoring target orientation in DLS and convergence.

joint_names: list[str] | None

List of joint names for the solver.

If None, all joints in the URDF will be used. If specified, only these named joints will be included in the kinematic chain.

max_iterations: int

Maximum number of iterations for the solver

num_samples: int

Number of samples to generate different joint seeds for IK iterations.

A higher number of samples increases the chances of finding a valid solution

pos_eps: float

Tolerance for convergence for position

root_link_name: str

The name of the root/base link for the solver.

This defines the starting point of the kinematic chain. Must match a link name in the URDF file.

rot_eps: float

Tolerance for convergence for rotation

seed_db_size: int

Number of joint configurations stored in the seed-selection database.

seed_rot_scale: float

Metres-per-radian weight of the rotation block in the seed-retrieval pose metric.

tcp: torch.Tensor | np.ndarray

The tool center point (TCP) position as a 4x4 homogeneous matrix.

This represents the position and orientation of the tool in the robot’s end-effector frame. It must be a finite SE(3) rigid transform; affine transforms are not supported.

urdf_path: str | None

The file path to the URDF model of the robot.

user_qpos_limits: List[float] | None

User defined Joint position limits [2, DOF] for the solver. If not provided (None), this value will replace by joint limits defined in urdf when solver init from robot. If provided, the solver will use the intersection of user defined limits and urdf limits as the final joint limits.

class embodichain.lab.sim.motion.solvers.PytorchSolver

Bases: BaseSolver

Methods:

__init__(cfg[, device])	Initializes the PyTorch kinematics solver.
get_all_fk(qpos)	Get the forward kinematics for all links from root to end link.
get_default_qpos_seed()	Get the feasibility-safe default IK seed: the joint-range midpoint.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_fk_batch(qpos)	Compute chain-root FK while preserving arbitrary leading batch axes.
get_ik(target_xpos, *args, **kwargs)	Computes the inverse kinematics for a given target pose.
get_ik_batch(target_xpos[, qpos_seed])	Compute nearest-solution IK with a uniform batch result contract.
get_ik_nearest_weight()	Gets the inverse kinematics nearest weight.
get_iteration_params()	Returns the current iteration parameters.
get_jacobian(qpos[, locations, jac_type])	Compute the Jacobian matrix for the given joint positions.
get_qpos_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
prepare_buffers(max_batch)	Reserve reusable analytic IK scratch capacity.
set_ik_nearest_weight(ik_weight[, joint_ids])	Sets the inverse kinematics nearest weight.
set_iteration_params([pos_eps, rot_eps, ...])	Sets the iteration parameters for the kinematics solver.
set_qpos_limits(lower_qpos_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Set the TCP and refresh the position-only IK task frame.
update_with_robot_limit(robot_qpos_limits)	Intersect solver joint limits with the robot's effective qpos limits.

Attributes:

supports_continuous_batch_ik

Return whether batched candidates support continuous path selection.

__init__(cfg, device=None, **kwargs)

Initializes the PyTorch kinematics solver.

This constructor sets up the kinematics solver using PyTorch, allowing for efficient computation of robot kinematics based on the specified URDF model.

Parameters:

• cfg (PytorchSolverCfg) – The configuration for the solver.

• device (str, optional) – The device to use for the solver (e.g., “cpu” or “cuda”).

• **kwargs – Additional keyword arguments passed to the base solver.

get_all_fk(qpos)

Get the forward kinematics for all links from root to end link.

Parameters:

qpos (torch.Tensor) – The joint positions.

Returns:

A list of 4x4 homogeneous transformation matrices representing the poses of all links from root to end link.

Return type:

list

get_default_qpos_seed()

Get the feasibility-safe default IK seed: the joint-range midpoint.

A zero configuration violates the joint limits of some robots (for example Franka FR3, whose joints 4 and 6 exclude zero), which wastes a multi-start slot, biases nearest-solution selection toward the limits, and can start iterative solvers from an infeasible configuration. The midpoint is inside the limits by construction and maximises the distance to both bounds.

Returns:

Default joint seed with shape (dof,) on the solver device.

Return type:

torch.Tensor

Raises:

ValueError – If the solver joint limits are not initialized.

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_fk_batch(qpos)

Compute chain-root FK while preserving arbitrary leading batch axes.

Parameters:

qpos (Tensor) – Joint positions with shape (..., dof) on the solver device.

Return type:

Tensor

Returns:

TCP transforms with shape (..., 4, 4).

get_ik(target_xpos, *args, **kwargs)

Computes the inverse kinematics for a given target pose.

This method generates random joint configurations within the specified limits, including the provided qpos_seed, and attempts to find valid inverse kinematics solutions. It then identifies the joint position that is closest to the qpos_seed.

Parameters:

• target_pose (torch.Tensor) – The target pose represented as a 4x4 transformation matrix.

• qpos_seed (torch.Tensor | None) – The initial joint positions used as a seed.

• num_samples (int | None) – The number of random joint seeds to generate.

• **kwargs – Additional keyword arguments for customization.

Returns:

• success (torch.Tensor): Boolean tensor indicating IK solution validity for each environment, shape (num_envs,).

• target_joints (torch.Tensor): Computed target joint positions, shape (num_envs, num_joints).

Return type:

Tuple[torch.Tensor, torch.Tensor]

get_ik_batch(target_xpos, qpos_seed=None)

Compute nearest-solution IK with a uniform batch result contract.

Parameters:

• target_xpos (Tensor) – Chain-root TCP transforms with shape (..., 4, 4).

• qpos_seed (Tensor | None) – Optional seeds with matching leading axes and dof joints.

Return type:

tuple[Tensor, Tensor]

Returns:

Boolean success (...) and joint positions (..., dof). No candidate axis is exposed; concrete get_ik APIs stay unchanged.

get_ik_nearest_weight()

Gets the inverse kinematics nearest weight.

Returns:

A numpy array representing the nearest weights for inverse kinematics.

Return type:

np.ndarray

get_iteration_params()

Returns the current iteration parameters.

Returns:

A dictionary containing the current values of:

• pos_eps (float): Pos convergence threshold

• rot_eps (float): Rot convergence threshold

• max_iterations (int): Maximum number of iterations.

• dt (float): Time step size.

• damp (float): Damping factor.

• num_samples (int): Number of samples.

• is_only_position_constraint (bool): Flag to indicate whether the solver should only consider position constraints.

Return type:

dict

get_jacobian(qpos, locations=None, jac_type='full')

Compute the Jacobian matrix for the given joint positions.

Parameters:

• qpos (torch.Tensor) – The joint positions. Shape: (dof,) or (batch_size, dof).

• locations (torch.Tensor | np.ndarray | None) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str) – ‘full’, ‘trans’, or ‘rot’ for full, translational, or rotational Jacobian. Defaults to ‘full’.

Returns:

The Jacobian matrix. Shape:

• (batch_size, 6, dof) for ‘full’

• (batch_size, 3, dof) for ‘trans’ or ‘rot’

Return type:

torch.Tensor

get_qpos_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

• lower_qpos_limits (List[float]): The current lower limits for each joint.

• upper_qpos_limits (List[float]): The current upper limits for each joint.

Return type:

dict

get_tcp()

Returns the current TCP position.

Returns:

The current TCP position.

Return type:

np.ndarray

Raises:

ValueError – If the TCP position has not been set.

prepare_buffers(max_batch)

Reserve reusable analytic IK scratch capacity.

Parameters:

max_batch (int) – Maximum flattened target count, including IK seeds. Storage is allocated lazily by analytic backends and grows when necessary. Returned FK/IK tensors remain owned by the caller.

Return type:

None

set_ik_nearest_weight(ik_weight, joint_ids=None)

Sets the inverse kinematics nearest weight.

Parameters:

• ik_weight (np.ndarray) – A numpy array representing the nearest weights for inverse kinematics.

• joint_ids (np.ndarray, optional) – A numpy array representing the indices of the joints to which the weights apply. If None, defaults to all joint indices.

Returns:

True if the weights are set successfully, False otherwise.

Return type:

bool

set_iteration_params(pos_eps=0.0005, rot_eps=0.0005, max_iterations=1000, dt=0.1, damp=1e-06, num_samples=30, is_only_position_constraint=False)

Sets the iteration parameters for the kinematics solver.

Parameters:

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

• rot_eps (float) – Rot 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.

• is_only_position_constraint (bool) – Flag to indicate whether the solver should only consider position constraints.

Returns:

True if all parameters are valid and set, False otherwise.

Return type:

bool

set_qpos_limits(lower_qpos_limits, upper_qpos_limits)

Sets the upper and lower joint position limits.

Parameters:

• lower_qpos_limits (List[float]) – A list of lower limits for each joint.

• upper_qpos_limits (List[float]) – A list of upper limits for each joint.

Returns:

True if limits are successfully set, False if the input is invalid.

Return type:

bool

set_tcp(xpos)

Set the TCP and refresh the position-only IK task frame.

Parameters:

xpos (ndarray) – Finite 4x4 SE(3) transform from the end link to the TCP.

Return type:

None

property supports_continuous_batch_ik: bool

Return whether batched candidates support continuous path selection.

update_with_robot_limit(robot_qpos_limits)

Intersect solver joint limits with the robot’s effective qpos limits.

Robot-side articulation limits are the hard physical bound. Solver-specific limits from SolverCfg.user_qpos_limits may be even tighter for planning. The final solver limits must satisfy both constraints.

Parameters:

robot_qpos_limits (torch.Tensor) – [DOF, 2] tensor of joint limits from the robot data.

class embodichain.lab.sim.motion.solvers.pytorch_solver.PytorchSolverCfg

Implementation of PytorchSolverCfg, configuring DLS tolerances, position-only constraints and multi-start candidate selection.

class embodichain.lab.sim.motion.solvers.pytorch_solver.PytorchSolver

Implementation of PytorchSolver, including runtime TCP and constraint-mode updates for the batched PK backend.

FEP Solver

Geometric IK for Franka-compatible screw-axis layouts, following the fixed-q7 construction in HolisticMotion/GeoFIK. Dimensions, axis signs and fixed transforms are extracted from the URDF; incompatible seven-axis chains raise an error. CPU and CUDA share one Warp kernel.

The seed fixes q7 by default and selects the nearest valid branch. Without a seed, the joint-limit midpoint is used. return_all_solutions=True returns up to eight branches with validity masks; invalid slots contain the clamped seed. Every accepted solution satisfies the actual URDF FK, TCP and joint limits.

ik_solution_selection="nearest" is the default. Set it to "manipulability" to select the valid candidate with the highest shared Yoshikawa score. At fixed q7 this ranks the geometric branches. During search it ranks only the eight candidates retained by the continuity, arm-angle and limit-margin scores, not every sampled candidate. All-solutions ordering is unchanged. Scores within a small relative tolerance use weighted seed distance as a deterministic continuity tie-break. Nearest-result shapes are (N,) and (N, 7); Robot’s IK interface and the batch adapter also return joints without a candidate axis.

Enable redundancy_search to sample and refine q7 using seed distance, arm-angle preference and joint-limit margin. max_joint_step remains a hard per-joint displacement bound in radians. Pass the previous solution as the next seed for sequential motion. Search returns the best sampled solutions, not an exhaustive continuous family; failure does not prove global unreachability. num_samples is rejected in both configuration and IK calls: numerical multi-start sampling is not equivalent to q7 search. Franka retains its existing Pytorch default; FEP is selected explicitly through solver_cfg.

get_arm_angle(qpos) measures the TCP-independent GeoFIK swivel angle in radians, returning NaN at undefined planes. It differs from q7. During search, get_ik(..., arm_angle=...) overrides cfg.arm_angle. With neither supplied, the seed’s angle is preferred when defined. This is a soft posture preference, not a complete humanlike or collision model. See the API below for shapes and options.

Run python -m examples.sim.motion.solvers.fep_solver --device cuda --radius 0.15 from the repository root (or use --device cpu). Add --redundancy-search and optionally --arm-angle 0.3 for posture optimization, or --headless --max-steps 301 for a finite run. The circle example draws target and actual TCP paths and reports IK and physical tracking errors separately.

FEPSolverCfg	Geometric IK tolerances and optional sampled redundancy optimization.
FEPSolver	Geometric IK for the Franka axis layout, with up to eight branches.

class embodichain.lab.sim.motion.solvers.FEPSolverCfg

Geometric IK tolerances and optional sampled redundancy optimization.

Attributes:

arm_angle	Preferred GeoFIK swivel angle in radians; None prefers the seed angle.
arm_angle_weight	Weight on squared wrapped arm-angle error during search.
axis_alignment_tolerance	Maximum sine of axis misalignment when extracting the geometric model.
batch_size	Targets per chunk; default 16384, capped at 4096 for search or 1024 for Jacobians.
class_type	The class type of the solver to be used.
end_link_name	The name of the end-effector link for the solver.
ik_nearest_weight	Weights for the inverse kinematics nearest calculation.
ik_solution_selection	Select by weighted seed distance or "manipulability".
joint_limit_weight	Weight on inverse normalized limit margins during search.
joint_names	List of joint names for the solver.
max_joint_step	Hard per-joint displacement bound from the seed, in radians, during search.
num_samples	Unsupported numerical multi-start option; use redundancy_search instead.
redundancy_search	Search q7 near the seed, expanding and refining when needed.
reference_qpos	URDF joint angles defining the canonical FEP posture, in radians.
root_link_name	The name of the root/base link for the solver.
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.
user_qpos_limits	User defined Joint position limits [2, DOF] for the solver.

Methods:

init_solver([device])

Construct a geometric solver with the configured URDF chain and TCP.

arm_angle: float | None

Preferred GeoFIK swivel angle in radians; None prefers the seed angle.

arm_angle_weight: float

Weight on squared wrapped arm-angle error during search.

axis_alignment_tolerance: float

Maximum sine of axis misalignment when extracting the geometric model.

An explicit calibration allowance does not relax FK acceptance against the original URDF, controlled by position and rotation tolerances.

batch_size: int | None

Targets per chunk; default 16384, capped at 4096 for search or 1024 for Jacobians.

class_type: str

The class type of the solver to be used.

end_link_name: str

The name of the end-effector link for the solver.

This defines the target link for forward/inverse kinematics calculations. Must match a link name in the URDF file.

ik_nearest_weight: List[float] | None

Weights for the inverse kinematics nearest calculation.

The weights influence how the solver prioritizes closeness to the seed position when multiple solutions are available.

ik_solution_selection: str

Select by weighted seed distance or "manipulability".

Manipulability uses the shared Yoshikawa metric on the valid fixed-q7 branches. With redundancy search, it ranks only the eight candidates retained by the continuity, arm-angle and limit-margin scores.

init_solver(device='cpu', **kwargs)

Construct a geometric solver with the configured URDF chain and TCP.

Parameters:

• device (str | device) – CPU or CUDA execution device.

• **kwargs (Any) – Optional pk_serial_chain in solver joint order.

Return type:

FEPSolver

Returns:

Configured Franka-family solver.

joint_limit_weight: float

Weight on inverse normalized limit margins during search.

joint_names: list[str] | None

List of joint names for the solver.

If None, all joints in the URDF will be used. If specified, only these named joints will be included in the kinematic chain.

max_joint_step: float | None

Hard per-joint displacement bound from the seed, in radians, during search.

num_samples: int | None

Unsupported numerical multi-start option; use redundancy_search instead.

redundancy_search: bool

Search q7 near the seed, expanding and refining when needed.

reference_qpos: list[float] | None

URDF joint angles defining the canonical FEP posture, in radians.

Defaults to zero. Inputs, outputs, seeds and limits always retain the original URDF joint coordinates; only geometric reconstruction is rebased.

root_link_name: str

The name of the root/base link for the solver.

This defines the starting point of the kinematic chain. Must match a link name in the URDF file.

tcp: torch.Tensor | np.ndarray

The tool center point (TCP) position as a 4x4 homogeneous matrix.

This represents the position and orientation of the tool in the robot’s end-effector frame. It must be a finite SE(3) rigid transform; affine transforms are not supported.

urdf_path: str | None

The file path to the URDF model of the robot.

user_qpos_limits: List[float] | None

User defined Joint position limits [2, DOF] for the solver. If not provided (None), this value will replace by joint limits defined in urdf when solver init from robot. If provided, the solver will use the intersection of user defined limits and urdf limits as the final joint limits.

class embodichain.lab.sim.motion.solvers.FEPSolver

Bases: BaseSolver

Geometric IK for the Franka axis layout, with up to eight branches.

By default the seed fixes q7 and selects the nearest discrete branch. Optional q7 sampling scores continuity, arm-angle preference and limit margin while preserving geometric FK acceptance. Finite sampling does not guarantee a global optimum or establish global unreachability. CPU and CUDA share the Warp kernels. No collision or bimanual constraints are enforced.

Parameters:

• cfg (FEPSolverCfg) – Model and FK acceptance tolerances.

• device (str | device) – CPU or CUDA execution device.

• **kwargs (Any) – Optional pk_serial_chain in solver joint order.

Methods:

__init__(cfg[, device])	Initializes the kinematics solver with a robot model.
get_arm_angle(qpos)	Measure the oriented shoulder/elbow/joint-7 plane angle.
get_default_qpos_seed()	Get the feasibility-safe default IK seed: the joint-range midpoint.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_fk_batch(qpos)	Compute chain-root FK while preserving arbitrary leading batch axes.
get_ik(target_xpos[, qpos_seed, ...])	Solve geometric branches, optionally optimizing the redundant q7.
get_ik_batch(target_xpos[, qpos_seed])	Compute nearest-solution IK with a uniform batch result contract.
get_ik_nearest_weight()	Gets the inverse kinematics nearest weight.
get_jacobian(qpos[, locations, jac_type])	Compute the Jacobian matrix for the given joint positions.
get_qpos_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
prepare_buffers(max_batch)	Reserve reusable analytic IK scratch capacity.
set_ik_nearest_weight(ik_weight[, joint_ids])	Sets the inverse kinematics nearest weight.
set_qpos_limits(lower_qpos_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Set the TCP and refresh fixed transforms used by geometric IK.
update_with_robot_limit(robot_qpos_limits)	Intersect solver joint limits with the robot's effective qpos limits.

Attributes:

supports_continuous_batch_ik

Whether all-candidate IK and temporally continuous selection are supported.

__init__(cfg, device='cpu', **kwargs)

Initializes the kinematics solver with a robot model.

Parameters:

• cfg (SolverCfg) – The configuration for the solver.

• device (str or torch.device, optional) – The device to run the solver on. Defaults to “cuda” if available, otherwise “cpu”.

• **kwargs (Any) – Additional keyword arguments for customization.

get_arm_angle(qpos)

Measure the oriented shoulder/elbow/joint-7 plane angle.

This is GeoFIK’s swivel convention in the extracted canonical shoulder frame, independent of TCP. It is not the seventh joint angle.

Parameters:

qpos (Tensor) – Joint positions (7,) or (N, 7) in solver order.

Return type:

Tensor

Returns:

Angles (N,) in radians in [-pi, pi]; NaN where the reference plane or elbow plane is singular.

Raises:

ValueError – Joint positions are malformed or nonfinite.

get_default_qpos_seed()

Get the feasibility-safe default IK seed: the joint-range midpoint.

A zero configuration violates the joint limits of some robots (for example Franka FR3, whose joints 4 and 6 exclude zero), which wastes a multi-start slot, biases nearest-solution selection toward the limits, and can start iterative solvers from an infeasible configuration. The midpoint is inside the limits by construction and maximises the distance to both bounds.

Returns:

Default joint seed with shape (dof,) on the solver device.

Return type:

torch.Tensor

Raises:

ValueError – If the solver joint limits are not initialized.

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_fk_batch(qpos)

Compute chain-root FK while preserving arbitrary leading batch axes.

Parameters:

qpos (Tensor) – Joint positions with shape (..., dof) on the solver device.

Return type:

Tensor

Returns:

TCP transforms with shape (..., 4, 4).

get_ik(target_xpos, qpos_seed=None, return_all_solutions=False, arm_angle=None, num_samples=None, **kwargs)

Solve geometric branches, optionally optimizing the redundant q7.

Parameters:

• target_xpos (Tensor) – Rigid TCP targets (N, 4, 4) or (4, 4) in the chain-root frame.

• qpos_seed (Tensor | None) – (N, 7), (1, 7) or (7,). Defaults to the joint-limit midpoint. q7 stays fixed unless search is enabled.

• return_all_solutions (bool) – Return up to eight branches at fixed q7, or the eight best distinct sampled solutions during search.

• arm_angle (float | Tensor | None) – Optional scalar or (N,) preferred swivel angle in radians, overriding cfg.arm_angle for this call. Requires redundancy search. If both are None, use the seed’s arm angle; disable that score where the seed angle is undefined. Undefined candidate angles are rejected when an angle preference is active.

• num_samples (int | None) – Must be None. Numerical multi-start sampling is not equivalent to FEP’s adaptive q7 search; use redundancy_search.

• **kwargs (Any) – Reserved common solver arguments.

Return type:

tuple[Tensor, Tensor]

Returns:

Validity (N,) and joints (N, 7); with all solutions, shapes are (N, 8) and (N, 8, 7). Invalid slots contain the clamped seed. Failure at one q7 does not prove the target unreachable at other q7 values. Singular continuous families use seed representatives. ik_solution_selection="manipulability" selects the highest Yoshikawa score from the valid branches, or the eight retained search candidates. All-solutions ordering remains by seed distance or the search score, regardless of the selection mode.

Raises:

ValueError – Malformed inputs/configuration or unsupported num_samples.

get_ik_batch(target_xpos, qpos_seed=None)

Compute nearest-solution IK with a uniform batch result contract.

Parameters:

• target_xpos (Tensor) – Chain-root TCP transforms with shape (..., 4, 4).

• qpos_seed (Tensor | None) – Optional seeds with matching leading axes and dof joints.

Return type:

tuple[Tensor, Tensor]

Returns:

Boolean success (...) and joint positions (..., dof). No candidate axis is exposed; concrete get_ik APIs stay unchanged.

get_ik_nearest_weight()

Gets the inverse kinematics nearest weight.

Returns:

A numpy array representing the nearest weights for inverse kinematics.

Return type:

np.ndarray

get_jacobian(qpos, locations=None, jac_type='full')

Compute the Jacobian matrix for the given joint positions.

Parameters:

• qpos (torch.Tensor) – The joint positions. Shape: (dof,) or (batch_size, dof).

• locations (torch.Tensor | np.ndarray | None) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str) – ‘full’, ‘trans’, or ‘rot’ for full, translational, or rotational Jacobian. Defaults to ‘full’.

Returns:

The Jacobian matrix. Shape:

• (batch_size, 6, dof) for ‘full’

• (batch_size, 3, dof) for ‘trans’ or ‘rot’

Return type:

torch.Tensor

get_qpos_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

• lower_qpos_limits (List[float]): The current lower limits for each joint.

• upper_qpos_limits (List[float]): The current upper limits for each joint.

Return type:

dict

get_tcp()

Returns the current TCP position.

Returns:

The current TCP position.

Return type:

np.ndarray

Raises:

ValueError – If the TCP position has not been set.

prepare_buffers(max_batch)

Reserve reusable analytic IK scratch capacity.

Parameters:

max_batch (int) – Maximum flattened target count, including IK seeds. Storage is allocated lazily by analytic backends and grows when necessary. Returned FK/IK tensors remain owned by the caller.

Return type:

None

set_ik_nearest_weight(ik_weight, joint_ids=None)

Sets the inverse kinematics nearest weight.

Parameters:

• ik_weight (np.ndarray) – A numpy array representing the nearest weights for inverse kinematics.

• joint_ids (np.ndarray, optional) – A numpy array representing the indices of the joints to which the weights apply. If None, defaults to all joint indices.

Returns:

True if the weights are set successfully, False otherwise.

Return type:

bool

set_qpos_limits(lower_qpos_limits, upper_qpos_limits)

Sets the upper and lower joint position limits.

Parameters:

• lower_qpos_limits (List[float]) – A list of lower limits for each joint.

• upper_qpos_limits (List[float]) – A list of upper limits for each joint.

Returns:

True if limits are successfully set, False if the input is invalid.

Return type:

bool

set_tcp(xpos)

Set the TCP and refresh fixed transforms used by geometric IK.

Parameters:

xpos (ndarray) – TCP transform relative to the configured end link.

Return type:

None

property supports_continuous_batch_ik: bool

Whether all-candidate IK and temporally continuous selection are supported.

Supporting solvers return candidate validity shaped (M, K) and joint positions shaped (M, K, DOF) from get_ik(return_all_solutions=True). They must also implement _select_continuous_ik_path().

update_with_robot_limit(robot_qpos_limits)

Intersect solver joint limits with the robot’s effective qpos limits.

Robot-side articulation limits are the hard physical bound. Solver-specific limits from SolverCfg.user_qpos_limits may be even tighter for planning. The final solver limits must satisfy both constraints.

Parameters:

robot_qpos_limits (torch.Tensor) – [DOF, 2] tensor of joint limits from the robot data.

Pinocchio Solver

class embodichain.lab.sim.motion.solvers.PinocchioSolverCfg

PinocchioSolverCfg(class_type: ‘str’ = <factory>, urdf_path: ‘str | None’ = <factory>, joint_names: ‘list[str] | None’ = <factory>, end_link_name: ‘str’ = <factory>, root_link_name: ‘str’ = <factory>, tcp: ‘torch.Tensor | np.ndarray’ = <factory>, ik_nearest_weight: ‘List[float] | None’ = <factory>, user_qpos_limits: ‘List[float] | None’ = <factory>, mesh_path: ‘str’ = <factory>, pos_eps: ‘float’ = <factory>, rot_eps: ‘float’ = <factory>, max_iterations: ‘int’ = <factory>, dt: ‘float’ = <factory>, damp: ‘float’ = <factory>, is_only_position_constraint: ‘bool’ = <factory>, num_samples: ‘int’ = <factory>)

Attributes:

class_type	The class type of the solver to be used.
end_link_name	The name of the end-effector link for the solver.
ik_nearest_weight	Weights for the inverse kinematics nearest calculation.
joint_names	List of joint names for the solver.
root_link_name	The name of the root/base link for the solver.
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.
user_qpos_limits	User defined Joint position limits [2, DOF] for the solver.

Methods:

init_solver(**kwargs)

Initialize the solver with the configuration.

class_type: str

The class type of the solver to be used.

end_link_name: str

The name of the end-effector link for the solver.

This defines the target link for forward/inverse kinematics calculations. Must match a link name in the URDF file.

ik_nearest_weight: Optional[List[float]]

Weights for the inverse kinematics nearest calculation.

The weights influence how the solver prioritizes closeness to the seed position when multiple solutions are available.

init_solver(**kwargs)

Initialize the solver with the configuration.

Parameters:

**kwargs – Additional keyword arguments that may be used for solver initialization.

Returns:

An initialized solver instance.

Return type:

PinocchioSolver

joint_names: list[str] | None

List of joint names for the solver.

If None, all joints in the URDF will be used. If specified, only these named joints will be included in the kinematic chain.

root_link_name: str

The name of the root/base link for the solver.

This defines the starting point of the kinematic chain. Must match a link name in the URDF file.

tcp: Tensor | ndarray

The tool center point (TCP) position as a 4x4 homogeneous matrix.

This represents the position and orientation of the tool in the robot’s end-effector frame. It must be a finite SE(3) rigid transform; affine transforms are not supported.

urdf_path: str | None

The file path to the URDF model of the robot.

user_qpos_limits: Optional[List[float]]

User defined Joint position limits [2, DOF] for the solver. If not provided (None), this value will replace by joint limits defined in urdf when solver init from robot. If provided, the solver will use the intersection of user defined limits and urdf limits as the final joint limits.

class embodichain.lab.sim.motion.solvers.PinocchioSolver

Bases: BaseSolver

Methods:

__init__(cfg, **kwargs)	Initializes the kinematics solver with a robot model.
get_default_qpos_seed()	Get the feasibility-safe default IK seed: the joint-range midpoint.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_fk_batch(qpos)	Compute chain-root FK while preserving arbitrary leading batch axes.
get_ik(target_xpos[, qpos_seed, qvel_seed, ...])	Solve inverse kinematics (IK) for the robot to achieve the specified end-effector pose.
get_ik_batch(target_xpos[, qpos_seed])	Compute nearest-solution IK with a uniform batch result contract.
get_ik_nearest_weight()	Gets the inverse kinematics nearest weight.
get_iteration_params()	Returns the current iteration parameters.
get_jacobian(qpos[, locations, jac_type])	Compute the Jacobian matrix for the given joint positions.
get_qpos_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
prepare_buffers(max_batch)	Reserve reusable analytic IK scratch capacity.
qpos_to_limits(q, joint_seed)	Adjusts the joint positions (q) to be within specified limits and as close as possible to the joint seed, while minimizing the total weighted difference.
set_ik_nearest_weight(ik_weight[, joint_ids])	Sets the inverse kinematics nearest weight.
set_iteration_params([pos_eps, rot_eps, ...])	Sets the iteration parameters for the kinematics solver.
set_qpos_limits(lower_qpos_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(tcp)	Sets the TCP position with the given 4x4 homogeneous matrix.
update_with_robot_limit(robot_qpos_limits)	Intersect solver joint limits with the robot's effective qpos limits.

Attributes:

supports_continuous_batch_ik

Whether all-candidate IK and temporally continuous selection are supported.

__init__(cfg, **kwargs)

Initializes the kinematics solver with a robot model.

Parameters:

• cfg (SolverCfg) – The configuration for the solver.

• device (str or torch.device, optional) – The device to run the solver on. Defaults to “cuda” if available, otherwise “cpu”.

• **kwargs – Additional keyword arguments for customization.

get_default_qpos_seed()

Get the feasibility-safe default IK seed: the joint-range midpoint.

A zero configuration violates the joint limits of some robots (for example Franka FR3, whose joints 4 and 6 exclude zero), which wastes a multi-start slot, biases nearest-solution selection toward the limits, and can start iterative solvers from an infeasible configuration. The midpoint is inside the limits by construction and maximises the distance to both bounds.

Returns:

Default joint seed with shape (dof,) on the solver device.

Return type:

torch.Tensor

Raises:

ValueError – If the solver joint limits are not initialized.

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_fk_batch(qpos)

Compute chain-root FK while preserving arbitrary leading batch axes.

Parameters:

qpos (Tensor) – Joint positions with shape (..., dof) on the solver device.

Return type:

Tensor

Returns:

TCP transforms with shape (..., 4, 4).

get_ik(target_xpos, qpos_seed=None, qvel_seed=None, return_all_solutions=False, **kwargs)

Solve inverse kinematics (IK) for the robot to achieve the specified end-effector pose.

Parameters:

• target_xpos (torch.Tensor | np.ndarray | None) – Desired end-effector pose as a (4, 4) homogeneous transformation matrix.

• qpos_seed (np.ndarray | None) – Initial joint positions used as the seed for optimization. If None, uses the joint-range midpoint.

• qvel_seed (np.ndarray | None) – Initial joint velocities (not used in current implementation).

• return_all_solutions (bool, optional) – If True, return all valid IK solutions found; otherwise, return only the best solution. Default is False.

• **kwargs – Additional keyword arguments for future extensions.

Returns:

• success (bool or torch.BoolTensor): True if a valid solution is found, False otherwise.

• qpos (np.ndarray or torch.Tensor): Joint positions that achieve the target pose. If no solution, returns the seed joint positions.

Return type:

tuple[bool, np.ndarray]

get_ik_batch(target_xpos, qpos_seed=None)

Compute nearest-solution IK with a uniform batch result contract.

Parameters:

• target_xpos (Tensor) – Chain-root TCP transforms with shape (..., 4, 4).

• qpos_seed (Tensor | None) – Optional seeds with matching leading axes and dof joints.

Return type:

tuple[Tensor, Tensor]

Returns:

Boolean success (...) and joint positions (..., dof). No candidate axis is exposed; concrete get_ik APIs stay unchanged.

get_ik_nearest_weight()

Gets the inverse kinematics nearest weight.

Returns:

A numpy array representing the nearest weights for inverse kinematics.

Return type:

np.ndarray

get_iteration_params()

Returns the current iteration parameters.

Returns:

A dictionary containing the current values of:

• pos_eps (float): Pos convergence threshold

• rot_eps (float): Rot convergence threshold

• max_iterations (int): Maximum number of iterations.

• dt (float): Time step size.

• damp (float): Damping factor.

• num_samples (int): Number of samples.

• is_only_position_constraint (bool): Flag to indicate whether the solver should only consider position constraints.

Return type:

dict

get_jacobian(qpos, locations=None, jac_type='full')

Compute the Jacobian matrix for the given joint positions.

Parameters:

• qpos (torch.Tensor) – The joint positions. Shape: (dof,) or (batch_size, dof).

• locations (torch.Tensor | np.ndarray | None) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str) – ‘full’, ‘trans’, or ‘rot’ for full, translational, or rotational Jacobian. Defaults to ‘full’.

Returns:

The Jacobian matrix. Shape:

• (batch_size, 6, dof) for ‘full’

• (batch_size, 3, dof) for ‘trans’ or ‘rot’

Return type:

torch.Tensor

get_qpos_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

• lower_qpos_limits (List[float]): The current lower limits for each joint.

• upper_qpos_limits (List[float]): The current upper limits for each joint.

Return type:

dict

get_tcp()

Returns the current TCP position.

Returns:

The current TCP position.

Return type:

np.ndarray

Raises:

ValueError – If the TCP position has not been set.

prepare_buffers(max_batch)

Reserve reusable analytic IK scratch capacity.

Parameters:

max_batch (int) – Maximum flattened target count, including IK seeds. Storage is allocated lazily by analytic backends and grows when necessary. Returned FK/IK tensors remain owned by the caller.

Return type:

None

qpos_to_limits(q, joint_seed)

Adjusts the joint positions (q) to be within specified limits and as close as possible to the joint seed, while minimizing the total weighted difference.

Parameters:

• q (np.ndarray) – The original joint positions.

• joint_seed (np.ndarray) – The desired (seed) joint positions.

Returns:

The adjusted joint positions within the specified limits.

Return type:

np.ndarray

set_ik_nearest_weight(ik_weight, joint_ids=None)

Sets the inverse kinematics nearest weight.

Parameters:

• ik_weight (np.ndarray) – A numpy array representing the nearest weights for inverse kinematics.

• joint_ids (np.ndarray, optional) – A numpy array representing the indices of the joints to which the weights apply. If None, defaults to all joint indices.

Returns:

True if the weights are set successfully, False otherwise.

Return type:

bool

set_iteration_params(pos_eps=0.0005, rot_eps=0.0005, max_iterations=1000, dt=0.1, damp=1e-06, num_samples=30, is_only_position_constraint=False)

Sets the iteration parameters for the kinematics solver.

Parameters:

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

• rot_eps (float) – Rot 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.

• is_only_position_constraint (bool) – Flag to indicate whether the solver should only consider position constraints.

Returns:

True if all parameters are valid and set, False otherwise.

Return type:

bool

set_qpos_limits(lower_qpos_limits, upper_qpos_limits)

Sets the upper and lower joint position limits.

Parameters:

• lower_qpos_limits (List[float]) – A list of lower limits for each joint.

• upper_qpos_limits (List[float]) – A list of upper limits for each joint.

Returns:

True if limits are successfully set, False if the input is invalid.

Return type:

bool

set_tcp(tcp)

Sets the TCP position with the given 4x4 homogeneous matrix.

Parameters:

xpos (np.ndarray) – The 4x4 homogeneous matrix to be set as the TCP position.

Raises:

• ValueError – If the input is not a finite 4x4 SE(3) transform.

• numpy.linalg.LinAlgError – If the transform is singular.

Return type:

None

property supports_continuous_batch_ik: bool

Whether all-candidate IK and temporally continuous selection are supported.

Supporting solvers return candidate validity shaped (M, K) and joint positions shaped (M, K, DOF) from get_ik(return_all_solutions=True). They must also implement _select_continuous_ik_path().

update_with_robot_limit(robot_qpos_limits)

Intersect solver joint limits with the robot’s effective qpos limits.

Robot-side articulation limits are the hard physical bound. Solver-specific limits from SolverCfg.user_qpos_limits may be even tighter for planning. The final solver limits must satisfy both constraints.

Parameters:

robot_qpos_limits (torch.Tensor) – [DOF, 2] tensor of joint limits from the robot data.

Pink Solver

class embodichain.lab.sim.motion.solvers.PinkSolverCfg

Configure the Pink task-space IK solver.

Attributes:

class_type	The class type of the solver to be used.
damp	Initial isotropic QP damping.
damping_decay	Multiplier applied after an accepted step.
damping_growth	Multiplier applied after a rejected step.
dt	Integration timestep in seconds.
end_link_name	The name of the end-effector link for the solver.
fail_on_joint_limit_violation	Enable Pink's joint-limit safety break.
fixed_input_tasks	Tasks initialized once and kept fixed during IK calls.
ik_nearest_weight	Weights for the inverse kinematics nearest calculation.
is_only_position_constraint	Stop once position converges without requiring orientation convergence.
joint_names	List of joint names for the solver.
max_backtracks	Maximum damping/backtracking retries for a non-improving step.
max_damping	Upper bound for adaptive damping.
max_iterations	Maximum number of differential-IK iterations.
mesh_path	Optional directory containing URDF mesh assets.
pos_eps	Position convergence tolerance in metres.
root_link_name	The name of the root/base link for the solver.
rot_eps	Orientation convergence tolerance in radians.
show_ik_warnings	Log solver exceptions and non-convergence warnings.
solver_type	QP backend passed to pink.solve_ik().
stagnation_iterations	Consecutive stagnant iterations before terminating.
stagnation_tolerance	Minimum accepted objective improvement before an iteration stagnates.
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.
user_qpos_limits	User defined Joint position limits [2, DOF] for the solver.
variable_input_tasks	Tasks whose first frame target is updated by PinkSolver.get_ik().

Methods:

init_solver(device, **kwargs)

Create a Pink solver and apply the configured TCP.

class_type: str

The class type of the solver to be used.

damp: float

Initial isotropic QP damping.

damping_decay: float

Multiplier applied after an accepted step.

damping_growth: float

Multiplier applied after a rejected step.

dt: float

Integration timestep in seconds.

end_link_name: str

The name of the end-effector link for the solver.

This defines the target link for forward/inverse kinematics calculations. Must match a link name in the URDF file.

fail_on_joint_limit_violation: bool

Enable Pink’s joint-limit safety break.

fixed_input_tasks: list['pink.tasks.Task'] | None

Tasks initialized once and kept fixed during IK calls.

ik_nearest_weight: List[float] | None

Weights for the inverse kinematics nearest calculation.

The weights influence how the solver prioritizes closeness to the seed position when multiple solutions are available.

init_solver(device, **kwargs)

Create a Pink solver and apply the configured TCP.

Parameters:

• device (device) – Torch device used by the solver.

• **kwargs (Any) – Arguments forwarded to PinkSolver.

Return type:

PinkSolver

Returns:

Initialized Pink solver.

is_only_position_constraint: bool

Stop once position converges without requiring orientation convergence.

joint_names: list[str] | None

List of joint names for the solver.

If None, all joints in the URDF will be used. If specified, only these named joints will be included in the kinematic chain.

max_backtracks: int

Maximum damping/backtracking retries for a non-improving step.

max_damping: float

Upper bound for adaptive damping.

max_iterations: int

Maximum number of differential-IK iterations.

mesh_path: str | None

Optional directory containing URDF mesh assets.

pos_eps: float

Position convergence tolerance in metres.

root_link_name: str

The name of the root/base link for the solver.

This defines the starting point of the kinematic chain. Must match a link name in the URDF file.

rot_eps: float

Orientation convergence tolerance in radians.

show_ik_warnings: bool

Log solver exceptions and non-convergence warnings.

solver_type: str

QP backend passed to pink.solve_ik().

stagnation_iterations: int

Consecutive stagnant iterations before terminating.

stagnation_tolerance: float

Minimum accepted objective improvement before an iteration stagnates.

tcp: torch.Tensor | np.ndarray

The tool center point (TCP) position as a 4x4 homogeneous matrix.

This represents the position and orientation of the tool in the robot’s end-effector frame. It must be a finite SE(3) rigid transform; affine transforms are not supported.

urdf_path: str | None

The file path to the URDF model of the robot.

user_qpos_limits: List[float] | None

User defined Joint position limits [2, DOF] for the solver. If not provided (None), this value will replace by joint limits defined in urdf when solver init from robot. If provided, the solver will use the intersection of user defined limits and urdf limits as the final joint limits.

variable_input_tasks: list['pink.tasks.Task'] | None

Tasks whose first frame target is updated by PinkSolver.get_ik().

class embodichain.lab.sim.motion.solvers.PinkSolver

Bases: BaseSolver

Iterative task-space IK with adaptive damping and convergence checks.

Methods:

__init__(cfg, **kwargs)	Initialize Pinocchio, Pink, task state, and joint ordering.
get_default_qpos_seed()	Get the feasibility-safe default IK seed: the joint-range midpoint.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_fk_batch(qpos)	Compute chain-root FK while preserving arbitrary leading batch axes.
get_ik(target_xpos[, qpos_seed, ...])	Solve one or more target poses sequentially.
get_ik_batch(target_xpos[, qpos_seed])	Compute nearest-solution IK with a uniform batch result contract.
get_ik_nearest_weight()	Gets the inverse kinematics nearest weight.
get_jacobian(qpos[, locations, jac_type])	Compute the Jacobian matrix for the given joint positions.
get_qpos_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
prepare_buffers(max_batch)	Reserve reusable analytic IK scratch capacity.
reorder_array(input_array, reordering_array)	Reorder an array with an index mapping.
set_ik_nearest_weight(ik_weight[, joint_ids])	Sets the inverse kinematics nearest weight.
set_qpos_limits(lower_qpos_limits, ...)	Set simulator-ordered limits and synchronize an initialized Pink model.
set_tcp(xpos)	Set the TCP and refresh its inverse used for IK targets.
update_null_space_joint_targets(current_qpos)	Update all null-space posture targets.
update_with_robot_limit(robot_qpos_limits)	Intersect robot limits and synchronize them with Pink.

Attributes:

supports_continuous_batch_ik

Whether all-candidate IK and temporally continuous selection are supported.

__init__(cfg, **kwargs)

Initialize Pinocchio, Pink, task state, and joint ordering.

Parameters:

• cfg (PinkSolverCfg) – Pink solver configuration.

• **kwargs (Any) – Arguments forwarded to BaseSolver.

get_default_qpos_seed()

Get the feasibility-safe default IK seed: the joint-range midpoint.

A zero configuration violates the joint limits of some robots (for example Franka FR3, whose joints 4 and 6 exclude zero), which wastes a multi-start slot, biases nearest-solution selection toward the limits, and can start iterative solvers from an infeasible configuration. The midpoint is inside the limits by construction and maximises the distance to both bounds.

Returns:

Default joint seed with shape (dof,) on the solver device.

Return type:

torch.Tensor

Raises:

ValueError – If the solver joint limits are not initialized.

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_fk_batch(qpos)

Compute chain-root FK while preserving arbitrary leading batch axes.

Parameters:

qpos (Tensor) – Joint positions with shape (..., dof) on the solver device.

Return type:

Tensor

Returns:

TCP transforms with shape (..., 4, 4).

get_ik(target_xpos, qpos_seed=None, return_all_solutions=False, **kwargs)

Solve one or more target poses sequentially.

Parameters:

• target_xpos (Tensor | ndarray) – Target TCP pose with shape (4, 4) or (N, 4, 4).

• qpos_seed (Tensor | ndarray | None) – Joint seed with shape (dof,), (1, dof), (N, dof), or (N, 1, dof). A single seed is broadcast over the batch.

• return_all_solutions (bool) – Accepted for solver-interface compatibility; Pink returns one locally optimal solution per target.

• **kwargs (Any) – Reserved for future solver options.

Return type:

tuple[Tensor, Tensor]

Returns:

A success tensor with shape (N,) and joint solutions with shape (N, 1, dof). Failed targets return their corresponding seeds.

get_ik_batch(target_xpos, qpos_seed=None)

Compute nearest-solution IK with a uniform batch result contract.

Parameters:

• target_xpos (Tensor) – Chain-root TCP transforms with shape (..., 4, 4).

• qpos_seed (Tensor | None) – Optional seeds with matching leading axes and dof joints.

Return type:

tuple[Tensor, Tensor]

Returns:

Boolean success (...) and joint positions (..., dof). No candidate axis is exposed; concrete get_ik APIs stay unchanged.

get_ik_nearest_weight()

Gets the inverse kinematics nearest weight.

Returns:

A numpy array representing the nearest weights for inverse kinematics.

Return type:

np.ndarray

get_jacobian(qpos, locations=None, jac_type='full')

Compute the Jacobian matrix for the given joint positions.

Parameters:

• qpos (torch.Tensor) – The joint positions. Shape: (dof,) or (batch_size, dof).

• locations (torch.Tensor | np.ndarray | None) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str) – ‘full’, ‘trans’, or ‘rot’ for full, translational, or rotational Jacobian. Defaults to ‘full’.

Returns:

The Jacobian matrix. Shape:

• (batch_size, 6, dof) for ‘full’

• (batch_size, 3, dof) for ‘trans’ or ‘rot’

Return type:

torch.Tensor

get_qpos_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

• lower_qpos_limits (List[float]): The current lower limits for each joint.

• upper_qpos_limits (List[float]): The current upper limits for each joint.

Return type:

dict

get_tcp()

Returns the current TCP position.

Returns:

The current TCP position.

Return type:

np.ndarray

Raises:

ValueError – If the TCP position has not been set.

prepare_buffers(max_batch)

Reserve reusable analytic IK scratch capacity.

Parameters:

max_batch (int) – Maximum flattened target count, including IK seeds. Storage is allocated lazily by analytic backends and grows when necessary. Returned FK/IK tensors remain owned by the caller.

Return type:

None

static reorder_array(input_array, reordering_array)

Reorder an array with an index mapping.

Parameters:

• input_array (Sequence[float]) – Values to reorder.

• reordering_array (Sequence[int]) – Source indices in output order.

Return type:

ndarray

Returns:

Reordered NumPy array.

set_ik_nearest_weight(ik_weight, joint_ids=None)

Sets the inverse kinematics nearest weight.

Parameters:

• ik_weight (np.ndarray) – A numpy array representing the nearest weights for inverse kinematics.

• joint_ids (np.ndarray, optional) – A numpy array representing the indices of the joints to which the weights apply. If None, defaults to all joint indices.

Returns:

True if the weights are set successfully, False otherwise.

Return type:

bool

set_qpos_limits(lower_qpos_limits, upper_qpos_limits)

Set simulator-ordered limits and synchronize an initialized Pink model.

Parameters:

• lower_qpos_limits (list[float] | ndarray | Tensor) – Lower limit for every controlled joint.

• upper_qpos_limits (list[float] | ndarray | Tensor) – Upper limit for every controlled joint.

Return type:

bool

Returns:

Whether the limits were accepted.

set_tcp(xpos)

Set the TCP and refresh its inverse used for IK targets.

Parameters:

xpos (ndarray) – Homogeneous end-frame-to-TCP transform.

Return type:

None

property supports_continuous_batch_ik: bool

Whether all-candidate IK and temporally continuous selection are supported.

Supporting solvers return candidate validity shaped (M, K) and joint positions shaped (M, K, DOF) from get_ik(return_all_solutions=True). They must also implement _select_continuous_ik_path().

update_null_space_joint_targets(current_qpos)

Update all null-space posture targets.

Parameters:

current_qpos (Tensor | ndarray) – Joint target in simulator ordering.

Return type:

None

update_with_robot_limit(robot_qpos_limits)

Intersect robot limits and synchronize them with Pink.

Parameters:

robot_qpos_limits (Tensor) – Joint limits in simulator order with shape (dof, 2).

Return type:

None

Differential Solver

class embodichain.lab.sim.motion.solvers.DifferentialSolverCfg

Configuration for differential inverse kinematics controller.

Attributes:

class_type	The class type of the solver to be used.
end_link_name	The name of the end-effector link for the solver.
ik_nearest_weight	Weights for the inverse kinematics nearest calculation.
joint_names	List of joint names for the solver.
root_link_name	The name of the root/base link for the solver.
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.
user_qpos_limits	User defined Joint position limits [2, DOF] for the solver.

Methods:

init_solver([num_envs, device])

Initialize the solver with the configuration.

class_type: str

The class type of the solver to be used.

end_link_name: str

The name of the end-effector link for the solver.

This defines the target link for forward/inverse kinematics calculations. Must match a link name in the URDF file.

ik_nearest_weight: List[float] | None

Weights for the inverse kinematics nearest calculation.

The weights influence how the solver prioritizes closeness to the seed position when multiple solutions are available.

init_solver(num_envs=1, device=device(type='cpu'), **kwargs)

Initialize the solver with the configuration.

Parameters:

• device (torch.device) – The device to use for the solver. Defaults to CPU.

• num_envs (int) – The number of environments for which the solver is initialized.

• **kwargs – Additional keyword arguments that may be used for solver initialization.

Returns:

An initialized solver instance.

Return type:

DifferentialSolver

joint_names: list[str] | None

List of joint names for the solver.

If None, all joints in the URDF will be used. If specified, only these named joints will be included in the kinematic chain.

root_link_name: str

The name of the root/base link for the solver.

This defines the starting point of the kinematic chain. Must match a link name in the URDF file.

tcp: torch.Tensor | np.ndarray

The tool center point (TCP) position as a 4x4 homogeneous matrix.

This represents the position and orientation of the tool in the robot’s end-effector frame. It must be a finite SE(3) rigid transform; affine transforms are not supported.

urdf_path: str | None

The file path to the URDF model of the robot.

user_qpos_limits: List[float] | None

User defined Joint position limits [2, DOF] for the solver. If not provided (None), this value will replace by joint limits defined in urdf when solver init from robot. If provided, the solver will use the intersection of user defined limits and urdf limits as the final joint limits.

class embodichain.lab.sim.motion.solvers.DifferentialSolver

Bases: BaseSolver

Differential inverse kinematics (IK) controller.

This controller implements differential inverse kinematics using various methods for computing the inverse of the Jacobian matrix.

Methods:

__init__(cfg[, num_envs, device])	Initializes the differential kinematics solver.
get_default_qpos_seed()	Get the feasibility-safe default IK seed: the joint-range midpoint.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_fk_batch(qpos)	Compute chain-root FK while preserving arbitrary leading batch axes.
get_ik(target_xpos[, qpos_seed, ...])	Compute target joint positions using differential inverse kinematics.
get_ik_batch(target_xpos[, qpos_seed])	Compute nearest-solution IK with a uniform batch result contract.
get_ik_nearest_weight()	Gets the inverse kinematics nearest weight.
get_jacobian(qpos[, locations, jac_type])	Compute the Jacobian matrix for the given joint positions.
get_qpos_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
prepare_buffers(max_batch)	Reserve reusable analytic IK scratch capacity.
reset([env_ids])	Reset the internal buffers for the specified environments.
set_command(command[, ee_pos, ee_quat])	Set the target end-effector pose command.
set_ik_nearest_weight(ik_weight[, joint_ids])	Sets the inverse kinematics nearest weight.
set_qpos_limits(lower_qpos_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Sets the TCP position with the given 4x4 homogeneous matrix.
update_with_robot_limit(robot_qpos_limits)	Intersect solver joint limits with the robot's effective qpos limits.

Attributes:

action_dim	Returns the dimension of the controller's input command.
supports_continuous_batch_ik	Whether all-candidate IK and temporally continuous selection are supported.

__init__(cfg, num_envs=1, device='cpu', **kwargs)

Initializes the differential kinematics solver.

This constructor sets up the kinematics solver using differential methods, allowing for efficient computation of robot kinematics based on the specified URDF model.

Parameters:

• cfg (DifferentialSolverCfg) – The configuration for the solver.

• num_envs (int) – The number of environments for the solver. Defaults to 1.

• device (str, optional) – The device to use for the solver (e.g., “cpu” or “cuda”). Defaults to “cpu”.

• **kwargs – Additional keyword arguments passed to the base solver.

property action_dim: int

Returns the dimension of the controller’s input command.

Returns:

The dimension of the input command.

Return type:

int

get_default_qpos_seed()

Get the feasibility-safe default IK seed: the joint-range midpoint.

A zero configuration violates the joint limits of some robots (for example Franka FR3, whose joints 4 and 6 exclude zero), which wastes a multi-start slot, biases nearest-solution selection toward the limits, and can start iterative solvers from an infeasible configuration. The midpoint is inside the limits by construction and maximises the distance to both bounds.

Returns:

Default joint seed with shape (dof,) on the solver device.

Return type:

torch.Tensor

Raises:

ValueError – If the solver joint limits are not initialized.

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_fk_batch(qpos)

Compute chain-root FK while preserving arbitrary leading batch axes.

Parameters:

qpos (Tensor) – Joint positions with shape (..., dof) on the solver device.

Return type:

Tensor

Returns:

TCP transforms with shape (..., 4, 4).

get_ik(target_xpos, qpos_seed=None, return_all_solutions=False, jacobian=None, **kwargs)

Compute target joint positions using differential inverse kinematics.

Parameters:

• target_xpos (torch.Tensor) – Current end-effector position, shape (num_envs, 3).

• qpos_seed (torch.Tensor) – Current joint positions, shape (num_envs, num_joints). Defaults to zeros.

• return_all_solutions (bool, optional) – Whether to return all IK solutions or just the best one. Defaults to False.

• jacobian (torch.Tensor) – Jacobian matrix, shape (num_envs, 6, num_joints).

• **kwargs – Additional keyword arguments for future extensions.

Returns:

• success (torch.Tensor): Boolean tensor indicating IK solution validity for each environment, shape (num_envs,).

• target_joints (torch.Tensor): Computed target joint positions, shape (num_envs, num_joints).

Return type:

Tuple[torch.Tensor, torch.Tensor]

get_ik_batch(target_xpos, qpos_seed=None)

Compute nearest-solution IK with a uniform batch result contract.

Parameters:

• target_xpos (Tensor) – Chain-root TCP transforms with shape (..., 4, 4).

• qpos_seed (Tensor | None) – Optional seeds with matching leading axes and dof joints.

Return type:

tuple[Tensor, Tensor]

Returns:

Boolean success (...) and joint positions (..., dof). No candidate axis is exposed; concrete get_ik APIs stay unchanged.

get_ik_nearest_weight()

Gets the inverse kinematics nearest weight.

Returns:

A numpy array representing the nearest weights for inverse kinematics.

Return type:

np.ndarray

get_jacobian(qpos, locations=None, jac_type='full')

Compute the Jacobian matrix for the given joint positions.

Parameters:

• qpos (torch.Tensor) – The joint positions. Shape: (dof,) or (batch_size, dof).

• locations (torch.Tensor | np.ndarray | None) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str) – ‘full’, ‘trans’, or ‘rot’ for full, translational, or rotational Jacobian. Defaults to ‘full’.

Returns:

The Jacobian matrix. Shape:

• (batch_size, 6, dof) for ‘full’

• (batch_size, 3, dof) for ‘trans’ or ‘rot’

Return type:

torch.Tensor

get_qpos_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

• lower_qpos_limits (List[float]): The current lower limits for each joint.

• upper_qpos_limits (List[float]): The current upper limits for each joint.

Return type:

dict

get_tcp()

Returns the current TCP position.

Returns:

The current TCP position.

Return type:

np.ndarray

Raises:

ValueError – If the TCP position has not been set.

prepare_buffers(max_batch)

Reserve reusable analytic IK scratch capacity.

Parameters:

max_batch (int) – Maximum flattened target count, including IK seeds. Storage is allocated lazily by analytic backends and grows when necessary. Returned FK/IK tensors remain owned by the caller.

Return type:

None

reset(env_ids=None)

Reset the internal buffers for the specified environments.

Parameters:

env_ids (torch.Tensor | None) – The environment indices to reset. If None, reset all.

set_command(command, ee_pos=None, ee_quat=None)

Set the target end-effector pose command.

Parameters:

• command (torch.Tensor) – The command tensor.

• ee_pos (torch.Tensor | None) – Current end-effector position (for relative mode).

• ee_quat (torch.Tensor | None) – Current end-effector quaternion (for relative mode).

Returns:

True if the command was set successfully, False otherwise.

Return type:

bool

set_ik_nearest_weight(ik_weight, joint_ids=None)

Sets the inverse kinematics nearest weight.

Parameters:

• ik_weight (np.ndarray) – A numpy array representing the nearest weights for inverse kinematics.

• joint_ids (np.ndarray, optional) – A numpy array representing the indices of the joints to which the weights apply. If None, defaults to all joint indices.

Returns:

True if the weights are set successfully, False otherwise.

Return type:

bool

set_qpos_limits(lower_qpos_limits, upper_qpos_limits)

Sets the upper and lower joint position limits.

Parameters:

• lower_qpos_limits (List[float]) – A list of lower limits for each joint.

• upper_qpos_limits (List[float]) – A list of upper limits for each joint.

Returns:

True if limits are successfully set, False if the input is invalid.

Return type:

bool

set_tcp(xpos)

Sets the TCP position with the given 4x4 homogeneous matrix.

Parameters:

xpos (np.ndarray) – The 4x4 homogeneous matrix to be set as the TCP position.

Raises:

• ValueError – If the input is not a finite 4x4 SE(3) transform.

• numpy.linalg.LinAlgError – If the transform is singular.

Return type:

None

property supports_continuous_batch_ik: bool

Whether all-candidate IK and temporally continuous selection are supported.

Supporting solvers return candidate validity shaped (M, K) and joint positions shaped (M, K, DOF) from get_ik(return_all_solutions=True). They must also implement _select_continuous_ik_path().

update_with_robot_limit(robot_qpos_limits)

Intersect solver joint limits with the robot’s effective qpos limits.

Robot-side articulation limits are the hard physical bound. Solver-specific limits from SolverCfg.user_qpos_limits may be even tighter for planning. The final solver limits must satisfy both constraints.

Parameters:

robot_qpos_limits (torch.Tensor) – [DOF, 2] tensor of joint limits from the robot data.

OPW Solver

class embodichain.lab.sim.motion.solvers.OPWSolverCfg

Configuration for OPW inverse kinematics controller.

Attributes:

class_type	The class type of the solver to be used.
end_link_name	The name of the end-effector link for the solver.
ik_nearest_weight	Weights for the inverse kinematics nearest calculation.
joint_names	List of joint names for the solver.
root_link_name	The name of the root/base link for the solver.
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.
user_qpos_limits	User defined Joint position limits [2, DOF] for the solver.

Methods:

init_solver([device])

Initialize the solver with the configuration.

class_type: str

The class type of the solver to be used.

end_link_name: str

The name of the end-effector link for the solver.

This defines the target link for forward/inverse kinematics calculations. Must match a link name in the URDF file.

ik_nearest_weight: List[float] | None

Weights for the inverse kinematics nearest calculation.

The weights influence how the solver prioritizes closeness to the seed position when multiple solutions are available.

init_solver(device=device(type='cpu'), **kwargs)

Initialize the solver with the configuration.

Parameters:

• device (torch.device) – The device to use for the solver. Defaults to CPU.

• n_sample (int) – The number of environments for which the solver is initialized.

• **kwargs – Additional keyword arguments that may be used for solver initialization.

Returns:

An initialized solver instance.

Return type:

OPWSolver

joint_names: list[str] | None

List of joint names for the solver.

If None, all joints in the URDF will be used. If specified, only these named joints will be included in the kinematic chain.

root_link_name: str

The name of the root/base link for the solver.

This defines the starting point of the kinematic chain. Must match a link name in the URDF file.

tcp: torch.Tensor | np.ndarray

The tool center point (TCP) position as a 4x4 homogeneous matrix.

This represents the position and orientation of the tool in the robot’s end-effector frame. It must be a finite SE(3) rigid transform; affine transforms are not supported.

urdf_path: str | None

The file path to the URDF model of the robot.

user_qpos_limits: List[float] | None

User defined Joint position limits [2, DOF] for the solver. If not provided (None), this value will replace by joint limits defined in urdf when solver init from robot. If provided, the solver will use the intersection of user defined limits and urdf limits as the final joint limits.

class embodichain.lab.sim.motion.solvers.OPWSolver

Bases: BaseSolver

OPW inverse kinematics (IK) controller.

This controller implements OPW inverse kinematics using various methods for computing the inverse of the Jacobian matrix.

Methods:

__init__(cfg[, device])	Initializes the OPW kinematics solver.
get_default_qpos_seed()	Get the feasibility-safe default IK seed: the joint-range midpoint.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_fk_batch(qpos)	Compute chain-root FK while preserving arbitrary leading batch axes.
get_fk_warp(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_ik(target_xpos[, qpos_seed, ...])	Compute target joint positions using OPW inverse kinematics.
get_ik_batch(target_xpos[, qpos_seed])	Compute nearest-solution IK with a uniform batch result contract.
get_ik_nearest_weight()	Gets the inverse kinematics nearest weight.
get_ik_warp(target_xpos, qpos_seed[, ...])	Compute target joint positions using OPW inverse kinematics.
get_jacobian(qpos[, locations, jac_type])	Compute the Jacobian matrix for the given joint positions.
get_qpos_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
prepare_buffers(max_batch)	Reserve reusable analytic IK scratch capacity.
set_ik_nearest_weight(ik_weight[, joint_ids])	Sets the inverse kinematics nearest weight.
set_qpos_limits(lower_qpos_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Sets the TCP position with the given 4x4 homogeneous matrix.
update_with_robot_limit(robot_qpos_limits)	Intersect solver joint limits with the robot's effective qpos limits.

Attributes:

supports_continuous_batch_ik

Return whether OPW supports batched candidates and continuous selection.

__init__(cfg, device='cpu', **kwargs)

Initializes the OPW kinematics solver.

This constructor sets up the kinematics solver using OPW methods, allowing for efficient computation of robot kinematics based on the specified URDF model.

Parameters:

• cfg (OPWSolverCfg) – The configuration for the solver.

• device (str, optional) – The device to use for the solver (e.g., “cpu” or “cuda”). Defaults to “cpu”.

• **kwargs – Additional keyword arguments passed to the base solver.

get_default_qpos_seed()

Get the feasibility-safe default IK seed: the joint-range midpoint.

A zero configuration violates the joint limits of some robots (for example Franka FR3, whose joints 4 and 6 exclude zero), which wastes a multi-start slot, biases nearest-solution selection toward the limits, and can start iterative solvers from an infeasible configuration. The midpoint is inside the limits by construction and maximises the distance to both bounds.

Returns:

Default joint seed with shape (dof,) on the solver device.

Return type:

torch.Tensor

Raises:

ValueError – If the solver joint limits are not initialized.

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_fk_batch(qpos)

Compute chain-root FK while preserving arbitrary leading batch axes.

Parameters:

qpos (Tensor) – Joint positions with shape (..., dof) on the solver device.

Return type:

Tensor

Returns:

TCP transforms with shape (..., 4, 4).

get_fk_warp(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_ik(target_xpos, qpos_seed=None, return_all_solutions=False, **kwargs)

Compute target joint positions using OPW inverse kinematics.

Parameters:

• target_xpos (torch.Tensor) – Current end-effector pose, shape (n_sample, 4, 4).

• qpos_seed (torch.Tensor) – Current joint positions, shape (n_sample, num_joints). Defaults to None.

• return_all_solutions (bool, optional) – Whether to return all IK solutions or just the best one. Defaults to False.

• **kwargs – Additional keyword arguments for future extensions.

Returns:

• target_joints (torch.Tensor): Computed target joint positions, shape (n_sample, num_joints).

• success (torch.Tensor): Boolean tensor indicating IK solution validity for each environment, shape (n_sample,).

Return type:

Tuple[torch.Tensor, torch.Tensor]

get_ik_batch(target_xpos, qpos_seed=None)

Compute nearest-solution IK with a uniform batch result contract.

Parameters:

• target_xpos (Tensor) – Chain-root TCP transforms with shape (..., 4, 4).

• qpos_seed (Tensor | None) – Optional seeds with matching leading axes and dof joints.

Return type:

tuple[Tensor, Tensor]

Returns:

Boolean success (...) and joint positions (..., dof). No candidate axis is exposed; concrete get_ik APIs stay unchanged.

get_ik_nearest_weight()

Gets the inverse kinematics nearest weight.

Returns:

A numpy array representing the nearest weights for inverse kinematics.

Return type:

np.ndarray

get_ik_warp(target_xpos, qpos_seed, return_all_solutions=False, **kwargs)

Compute target joint positions using OPW inverse kinematics.

Parameters:

• target_xpos (torch.Tensor) – Current end-effector pose, shape (n_sample, 4, 4).

• qpos_seed (torch.Tensor) – Current joint positions, shape (n_sample, num_joints).

• return_all_solutions (bool, optional) – Whether to return all IK solutions or just the best one. Defaults to False.

• **kwargs – Additional keyword arguments for future extensions.

Returns:

• target_joints (torch.Tensor): Computed target joint positions, shape (n_sample, n_solution, num_joints).

• success (torch.Tensor): Boolean tensor indicating IK solution validity for each environment, shape (n_sample,).

Return type:

Tuple[torch.Tensor, torch.Tensor]

get_jacobian(qpos, locations=None, jac_type='full')

Compute the Jacobian matrix for the given joint positions.

Parameters:

• qpos (torch.Tensor) – The joint positions. Shape: (dof,) or (batch_size, dof).

• locations (torch.Tensor | np.ndarray | None) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str) – ‘full’, ‘trans’, or ‘rot’ for full, translational, or rotational Jacobian. Defaults to ‘full’.

Returns:

The Jacobian matrix. Shape:

• (batch_size, 6, dof) for ‘full’

• (batch_size, 3, dof) for ‘trans’ or ‘rot’

Return type:

torch.Tensor

get_qpos_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

• lower_qpos_limits (List[float]): The current lower limits for each joint.

• upper_qpos_limits (List[float]): The current upper limits for each joint.

Return type:

dict

get_tcp()

Returns the current TCP position.

Returns:

The current TCP position.

Return type:

np.ndarray

Raises:

ValueError – If the TCP position has not been set.

prepare_buffers(max_batch)

Reserve reusable analytic IK scratch capacity.

Parameters:

max_batch (int) – Maximum flattened target count, including IK seeds. Storage is allocated lazily by analytic backends and grows when necessary. Returned FK/IK tensors remain owned by the caller.

Return type:

None

set_ik_nearest_weight(ik_weight, joint_ids=None)

Sets the inverse kinematics nearest weight.

Parameters:

• ik_weight (np.ndarray) – A numpy array representing the nearest weights for inverse kinematics.

• joint_ids (np.ndarray, optional) – A numpy array representing the indices of the joints to which the weights apply. If None, defaults to all joint indices.

Returns:

True if the weights are set successfully, False otherwise.

Return type:

bool

set_qpos_limits(lower_qpos_limits, upper_qpos_limits)

Sets the upper and lower joint position limits.

Parameters:

• lower_qpos_limits (List[float]) – A list of lower limits for each joint.

• upper_qpos_limits (List[float]) – A list of upper limits for each joint.

Returns:

True if limits are successfully set, False if the input is invalid.

Return type:

bool

set_tcp(xpos)

Sets the TCP position with the given 4x4 homogeneous matrix.

Parameters:

xpos (np.ndarray) – The 4x4 homogeneous matrix to be set as the TCP position.

Raises:

• ValueError – If the input is not a finite 4x4 SE(3) transform.

• numpy.linalg.LinAlgError – If the transform is singular.

property supports_continuous_batch_ik: bool

Return whether OPW supports batched candidates and continuous selection.

update_with_robot_limit(robot_qpos_limits)

Intersect solver joint limits with the robot’s effective qpos limits.

Robot-side articulation limits are the hard physical bound. Solver-specific limits from SolverCfg.user_qpos_limits may be even tighter for planning. The final solver limits must satisfy both constraints.

Parameters:

robot_qpos_limits (torch.Tensor) – [DOF, 2] tensor of joint limits from the robot data.

SRS Solver

class embodichain.lab.sim.motion.solvers.SRSSolverCfg

Configuration for SRS inverse kinematics controller.

Attributes:

T_b_ob	Base to observed base transform.
T_e_oe	End-effector to observed end-effector transform.
class_type	Type of the solver class.
dh_params	Denavit-Hartenberg parameters for the robot's kinematic chain.
end_link_name	The name of the end-effector link for the solver.
ik_nearest_weight	Weights for each joint when finding the nearest IK solution.
joint_names	List of joint names for the solver.
link_lengths	Link lengths of the robot arm.
num_samples	Number of samples for elbow angle during IK computation.
redundancy_step	Angular step in radians for seed-centered redundancy search.
root_link_name	The name of the root/base link for the solver.
rotation_directions	Rotation directions for each joint.
search_mode	Redundancy search strategy.
sort_ik	Whether to sort IK solutions based on proximity to seed joint positions.
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.
user_qpos_limits	User defined Joint position limits [2, DOF] for the solver.

Methods:

init_solver([num_envs, device])

Initialize the solver with the configuration.

T_b_ob: ndarray

Base to observed base transform.

T_e_oe: ndarray

End-effector to observed end-effector transform.

class_type: str

Type of the solver class.

dh_params: list

Denavit-Hartenberg parameters for the robot’s kinematic chain.

end_link_name: str

The name of the end-effector link for the solver.

This defines the target link for forward/inverse kinematics calculations. Must match a link name in the URDF file.

ik_nearest_weight: np.array

Weights for each joint when finding the nearest IK solution.

init_solver(num_envs=1, device=device(type='cpu'), **kwargs)

Initialize the solver with the configuration.

Parameters:

• device (torch.device) – The device to use for the solver. Defaults to CPU.

• num_envs (int) – The number of environments for which the solver is initialized.

• **kwargs – Additional keyword arguments that may be used for solver initialization.

Returns:

An initialized solver instance.

Return type:

SRSSolver

joint_names: list[str] | None

List of joint names for the solver.

If None, all joints in the URDF will be used. If specified, only these named joints will be included in the kinematic chain.

link_lengths: list

Link lengths of the robot arm.

num_samples: int

Number of samples for elbow angle during IK computation.

redundancy_step: float

Angular step in radians for seed-centered redundancy search.

root_link_name: str

The name of the root/base link for the solver.

This defines the starting point of the kinematic chain. Must match a link name in the URDF file.

rotation_directions: list

Rotation directions for each joint.

search_mode: Literal['seeded', 'full']

Redundancy search strategy.

"seeded" searches the seed arm angle first and then expands radially; "full" samples the complete [-pi, pi) interval.

sort_ik: bool

Whether to sort IK solutions based on proximity to seed joint positions.

tcp: torch.Tensor | np.ndarray

The tool center point (TCP) position as a 4x4 homogeneous matrix.

This represents the position and orientation of the tool in the robot’s end-effector frame. It must be a finite SE(3) rigid transform; affine transforms are not supported.

urdf_path: str | None

The file path to the URDF model of the robot.

user_qpos_limits: List[float] | None

User defined Joint position limits [2, DOF] for the solver. If not provided (None), this value will replace by joint limits defined in urdf when solver init from robot. If provided, the solver will use the intersection of user defined limits and urdf limits as the final joint limits.

class embodichain.lab.sim.motion.solvers.SRSSolver

Bases: BaseSolver

SRS inverse kinematics (IK) controller.

This controller implements SRS inverse kinematics using various methods for computing the inverse of the Jacobian matrix.

Methods:

__init__(cfg, num_envs, device, **kwargs)	Initializes the SRS kinematics solver.
get_default_qpos_seed()	Get the feasibility-safe default IK seed: the joint-range midpoint.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_fk_batch(qpos)	Compute chain-root FK while preserving arbitrary leading batch axes.
get_ik(target_xpos[, qpos_seed, ...])	Compute inverse kinematics (IK) for the given target pose.
get_ik_batch(target_xpos[, qpos_seed])	Compute nearest-solution IK with a uniform batch result contract.
get_ik_nearest_weight()	Gets the inverse kinematics nearest weight.
get_jacobian(qpos[, locations, jac_type])	Compute the Jacobian matrix for the given joint positions.
get_qpos_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
prepare_buffers(max_batch)	Reserve reusable analytic IK scratch capacity.
set_ik_nearest_weight(ik_weight[, joint_ids])	Set nearest-solution weights and synchronize backend caches.
set_qpos_limits(lower_qpos_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Set TCP and synchronize the analytical backend caches.
update_with_robot_limit(robot_qpos_limits)	Intersect solver joint limits with the robot's effective qpos limits.

Attributes:

supports_continuous_batch_ik

Whether all-candidate IK and temporally continuous selection are supported.

__init__(cfg, num_envs, device, **kwargs)

Initializes the SRS kinematics solver.

This constructor sets up the kinematics solver using SRS methods, allowing for efficient computation of robot kinematics based on the specified URDF model.

Parameters:

• cfg (SRSSolverCfg) – The configuration for the solver.

• num_envs (int) – The number of environments for the solver.

• device (str, optional) – The device to use for the solver (e.g., “cpu” or “cuda”).

• **kwargs – Additional keyword arguments passed to the base solver.

get_default_qpos_seed()

Get the feasibility-safe default IK seed: the joint-range midpoint.

A zero configuration violates the joint limits of some robots (for example Franka FR3, whose joints 4 and 6 exclude zero), which wastes a multi-start slot, biases nearest-solution selection toward the limits, and can start iterative solvers from an infeasible configuration. The midpoint is inside the limits by construction and maximises the distance to both bounds.

Returns:

Default joint seed with shape (dof,) on the solver device.

Return type:

torch.Tensor

Raises:

ValueError – If the solver joint limits are not initialized.

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_fk_batch(qpos)

Compute chain-root FK while preserving arbitrary leading batch axes.

Parameters:

qpos (Tensor) – Joint positions with shape (..., dof) on the solver device.

Return type:

Tensor

Returns:

TCP transforms with shape (..., 4, 4).

get_ik(target_xpos, qpos_seed=None, return_all_solutions=False, **kwargs)

Compute inverse kinematics (IK) for the given target pose.

Parameters:

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

• qpos_seed (Tensor) – Initial joint positions (rad). Default is None.

• return_all_solutions (bool) – Whether to return all solutions. Default is False.

• kwargs – Additional keyword arguments.

Returns:

Success flag and joint positions.

Return type:

Tuple[torch.Tensor, torch.Tensor]

get_ik_batch(target_xpos, qpos_seed=None)

Compute nearest-solution IK with a uniform batch result contract.

Parameters:

• target_xpos (Tensor) – Chain-root TCP transforms with shape (..., 4, 4).

• qpos_seed (Tensor | None) – Optional seeds with matching leading axes and dof joints.

Return type:

tuple[Tensor, Tensor]

Returns:

Boolean success (...) and joint positions (..., dof). No candidate axis is exposed; concrete get_ik APIs stay unchanged.

get_ik_nearest_weight()

Gets the inverse kinematics nearest weight.

Returns:

A numpy array representing the nearest weights for inverse kinematics.

Return type:

np.ndarray

get_jacobian(qpos, locations=None, jac_type='full')

Compute the Jacobian matrix for the given joint positions.

Parameters:

• qpos (torch.Tensor) – The joint positions. Shape: (dof,) or (batch_size, dof).

• locations (torch.Tensor | np.ndarray | None) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str) – ‘full’, ‘trans’, or ‘rot’ for full, translational, or rotational Jacobian. Defaults to ‘full’.

Returns:

The Jacobian matrix. Shape:

• (batch_size, 6, dof) for ‘full’

• (batch_size, 3, dof) for ‘trans’ or ‘rot’

Return type:

torch.Tensor

get_qpos_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

• lower_qpos_limits (List[float]): The current lower limits for each joint.

• upper_qpos_limits (List[float]): The current upper limits for each joint.

Return type:

dict

get_tcp()

Returns the current TCP position.

Returns:

The current TCP position.

Return type:

np.ndarray

Raises:

ValueError – If the TCP position has not been set.

prepare_buffers(max_batch)

Reserve reusable analytic IK scratch capacity.

Parameters:

max_batch (int) – Maximum flattened target count, including IK seeds. Storage is allocated lazily by analytic backends and grows when necessary. Returned FK/IK tensors remain owned by the caller.

Return type:

None

set_ik_nearest_weight(ik_weight, joint_ids=None)

Set nearest-solution weights and synchronize backend caches.

Return type:

bool

set_qpos_limits(lower_qpos_limits, upper_qpos_limits)

Sets the upper and lower joint position limits.

Parameters:

• lower_qpos_limits (List[float]) – A list of lower limits for each joint.

• upper_qpos_limits (List[float]) – A list of upper limits for each joint.

Returns:

True if limits are successfully set, False if the input is invalid.

Return type:

bool

set_tcp(xpos)

Set TCP and synchronize the analytical backend caches.

Return type:

None

property supports_continuous_batch_ik: bool

Whether all-candidate IK and temporally continuous selection are supported.

Supporting solvers return candidate validity shaped (M, K) and joint positions shaped (M, K, DOF) from get_ik(return_all_solutions=True). They must also implement _select_continuous_ik_path().

update_with_robot_limit(robot_qpos_limits)

Intersect solver joint limits with the robot’s effective qpos limits.

Robot-side articulation limits are the hard physical bound. Solver-specific limits from SolverCfg.user_qpos_limits may be even tighter for planning. The final solver limits must satisfy both constraints.

Parameters:

robot_qpos_limits (torch.Tensor) – [DOF, 2] tensor of joint limits from the robot data.

UR Solver

URSolverCfg selects the analytical DH parameters for UR3, UR5, UR10 and their e-series variants. URSolver.get_ik() returns validity flags followed by joint positions. By default, a dedicated Warp kernel selects the nearest valid candidate using the joint seed and per-joint weights, returning shapes (N,) and (N, 6) without allocating the complete candidate tensor.

With return_all_solutions=True, the solver returns shapes (N, 512) and (N, 512, 6). These candidates contain eight analytical branches expanded over 64 combinations of periodic joint representatives, with validity flags for FK agreement and joint limits. When no shifted representative fits a joint’s limits, its base value is repeated.

URSolverCfg

URSolverCfg(class_type: 'str' = <factory>, urdf_path: 'str | None' = <factory>, joint_names: 'list[str] | None' = <factory>, end_link_name: 'str' = <factory>, root_link_name: 'str' = <factory>, tcp: 'torch.Tensor | np.ndarray' = <factory>, ik_nearest_weight: 'List[float] | None' = <factory>, user_qpos_limits: 'List[float] | None' = <factory>, ur_type: 'str' = <factory>, d1: 'float' = <factory>, a2: 'float' = <factory>, a3: 'float' = <factory>, d4: 'float' = <factory>, d5: 'float' = <factory>, d6: 'float' = <factory>, alpha1: 'float' = <factory>, alpha4: 'float' = <factory>, alpha5: 'float' = <factory>)

URSolver

class embodichain.lab.sim.motion.solvers.URSolverCfg

URSolverCfg(class_type: ‘str’ = <factory>, urdf_path: ‘str | None’ = <factory>, joint_names: ‘list[str] | None’ = <factory>, end_link_name: ‘str’ = <factory>, root_link_name: ‘str’ = <factory>, tcp: ‘torch.Tensor | np.ndarray’ = <factory>, ik_nearest_weight: ‘List[float] | None’ = <factory>, user_qpos_limits: ‘List[float] | None’ = <factory>, ur_type: ‘str’ = <factory>, d1: ‘float’ = <factory>, a2: ‘float’ = <factory>, a3: ‘float’ = <factory>, d4: ‘float’ = <factory>, d5: ‘float’ = <factory>, d6: ‘float’ = <factory>, alpha1: ‘float’ = <factory>, alpha4: ‘float’ = <factory>, alpha5: ‘float’ = <factory>)

Attributes:

class_type	The class type of the solver to be used.
end_link_name	The name of the end-effector link for the solver.
ik_nearest_weight	Weights for the inverse kinematics nearest calculation.
joint_names	List of joint names for the solver.
root_link_name	The name of the root/base link for the solver.
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.
user_qpos_limits	User defined Joint position limits [2, DOF] for the solver.

Methods:

init_solver([device])

Initialize the solver with the configuration.

class_type: str

The class type of the solver to be used.

end_link_name: str

The name of the end-effector link for the solver.

This defines the target link for forward/inverse kinematics calculations. Must match a link name in the URDF file.

ik_nearest_weight: List[float] | None

Weights for the inverse kinematics nearest calculation.

The weights influence how the solver prioritizes closeness to the seed position when multiple solutions are available.

init_solver(device=device(type='cpu'), **kwargs)

Initialize the solver with the configuration.

Parameters:

• device (torch.device) – The device to use for the solver. Defaults to CPU.

• **kwargs – Additional keyword arguments that may be used for solver initialization.

Returns:

An initialized solver instance.

Return type:

URSolver

joint_names: list[str] | None

List of joint names for the solver.

If None, all joints in the URDF will be used. If specified, only these named joints will be included in the kinematic chain.

root_link_name: str

The name of the root/base link for the solver.

This defines the starting point of the kinematic chain. Must match a link name in the URDF file.

tcp: torch.Tensor | np.ndarray

The tool center point (TCP) position as a 4x4 homogeneous matrix.

This represents the position and orientation of the tool in the robot’s end-effector frame. It must be a finite SE(3) rigid transform; affine transforms are not supported.

urdf_path: str | None

The file path to the URDF model of the robot.

user_qpos_limits: List[float] | None

User defined Joint position limits [2, DOF] for the solver. If not provided (None), this value will replace by joint limits defined in urdf when solver init from robot. If provided, the solver will use the intersection of user defined limits and urdf limits as the final joint limits.

class embodichain.lab.sim.motion.solvers.URSolver

Bases: BaseSolver

Methods:

__init__(cfg, device, **kwargs)	Initializes the kinematics solver with a robot model.
dh_matrix(theta_i, d_i, a_i, alpha_i)	Compute the Denavit-Hartenberg transformation matrix.
get_default_qpos_seed()	Get the feasibility-safe default IK seed: the joint-range midpoint.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_fk_batch(qpos)	Compute chain-root FK while preserving arbitrary leading batch axes.
get_ik(target_xpos[, qpos_seed, ...])	Compute target joint positions using UR inverse kinematics.
get_ik_batch(target_xpos[, qpos_seed])	Compute nearest-solution IK with a uniform batch result contract.
get_ik_nearest_weight()	Gets the inverse kinematics nearest weight.
get_jacobian(qpos[, locations, jac_type])	Compute the Jacobian matrix for the given joint positions.
get_qpos_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
prepare_buffers(max_batch)	Reserve reusable analytic IK scratch capacity.
set_ik_nearest_weight(ik_weight[, joint_ids])	Sets the inverse kinematics nearest weight.
set_qpos_limits(lower_qpos_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(tcp)	Sets the TCP position with the given 4x4 homogeneous matrix.
update_with_robot_limit(robot_qpos_limits)	Intersect solver joint limits with the robot's effective qpos limits.

Attributes:

supports_continuous_batch_ik

Whether all-candidate IK and temporally continuous selection are supported.

__init__(cfg, device, **kwargs)

Initializes the kinematics solver with a robot model.

Parameters:

• cfg (SolverCfg) – The configuration for the solver.

• device (str or torch.device, optional) – The device to run the solver on. Defaults to “cuda” if available, otherwise “cpu”.

• **kwargs – Additional keyword arguments for customization.

static dh_matrix(theta_i, d_i, a_i, alpha_i)

Compute the Denavit-Hartenberg transformation matrix.

Parameters:

• theta_i (float) – Joint angle in radians.

• d_i (float) – Link offset along the previous z-axis.

• a_i (float) – Link length along the previous x-axis.

• alpha_i (float) – Link twist angle in radians.

Returns:

A 4x4 transformation matrix representing the pose of the next link.

Return type:

torch.Tensor

get_default_qpos_seed()

Get the feasibility-safe default IK seed: the joint-range midpoint.

A zero configuration violates the joint limits of some robots (for example Franka FR3, whose joints 4 and 6 exclude zero), which wastes a multi-start slot, biases nearest-solution selection toward the limits, and can start iterative solvers from an infeasible configuration. The midpoint is inside the limits by construction and maximises the distance to both bounds.

Returns:

Default joint seed with shape (dof,) on the solver device.

Return type:

torch.Tensor

Raises:

ValueError – If the solver joint limits are not initialized.

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_fk_batch(qpos)

Compute chain-root FK while preserving arbitrary leading batch axes.

Parameters:

qpos (Tensor) – Joint positions with shape (..., dof) on the solver device.

Return type:

Tensor

Returns:

TCP transforms with shape (..., 4, 4).

get_ik(target_xpos, qpos_seed=None, return_all_solutions=False, **kwargs)

Compute target joint positions using UR inverse kinematics.

Parameters:

• target_xpos (torch.Tensor) – Current end-effector pose, shape (n_sample, 4, 4).

• qpos_seed (torch.Tensor) – Current joint positions, shape (n_sample, 6) or (1, 6). Defaults to the joint-limit midpoint.

• return_all_solutions (bool, optional) – Whether to return all 512 candidates. False uses a fused Warp selection, with bounded legacy selection for distances close enough to be affected by floating-point rounding.

• **kwargs – Additional keyword arguments for future extensions.

Returns:

• success (torch.Tensor): Boolean validity, shape (n_sample,) or (n_sample, 512) when all solutions are requested.

• target_joints (torch.Tensor): Joint positions, shape (n_sample, 6) or (n_sample, 512, 6), respectively.

Return type:

Tuple[torch.Tensor, torch.Tensor]

get_ik_batch(target_xpos, qpos_seed=None)

Compute nearest-solution IK with a uniform batch result contract.

Parameters:

• target_xpos (Tensor) – Chain-root TCP transforms with shape (..., 4, 4).

• qpos_seed (Tensor | None) – Optional seeds with matching leading axes and dof joints.

Return type:

tuple[Tensor, Tensor]

Returns:

Boolean success (...) and joint positions (..., dof). No candidate axis is exposed; concrete get_ik APIs stay unchanged.

get_ik_nearest_weight()

Gets the inverse kinematics nearest weight.

Returns:

A numpy array representing the nearest weights for inverse kinematics.

Return type:

np.ndarray

get_jacobian(qpos, locations=None, jac_type='full')

Compute the Jacobian matrix for the given joint positions.

Parameters:

• qpos (torch.Tensor) – The joint positions. Shape: (dof,) or (batch_size, dof).

• locations (torch.Tensor | np.ndarray | None) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str) – ‘full’, ‘trans’, or ‘rot’ for full, translational, or rotational Jacobian. Defaults to ‘full’.

Returns:

The Jacobian matrix. Shape:

• (batch_size, 6, dof) for ‘full’

• (batch_size, 3, dof) for ‘trans’ or ‘rot’

Return type:

torch.Tensor

get_qpos_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

• lower_qpos_limits (List[float]): The current lower limits for each joint.

• upper_qpos_limits (List[float]): The current upper limits for each joint.

Return type:

dict

get_tcp()

Returns the current TCP position.

Returns:

The current TCP position.

Return type:

np.ndarray

Raises:

ValueError – If the TCP position has not been set.

prepare_buffers(max_batch)

Reserve reusable analytic IK scratch capacity.

Parameters:

max_batch (int) – Maximum flattened target count, including IK seeds. Storage is allocated lazily by analytic backends and grows when necessary. Returned FK/IK tensors remain owned by the caller.

Return type:

None

set_ik_nearest_weight(ik_weight, joint_ids=None)

Sets the inverse kinematics nearest weight.

Parameters:

• ik_weight (np.ndarray) – A numpy array representing the nearest weights for inverse kinematics.

• joint_ids (np.ndarray, optional) – A numpy array representing the indices of the joints to which the weights apply. If None, defaults to all joint indices.

Returns:

True if the weights are set successfully, False otherwise.

Return type:

bool

set_qpos_limits(lower_qpos_limits, upper_qpos_limits)

Sets the upper and lower joint position limits.

Parameters:

• lower_qpos_limits (List[float]) – A list of lower limits for each joint.

• upper_qpos_limits (List[float]) – A list of upper limits for each joint.

Returns:

True if limits are successfully set, False if the input is invalid.

Return type:

bool

set_tcp(tcp)

Sets the TCP position with the given 4x4 homogeneous matrix.

Parameters:

xpos (np.ndarray) – The 4x4 homogeneous matrix to be set as the TCP position.

Raises:

• ValueError – If the input is not a finite 4x4 SE(3) transform.

• numpy.linalg.LinAlgError – If the transform is singular.

property supports_continuous_batch_ik: bool

Whether all-candidate IK and temporally continuous selection are supported.

Supporting solvers return candidate validity shaped (M, K) and joint positions shaped (M, K, DOF) from get_ik(return_all_solutions=True). They must also implement _select_continuous_ik_path().

update_with_robot_limit(robot_qpos_limits)

Intersect solver joint limits with the robot’s effective qpos limits.

Robot-side articulation limits are the hard physical bound. Solver-specific limits from SolverCfg.user_qpos_limits may be even tighter for planning. The final solver limits must satisfy both constraints.

Parameters:

robot_qpos_limits (torch.Tensor) – [DOF, 2] tensor of joint limits from the robot data.

Neural IK Solver

class embodichain.lab.sim.motion.solvers.NeuralIKSolverCfg

Configuration for the neural network IK solver.

Attributes:

action_scale	Action scaling factor (radians).
checkpoint_path	Path to the trained policy checkpoint (.pt file).
class_type	The class type of the solver to be used.
end_link_name	The name of the end-effector link for the solver.
hidden_dims	Hidden layer dimensions for the MLP policy network.
ik_nearest_weight	Weights for the inverse kinematics nearest calculation.
joint_names	List of joint names for the solver.
max_steps	Number of policy inference iterations per IK solve.
num_arm_joints	Number of arm joints (policy only controls arm, not fingers).
num_samples	Number of random initial qpos seeds to sample per target pose.
obs_dim	Observation dimension.
pos_eps	Position convergence tolerance (meters) for success check.
root_link_name	The name of the root/base link for the solver.
rot_eps	Rotation convergence tolerance (radians) for success check.
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.
user_qpos_limits	User defined Joint position limits [2, DOF] for the solver.

action_scale: float

Action scaling factor (radians).

checkpoint_path: str

Path to the trained policy checkpoint (.pt file).

class_type: str

The class type of the solver to be used.

end_link_name: str

The name of the end-effector link for the solver.

This defines the target link for forward/inverse kinematics calculations. Must match a link name in the URDF file.

hidden_dims: list[int]

Hidden layer dimensions for the MLP policy network.

ik_nearest_weight: List[float] | None

Weights for the inverse kinematics nearest calculation.

The weights influence how the solver prioritizes closeness to the seed position when multiple solutions are available.

joint_names: list[str] | None

List of joint names for the solver.

If None, all joints in the URDF will be used. If specified, only these named joints will be included in the kinematic chain.

max_steps: int

Number of policy inference iterations per IK solve.

num_arm_joints: int

Number of arm joints (policy only controls arm, not fingers).

num_samples: int

Number of random initial qpos seeds to sample per target pose.

obs_dim: int | None

Observation dimension. If None, auto-computed as 2 * num_arm_joints + 14.

pos_eps: float

Position convergence tolerance (meters) for success check.

root_link_name: str

The name of the root/base link for the solver.

This defines the starting point of the kinematic chain. Must match a link name in the URDF file.

rot_eps: float

Rotation convergence tolerance (radians) for success check.

tcp: torch.Tensor | np.ndarray

The tool center point (TCP) position as a 4x4 homogeneous matrix.

This represents the position and orientation of the tool in the robot’s end-effector frame. It must be a finite SE(3) rigid transform; affine transforms are not supported.

urdf_path: str | None

The file path to the URDF model of the robot.

user_qpos_limits: List[float] | None

User defined Joint position limits [2, DOF] for the solver. If not provided (None), this value will replace by joint limits defined in urdf when solver init from robot. If provided, the solver will use the intersection of user defined limits and urdf limits as the final joint limits.

class embodichain.lab.sim.motion.solvers.NeuralIKSolver

Bases: BaseSolver

IK solver using a trained neural network policy.

Loads a checkpoint containing actor_mean weights and obs_normalizer stats, then runs iterative inference to solve IK queries.

Methods:

__init__(cfg[, device])	Initializes the kinematics solver with a robot model.
get_default_qpos_seed()	Get the feasibility-safe default IK seed: the joint-range midpoint.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_fk_batch(qpos)	Compute chain-root FK while preserving arbitrary leading batch axes.
get_ik(target_xpos[, qpos_seed, num_samples])	Solve IK using the trained neural policy.
get_ik_batch(target_xpos[, qpos_seed])	Compute nearest-solution IK with a uniform batch result contract.
get_ik_nearest_weight()	Gets the inverse kinematics nearest weight.
get_jacobian(qpos[, locations, jac_type])	Compute the Jacobian matrix for the given joint positions.
get_qpos_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
prepare_buffers(max_batch)	Reserve reusable analytic IK scratch capacity.
set_ik_nearest_weight(ik_weight[, joint_ids])	Sets the inverse kinematics nearest weight.
set_qpos_limits(lower_qpos_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Sets the TCP position with the given 4x4 homogeneous matrix.
update_with_robot_limit(robot_qpos_limits)	Intersect solver joint limits with the robot's effective qpos limits.

Attributes:

supports_continuous_batch_ik

Whether all-candidate IK and temporally continuous selection are supported.

__init__(cfg, device=None, **kwargs)

Initializes the kinematics solver with a robot model.

Parameters:

• cfg (SolverCfg) – The configuration for the solver.

• device (str or torch.device, optional) – The device to run the solver on. Defaults to “cuda” if available, otherwise “cpu”.

• **kwargs – Additional keyword arguments for customization.

get_default_qpos_seed()

Get the feasibility-safe default IK seed: the joint-range midpoint.

A zero configuration violates the joint limits of some robots (for example Franka FR3, whose joints 4 and 6 exclude zero), which wastes a multi-start slot, biases nearest-solution selection toward the limits, and can start iterative solvers from an infeasible configuration. The midpoint is inside the limits by construction and maximises the distance to both bounds.

Returns:

Default joint seed with shape (dof,) on the solver device.

Return type:

torch.Tensor

Raises:

ValueError – If the solver joint limits are not initialized.

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

• qpos (torch.Tensor) – Joint positions. Can be a single configuration (dof,) or a batch (batch_size, dof).

• **kwargs – Additional keyword arguments for customization.

Returns:

The homogeneous transformation matrix of the end link with TCP applied.

Shape is (4, 4) for single input, or (batch_size, 4, 4) for batch input.

Return type:

torch.Tensor

get_fk_batch(qpos)

Compute chain-root FK while preserving arbitrary leading batch axes.

Parameters:

qpos (Tensor) – Joint positions with shape (..., dof) on the solver device.

Return type:

Tensor

Returns:

TCP transforms with shape (..., 4, 4).

get_ik(target_xpos, qpos_seed=None, num_samples=None, **kwargs)

Solve IK using the trained neural policy.

Parameters:

• target_xpos (Tensor) – Target pose as 4x4 matrix, shape (4,4) or (B,4,4).

• qpos_seed (Tensor | None) – Initial joint positions, shape (dof,) or (B,dof).

• num_samples (int | None) – Number of random initial seeds per target pose. Defaults to cfg.num_samples (1). When > 1, generates multiple random seeds within joint limits and returns the solution closest to qpos_seed.

• return_all_solutions – If True, return all sampled solutions with shape (B, num_samples, dof) instead of the closest.

Return type:

tuple[Tensor, Tensor]

Returns:

Tuple of (success [B], target_joints [B,1,dof] or [B,num_samples,dof]).

get_ik_batch(target_xpos, qpos_seed=None)

Compute nearest-solution IK with a uniform batch result contract.

Parameters:

• target_xpos (Tensor) – Chain-root TCP transforms with shape (..., 4, 4).

• qpos_seed (Tensor | None) – Optional seeds with matching leading axes and dof joints.

Return type:

tuple[Tensor, Tensor]

Returns:

Boolean success (...) and joint positions (..., dof). No candidate axis is exposed; concrete get_ik APIs stay unchanged.

get_ik_nearest_weight()

Gets the inverse kinematics nearest weight.

Returns:

A numpy array representing the nearest weights for inverse kinematics.

Return type:

np.ndarray

get_jacobian(qpos, locations=None, jac_type='full')

Compute the Jacobian matrix for the given joint positions.

Parameters:

• qpos (torch.Tensor) – The joint positions. Shape: (dof,) or (batch_size, dof).

• locations (torch.Tensor | np.ndarray | None) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str) – ‘full’, ‘trans’, or ‘rot’ for full, translational, or rotational Jacobian. Defaults to ‘full’.

Returns:

The Jacobian matrix. Shape:

• (batch_size, 6, dof) for ‘full’

• (batch_size, 3, dof) for ‘trans’ or ‘rot’

Return type:

torch.Tensor

get_qpos_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

• lower_qpos_limits (List[float]): The current lower limits for each joint.

• upper_qpos_limits (List[float]): The current upper limits for each joint.

Return type:

dict

get_tcp()

Returns the current TCP position.

Returns:

The current TCP position.

Return type:

np.ndarray

Raises:

ValueError – If the TCP position has not been set.

prepare_buffers(max_batch)

Reserve reusable analytic IK scratch capacity.

Parameters:

max_batch (int) – Maximum flattened target count, including IK seeds. Storage is allocated lazily by analytic backends and grows when necessary. Returned FK/IK tensors remain owned by the caller.

Return type:

None

set_ik_nearest_weight(ik_weight, joint_ids=None)

Sets the inverse kinematics nearest weight.

Parameters:

• ik_weight (np.ndarray) – A numpy array representing the nearest weights for inverse kinematics.

• joint_ids (np.ndarray, optional) – A numpy array representing the indices of the joints to which the weights apply. If None, defaults to all joint indices.

Returns:

True if the weights are set successfully, False otherwise.

Return type:

bool

set_qpos_limits(lower_qpos_limits, upper_qpos_limits)

Sets the upper and lower joint position limits.

Parameters:

• lower_qpos_limits (List[float]) – A list of lower limits for each joint.

• upper_qpos_limits (List[float]) – A list of upper limits for each joint.

Returns:

True if limits are successfully set, False if the input is invalid.

Return type:

bool

set_tcp(xpos)

Sets the TCP position with the given 4x4 homogeneous matrix.

Parameters:

xpos (np.ndarray) – The 4x4 homogeneous matrix to be set as the TCP position.

Raises:

• ValueError – If the input is not a finite 4x4 SE(3) transform.

• numpy.linalg.LinAlgError – If the transform is singular.

Return type:

None

property supports_continuous_batch_ik: bool

Whether all-candidate IK and temporally continuous selection are supported.

Supporting solvers return candidate validity shaped (M, K) and joint positions shaped (M, K, DOF) from get_ik(return_all_solutions=True). They must also implement _select_continuous_ik_path().

update_with_robot_limit(robot_qpos_limits)

Intersect solver joint limits with the robot’s effective qpos limits.

Robot-side articulation limits are the hard physical bound. Solver-specific limits from SolverCfg.user_qpos_limits may be even tighter for planning. The final solver limits must satisfy both constraints.

Parameters:

robot_qpos_limits (torch.Tensor) – [DOF, 2] tensor of joint limits from the robot data.

Seed Selection

class embodichain.lab.sim.motion.solvers.qpos_seed_sel_sampler.QposSeedSelSampler

Bases: QposSeedSampler

SELIK-style seed sampler backed by a forward-kinematics database.

The database is built lazily on the first target-aware sample() call using the joint limits supplied by the caller, and rebuilt automatically whenever those limits change. Building is a batched FK sweep and typically takes well under a second on GPU for the default database size.

Parameters:

• num_samples (int) – Number of seeds per target (including the caller seed).

• dof (int) – Degrees of freedom.

• device (device) – Target device.

• fk_fn (Callable[[Tensor], Tensor]) – Batched forward kinematics of the solved chain, in the same frame and with the same TCP as the IK targets.

• jacobian_fn (Optional[Callable[[Tensor], Tensor]]) – Optional batched Jacobian provider. When given, retrieved candidates are re-ranked by predicted joint-space step length.

• db_size (int) – Number of joint configurations stored in the database.

• rot_scale (float) – Metres-per-radian weight applied to the rotation block of the pose metric used for nearest-neighbour retrieval.

• k_max (int) – Number of nearest neighbours fetched before re-ranking.

• use_caller_seed (bool) – If True (default), slot 0 of every returned batch is the caller-provided seed; if False all slots come from the database.

• sobol_seed (int) – Scramble seed of the low-discrepancy joint sampler.

Methods:

__init__(num_samples, dof, device, *, fk_fn)
sample(qpos_seed, lower_limits, ...[, ...])	Generate joint seeds, retrieving from the database when possible.

Attributes:

database_size

Number of entries in the built database, or 0 before build.

__init__(num_samples, dof, device, *, fk_fn, jacobian_fn=None, db_size=20000, rot_scale=0.2, k_max=200, use_caller_seed=True, sobol_seed=0)

property database_size: int

Number of entries in the built database, or 0 before build.

sample(qpos_seed, lower_limits, upper_limits, batch_size, target_xpos=None)

Generate joint seeds, retrieving from the database when possible.

Parameters:

• qpos_seed (Tensor) – (batch_size, dof) or (dof,) caller seed.

• lower_limits (Tensor) – (dof,) lower joint limits.

• upper_limits (Tensor) – (dof,) upper joint limits.

• batch_size (int) – Number of targets.

• target_xpos (Tensor | None) – Optional (batch_size, 4, 4) target poses in the same frame fk_fn produces. When omitted, behaviour is identical to QposSeedSampler.

Returns:

(batch_size * num_samples, dof) joint seeds, target-major, slot 0 holding the caller seed unless use_caller_seed=False. The shape contract holds for every configuration: when retrieval returns fewer candidates than requested (database smaller than the seed count, or num_samples - 1 > k_max), the shortfall is filled with uniform random draws within the limits.

Return type:

torch.Tensor