embodichain.lab.sim.motion.solvers

embodichain.lab.sim.motion.solvers#

Inverse-kinematics solvers sharing the BaseSolver interface.

Provides analytic (SRS, OPW, UR), 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 applied to the FK path.

Available implementations: analytic/closed-form (SRS, OPW, UR), 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

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

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#

class embodichain.lab.sim.motion.solvers.SolverCfg[source]#

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

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.

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

Bases: object

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_ik`(target_pose[, qpos_seed, num_samples])	Computes the inverse kinematics for a given target pose.
`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.
`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.

__init__(cfg=None, device=None, **kwargs)[source]#

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

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

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

abstract get_ik(target_pose, qpos_seed=None, num_samples=None, **kwargs)[source]#

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

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

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

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

Returns the current TCP position.

Returns:: The current TCP position.
Return type:: np.ndarray
Raises:: ValueError – If the TCP position has not been set.

set_ik_nearest_weight(ik_weight, joint_ids=None)[source]#

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

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

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 4x4 numpy array.

update_with_robot_limit(robot_qpos_limits)[source]#

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#

class embodichain.lab.sim.motion.solvers.PytorchSolverCfg[source]#

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`	Damping factor to prevent numerical instability
`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.
`is_only_position_constraint`	Flag to indicate whether the solver should only consider position constraints.
`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#: Damping factor to prevent numerical instability

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.

init_solver(device=device(type='cpu'), **kwargs)[source]#

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#: Flag to indicate whether the solver should only consider position constraints.

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.

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

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_ik`(target_xpos, args, *kwargs)	Computes the inverse kinematics for a given target pose.
`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.
`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)	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.

__init__(cfg, device=None, **kwargs)[source]#

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

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_ik(target_xpos, *args, **kwargs)[source]#

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

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.

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

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

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 4x4 numpy array.

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

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

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.

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

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_ik`(target_xpos[, qpos_seed, qvel_seed, ...])	Solve inverse kinematics (IK) for the robot to achieve the specified end-effector pose.
`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.
`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.

__init__(cfg, **kwargs)[source]#

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_ik(target_xpos, qpos_seed=None, qvel_seed=None, return_all_solutions=False, **kwargs)[source]#

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

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.

qpos_to_limits(q, joint_seed)[source]#

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

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

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 4x4 numpy array.

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

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

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.

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

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_ik`(target_xpos[, qpos_seed, ...])	Solve one or more target poses sequentially.
`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.
`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.

__init__(cfg, **kwargs)[source]#

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_ik(target_xpos, qpos_seed=None, return_all_solutions=False, **kwargs)[source]#

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

static reorder_array(input_array, reordering_array)[source]#

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

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

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

Parameters:: xpos (ndarray) – Homogeneous end-frame-to-TCP transform.
Return type:: None

update_null_space_joint_targets(current_qpos)[source]#

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

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

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

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.

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

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_ik`(target_xpos[, qpos_seed, ...])	Compute target joint positions using differential inverse kinematics.
`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.
`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.

__init__(cfg, num_envs=1, device='cpu', **kwargs)[source]#

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_ik(target_xpos, qpos_seed=None, return_all_solutions=False, jacobian=None, **kwargs)[source]#

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

reset(env_ids=None)[source]#

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

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 4x4 numpy array.

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

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

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.

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

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

__init__(cfg, device='cpu', **kwargs)[source]#

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

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_warp(qpos, **kwargs)[source]#

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

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

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.

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

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 4x4 numpy array.

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

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

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.

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

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_ik`(target_xpos[, qpos_seed, ...])	Compute inverse kinematics (IK) for the given target pose.
`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.
`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.

__init__(cfg, num_envs, device, **kwargs)[source]#

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_ik(target_xpos, qpos_seed=None, return_all_solutions=False, **kwargs)[source]#

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

set_ik_nearest_weight(ik_weight, joint_ids=None)[source]#

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

Set TCP and synchronize the analytical backend caches.

Return type:: None

update_with_robot_limit(robot_qpos_limits)[source]#

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#

class embodichain.lab.sim.motion.solvers.URSolverCfg[source]#

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

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.

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

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_ik`(target_xpos[, qpos_seed, ...])	Compute target joint positions using OPW inverse kinematics.
`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.
`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.

__init__(cfg, device, **kwargs)[source]#

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

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_ik(target_xpos, qpos_seed=None, return_all_solutions=False, **kwargs)[source]#

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

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

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 4x4 numpy array.

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

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.

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

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_ik`(target_xpos[, qpos_seed, num_samples])	Solve IK using the trained neural policy.
`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.
`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.

__init__(cfg, device=None, **kwargs)[source]#

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_ik(target_xpos, qpos_seed=None, num_samples=None, **kwargs)[source]#

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

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 4x4 numpy array.

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

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

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

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

`SolverCfg`	Configuration for the kinematic solver used in the robot simulation.
`BaseSolver`
`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`
`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.

embodichain.lab.sim.motion.solvers

Contents

embodichain.lab.sim.motion.solvers#

Overview#

Base Solver#

PyTorch Solver#

Pinocchio Solver#

Pink Solver#

Differential Solver#

OPW Solver#

SRS Solver#

UR Solver#

Neural IK Solver#

Seed Selection#