embodichain.lab.sim.solvers

Classes

SolverCfg	Configuration for the kinematic solver used in the robot simulation.
BaseSolver
PytorchSolverCfg	Configuration for the pytorch kinematics solver used in the robot simulation.
PytorchSolver
PinocchioSolverCfg	PinocchioSolverCfg(class_type: str = <factory>, urdf_path: Optional[str] = <factory>, joint_names: Optional[list[str]] = <factory>, end_link_name: str = <factory>, root_link_name: str = <factory>, tcp: Union[torch.Tensor, numpy.ndarray] = <factory>, ik_nearest_weight: Optional[List[float]] = <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	Configuration for Pink IK Solver.
PinkSolver	Standalone implementation of Pink IK Solver.
DifferentialSolverCfg	Configuration for differential inverse kinematics controller.
DifferentialSolver	Differential inverse kinematics (IK) controller.
OPWSolverCfg	Configuration for OPW inverse kinematics controller.
OPWSolver	OPW inverse kinematics (IK) controller.

Base Solver

class embodichain.lab.sim.solvers.SolverCfg

Configuration for the kinematic solver used in the robot simulation.

Attributes:

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

Methods:

from_dict(init_dict)

Initialize the configuration from a dictionary.

class_type: str

The class type of the solver to be used.

end_link_name: str

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

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

classmethod from_dict(init_dict)

Initialize the configuration from a dictionary.

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: Optional[list[str]]

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: Union[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: Optional[str]

The file path to the URDF model of the robot.

class embodichain.lab.sim.solvers.BaseSolver

Bases: object

Methods:

__init__([cfg, device])	Initializes the kinematics solver with a robot model.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_ik(target_pose[, joint_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_position_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_position_limits(lower_position_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Sets the TCP position with the given 4x4 homogeneous matrix.

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

Initializes the kinematics solver with a robot model.

Parameters:

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

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

• **kwargs – Additional keyword arguments for customization.

get_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

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

Computes the inverse kinematics for a given target pose.

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

Parameters:

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

• joint_seed (Optional[torch.Tensor]) – The initial joint positions used as a seed.

• num_samples (Optional[int]) – 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_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 (Optional[torch.Tensor]) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str, optional) – ‘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_position_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

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

• upper_position_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_position_limits(lower_position_limits, upper_position_limits)

Sets the upper and lower joint position limits.

Parameters:

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

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

PyTorch Solver

class embodichain.lab.sim.solvers.PytorchSolverCfg

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

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

Attributes:

class_type	The class type of the solver to be used.
damp	Damping factor to prevent numerical instability
dt	Time step for numerical integration
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
tcp	The tool center point (TCP) position as a 4x4 homogeneous matrix.
urdf_path	The file path to the URDF model of the robot.

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

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(device=device(type='cpu'), **kwargs)

Initialize the solver with the configuration.

Parameters:

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

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

Returns:

An initialized solver instance.

Return type:

PytorchSolver

is_only_position_constraint: bool

Flag to indicate whether the solver should only consider position constraints.

joint_names: Optional[list[str]]

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

tcp: Union[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: Optional[str]

The file path to the URDF model of the robot.

class embodichain.lab.sim.solvers.PytorchSolver

Bases: BaseSolver

Methods:

__init__(cfg[, device])	Initializes the PyTorch kinematics solver.
get_all_fk(qpos)	Get the forward kinematics for all links from root to end link.
get_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_position_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_position_limits(lower_position_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Sets the TCP position with the given 4x4 homogeneous matrix.

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

Initializes the PyTorch kinematics solver.

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

Parameters:

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

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

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

get_all_fk(qpos)

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

Parameters:

qpos (torch.Tensor) – The joint positions.

Returns:

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

Return type:

list

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

Computes the inverse kinematics for a given target pose.

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

Parameters:

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

• joint_seed (Optional[torch.Tensor]) – The initial joint positions used as a seed.

• num_samples (Optional[int]) – 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()

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 (Optional[torch.Tensor]) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str, optional) – ‘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_position_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

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

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

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_position_limits(lower_position_limits, upper_position_limits)

Sets the upper and lower joint position limits.

Parameters:

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

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

Pinocchio Solver

class embodichain.lab.sim.solvers.PinocchioSolverCfg

PinocchioSolverCfg(class_type: str = <factory>, urdf_path: Optional[str] = <factory>, joint_names: Optional[list[str]] = <factory>, end_link_name: str = <factory>, root_link_name: str = <factory>, tcp: Union[torch.Tensor, numpy.ndarray] = <factory>, ik_nearest_weight: Optional[List[float]] = <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.

Methods:

init_solver(**kwargs)

Initialize the solver with the configuration.

class_type: str

The class type of the solver to be used.

end_link_name: str

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

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

ik_nearest_weight: Optional[List[float]]

Weights for the inverse kinematics nearest calculation.

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

init_solver(**kwargs)

Initialize the solver with the configuration.

Parameters:

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

Returns:

An initialized solver instance.

Return type:

PinocchioSolver

joint_names: Optional[list[str]]

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: Union[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: Optional[str]

The file path to the URDF model of the robot.

class embodichain.lab.sim.solvers.PinocchioSolver

Bases: BaseSolver

Methods:

__init__(cfg, **kwargs)	Initializes the kinematics solver with a robot model.
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_position_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_position_limits(lower_position_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(tcp)	Sets the TCP position with the given 4x4 homogeneous matrix.

__init__(cfg, **kwargs)

Initializes the kinematics solver with a robot model.

Parameters:

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

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

• **kwargs – Additional keyword arguments for customization.

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

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

Parameters:

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

• qpos_seed (np.ndarray, optional) – Initial joint positions used as the seed for optimization. If None, uses zero configuration.

• qvel_seed (np.ndarray, optional) – 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()

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 (Optional[torch.Tensor]) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str, optional) – ‘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_position_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

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

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

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

Parameters:

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

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

Returns:

The adjusted joint positions within the specified limits.

Return type:

np.ndarray

set_ik_nearest_weight(ik_weight, joint_ids=None)

Sets the inverse kinematics nearest weight.

Parameters:

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

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

Returns:

True if the weights are set successfully, False otherwise.

Return type:

bool

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

Sets the iteration parameters for the kinematics solver.

Parameters:

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

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

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

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

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

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

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

Returns:

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

Return type:

bool

set_position_limits(lower_position_limits, upper_position_limits)

Sets the upper and lower joint position limits.

Parameters:

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

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

Returns:

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

Return type:

bool

set_tcp(tcp)

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

Parameters:

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

Raises:

ValueError – If the input is not a 4x4 numpy array.

Pink Solver

class embodichain.lab.sim.solvers.PinkSolverCfg

Configuration for Pink IK Solver.

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.

Methods:

init_solver(**kwargs)

Initialize the solver with the configuration.

class_type: str

The class type of the solver to be used.

end_link_name: str

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

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

ik_nearest_weight: Optional[List[float]]

Weights for the inverse kinematics nearest calculation.

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

init_solver(**kwargs)

Initialize the solver with the configuration.

Parameters:

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

Returns:

An initialized solver instance.

Return type:

PinkSolver

joint_names: Optional[list[str]]

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: Union[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: Optional[str]

The file path to the URDF model of the robot.

class embodichain.lab.sim.solvers.PinkSolver

Bases: BaseSolver

Standalone implementation of Pink IK Solver.

Methods:

__init__(cfg, **kwargs)	Initialize the solver with the configuration.
get_fk(qpos, **kwargs)	Computes the forward kinematics for the end-effector link.
get_ik(target_xpos[, qpos_seed, ...])	Compute target joint positions using 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_position_limits()	Returns the current joint position limits.
get_tcp()	Returns the current TCP position.
reorder_array(input_array, reordering_array)	Reorder array elements based on provided indices.
set_ik_nearest_weight(ik_weight[, joint_ids])	Sets the inverse kinematics nearest weight.
set_position_limits(lower_position_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Sets the TCP position with the given 4x4 homogeneous matrix.
update_null_space_joint_targets(current_qpos)	Update the null space joint targets.

__init__(cfg, **kwargs)

Initialize the solver with the configuration.

Parameters:

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

Returns:

An initialized solver instance.

Return type:

PinkSolver

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)

Compute target joint positions using inverse kinematics.

Parameters:

• target_pose (Optional[Union[torch.Tensor, np.ndarray]]) – Target end-effector pose

• qpos_seed (Optional[Union[torch.Tensor, np.ndarray]]) – Seed joint positions

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

Return type:

Tuple[Tensor, Tensor]

Returns:

Target joint positions. (n_sample, 1, dof) of float.

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 (Optional[torch.Tensor]) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str, optional) – ‘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_position_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

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

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

reorder_array(input_array, reordering_array)

Reorder array elements based on provided indices.

Parameters:

• input_array (List[float]) – Array to reorder

• reordering_array (List[int]) – Indices for reordering

Return type:

List[float]

Returns:

Reordered 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_position_limits(lower_position_limits, upper_position_limits)

Sets the upper and lower joint position limits.

Parameters:

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

• upper_position_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_null_space_joint_targets(current_qpos)

Update the null space joint targets.

This method updates the target joint positions for null space posture tasks based on the current joint configuration. This is useful for maintaining desired joint configurations when the primary task allows redundancy.

Parameters:

current_qpos (ndarray) – The current joint positions of shape (num_joints,).

Differential Solver

class embodichain.lab.sim.solvers.DifferentialSolverCfg

Configuration for differential inverse kinematics controller.

Attributes:

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

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: 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(num_envs=1, device=device(type='cpu'), **kwargs)

Initialize the solver with the configuration.

Parameters:

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

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

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

Returns:

An initialized solver instance.

Return type:

DifferentialSolver

joint_names: Optional[list[str]]

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: Union[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: Optional[str]

The file path to the URDF model of the robot.

class embodichain.lab.sim.solvers.DifferentialSolver

Bases: BaseSolver

Differential inverse kinematics (IK) controller.

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

Methods:

__init__(cfg[, num_envs, device])	Initializes the differential kinematics solver.
get_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_position_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_position_limits(lower_position_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Sets the TCP position with the given 4x4 homogeneous matrix.

Attributes:

action_dim

Returns the dimension of the controller's input command.

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

Initializes the differential kinematics solver.

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

Parameters:

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

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

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

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

property action_dim: int

Returns the dimension of the controller’s input command.

Returns:

The dimension of the input command.

Return type:

int

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

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 (Optional[torch.Tensor]) – The offset points (relative to the end-effector coordinate system). Shape: (batch_size, 3) or (3,) for a single offset.

• jac_type (str, optional) – ‘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_position_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

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

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

Reset the internal buffers for the specified environments.

Parameters:

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

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

Set the target end-effector pose command.

Parameters:

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

• ee_pos (Optional[torch.Tensor]) – Current end-effector position (for relative mode).

• ee_quat (Optional[torch.Tensor]) – 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_position_limits(lower_position_limits, upper_position_limits)

Sets the upper and lower joint position limits.

Parameters:

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

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

OPW Solver

class embodichain.lab.sim.solvers.OPWSolverCfg

Configuration for OPW inverse kinematics controller.

Attributes:

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

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: 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(device=device(type='cpu'), **kwargs)

Initialize the solver with the configuration.

Parameters:

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

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

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

Returns:

An initialized solver instance.

Return type:

OPWSolver

joint_names: Optional[list[str]]

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: Union[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: Optional[str]

The file path to the URDF model of the robot.

class embodichain.lab.sim.solvers.OPWSolver

Bases: BaseSolver

OPW inverse kinematics (IK) controller.

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

Methods:

__init__(cfg[, device])	Initializes the OPW kinematics solver.
get_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_py_opw(target_xpos, qpos_seed[, ...])	Compute target joint positions using OPW inverse kinematics.
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_position_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_position_limits(lower_position_limits, ...)	Sets the upper and lower joint position limits.
set_tcp(xpos)	Sets the TCP position with the given 4x4 homogeneous matrix.

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

Initializes the OPW kinematics solver.

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

Parameters:

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

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

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

get_fk(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

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

• **kwargs – Additional keyword arguments for customization.

Returns:

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

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

Return type:

torch.Tensor

get_fk_warp(qpos, **kwargs)

Computes the forward kinematics for the end-effector link.

Parameters:

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

• **kwargs – Additional keyword arguments for customization.

Returns:

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

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

Return type:

torch.Tensor

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

Compute target joint positions using OPW inverse kinematics.

Parameters:

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

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

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

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

Returns:

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

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

Return type:

Tuple[torch.Tensor, torch.Tensor]

get_ik_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_py_opw(target_xpos, qpos_seed, return_all_solutions=False, **kwargs)

Compute target joint positions using OPW inverse kinematics.

Parameters:

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

• 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, 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_warp(target_xpos, qpos_seed, return_all_solutions=False, **kwargs)

Compute target joint positions using OPW inverse kinematics.

Parameters:

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

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

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

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

Returns:

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

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

Return type:

Tuple[torch.Tensor, torch.Tensor]

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

Compute the Jacobian matrix for the given joint positions.

Parameters:

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

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

• jac_type (str, optional) – ‘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_position_limits()

Returns the current joint position limits.

Returns:

A dictionary containing:

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

• upper_position_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_position_limits(lower_position_limits, upper_position_limits)

Sets the upper and lower joint position limits.

Parameters:

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

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