curobo.kinematics module

Forward kinematics module.

Differentiable forward kinematics with Jacobian computation, center of mass calculation, and collision sphere generation.

Example

```python from curobo.kinematics import Kinematics, KinematicsCfg from curobo.types import JointState

config = KinematicsCfg.from_robot_yaml_file(“franka.yml”) kin = Kinematics(config) js = JointState.from_position(q, joint_names=kin.joint_names) state = kin.compute_kinematics(js) ```

class Kinematics(
config,
compute_jacobian=False,
compute_spheres=True,
compute_com=False,
)

Bases: object

CUDA Accelerated Robot Model

Load basic kinematics from an URDF with from_basic_urdf. Check tut_robot_configuration for details on how to also create a geometric representation of the robot. Currently dof is created only for links that we need to compute kinematics. E.g., for robots with many serial chains, add all links of the robot to get the correct dof. This is not an issue if you are loading collision spheres as that will cover the full geometry of the robot.

Initialize kinematics instance with a robot model configuration.

Parameters:

  • config (KinematicsCfg) – Input robot model configuration.

  • compute_jacobian (bool)

  • compute_spheres (bool)

  • compute_com (bool)

__init__(
config,
compute_jacobian=False,
compute_spheres=True,
compute_com=False,
)

Initialize kinematics instance with a robot model configuration.

Parameters:

  • config (KinematicsCfg) – Input robot model configuration.

  • compute_jacobian (bool)

  • compute_spheres (bool)

  • compute_com (bool)

property tool_frames: List[str]
update_batch_size(
batch,
horizon,
force_update=False,
reset_buffers=False,
)

Update batch size of the robot model.

Parameters:

  • batch (int) – Batch dimension of [batch, horizon, dof].

  • horizon (int) – Trajectory horizon dimension.

  • force_update (bool) – Detach gradients of tensors. This is not supported.

  • reset_buffers (bool) – Recreate the tensors even if the batch size is same.

compute_kinematics(
joint_state,
idxs_env=None,
)

Compute forward kinematics of the robot.

This is the single public API for forward kinematics. All external callers must use this method.

Parameters:

  • joint_state (JointState) – Joint state of robot. joint_names must be set for validation.

  • idxs_env (Optional[Tensor]) – Environment query index. Shape [num_envs], compact.

Returns:

Kinematic state of the robot.

Return type:

KinematicsState

Get meshes of all links of the robot.

Returns:

List of all link meshes.

Return type:

List[Mesh]

get_robot_as_mesh(
joint_position,
)

Transform robot links to Cartesian poses using forward kinematics and return as meshes.

Parameters:

joint_position (Tensor) – Joint configuration of the robot, shape should be [1, dof].

Returns:

List of all link meshes.

Return type:

List[Mesh]

get_robot_as_spheres(
q,
filter_valid=True,
)

Get robot spheres using forward kinematics on given joint configuration q.

Parameters:

  • q (Tensor) – Joint configuration of the robot, shape should be [1, dof].

  • filter_valid (bool) – Filter out spheres with radius <= 0.

Returns:

List of all robot spheres.

Return type:

List[Sphere]

Get Pose of links at given joint configuration using forward kinematics.

Note that only the links specified in tool_frames are returned.

Parameters:

  • joint_position (Tensor) – Joint configuration of the robot, shape should be [batch_size, dof].

  • query_link_names (List[str]) – Names of links to get pose of. This should be a subset of tool_frames.

Returns:

Poses of links at given joint configuration.

Return type:

Pose

property all_articulated_joint_names: List[str]

Names of all articulated joints of the robot.

get_self_collision_config()

Get self collision configuration parameters of the robot.

Return type:

SelfCollisionKinematicsCfg

Get mesh of a link of the robot.

Return type:

Mesh

Parameters:

link_name (str)

Get pose offset of a link from it’s parent joint.

Parameters:

link_name (str) – Name of link to get pose of.

Returns:

Pose of the link.

Return type:

Pose

Get offset pose of all links with respect to their parent joint.

Return type:

Pose

get_dof()

Get degrees of freedom of the robot.

Return type:

int

property dof: int

Degrees of freedom of the robot.

property joint_names: List[str]

Names of actuated joints.

property total_spheres: int

Number of spheres used to approximate robot geometry.

property lock_jointstate: curobo._src.state.state_joint.JointState

State of joints that are locked in the kinematic representation.

get_full_js(joint_state)

Get state of all joints, including locked joints.

This function will not provide state of mimic joints. If you need mimic joints, use get_mimic_js.

Parameters:

joint_state (JointState) – State containing articulated joints.

Returns:

State of all joints.

Return type:

JointState

get_mimic_js(joint_state)

Get state of mimic joints from active joints.

Current implementation uses a for loop over joints to calculate the state. This can be optimized by using a custom CUDA kernel or a matrix multiplication.

Parameters:

joint_state (JointState) – State containing articulated joints.

Returns:

State of active, locked, and mimic joints.

Return type:

JointState

update_kinematics_config(
new_kin_config,
)

Update kinematics representation of the robot.

A kinematics representation can be updated with new parameters. Some parameters that could require updating are state of locked joints, when a robot grasps an object. Another instance is when using different planners for different parts of the robot, example updating the state of robot base or another arm. Updations should result in the same tensor dimensions, if not then the instance of this class requires reinitialization.

Parameters:

new_kin_config (KinematicsParams) – New kinematics representation of the robot.

get_active_js(full_js)

Get joint state of active joints of the robot.

Parameters:

full_js (JointState) – Joint state of all joints.

Returns:

Joint state of active joints.

Return type:

JointState

Base link of the robot. Changing requires reinitializing this class.

property robot_spheres

Spheres representing robot geometry (config 0). Shape [num_spheres, 4].

property default_joint_position: torch.Tensor

Default joint position of the robot. Use joint_names to get joint names.

property default_joint_state: curobo._src.state.state_joint.JointState

Default joint state of the robot. Use joint_names to get joint names.

get_joint_limits()

Get joint limits of the robot.

Return type:

JointLimits

property kinematics_config: curobo._src.robot.types.kinematics_params.KinematicsParams

Kinematics configuration of the robot.

class KinematicsCfg(
device_cfg,
tool_frames,
kinematics_config,
self_collision_config=None,
kinematics_parser=None,
generator_config=None,
)

Bases: object

Configuration for robot kinematics on GPU.

Helper functions are provided to load this configuration from an URDF file or from a cuRobo robot configuration file (tut_robot_configuration). To create from a XRDF, use curobo.util.xrdf_utils.convert_xrdf_to_curobo.

Parameters:

  • device_cfg (curobo._src.types.device_cfg.DeviceCfg)

  • tool_frames (List[str])

  • kinematics_config (curobo._src.robot.types.kinematics_params.KinematicsParams)

  • self_collision_config (curobo._src.robot.types.self_collision_params.SelfCollisionKinematicsCfg | None)

  • kinematics_parser (curobo._src.robot.parser.parser_base.RobotParser | None)

  • generator_config (curobo._src.robot.loader.kinematics_loader_cfg.KinematicsLoaderCfg | None)

device_cfg: curobo._src.types.device_cfg.DeviceCfg

Device and floating point precision to use for kinematics.

tool_frames: List[str]

Names of links to compute poses with forward kinematics.

kinematics_config: curobo._src.robot.types.kinematics_params.KinematicsParams

Tensors representing kinematics of the robot. This can be created using KinematicsLoader.

self_collision_config: curobo._src.robot.types.self_collision_params.SelfCollisionKinematicsCfg | None = None

Collision pairs to ignore when computing self collision between spheres across all robot links. This also contains distance threshold between spheres pairs and which thread indices for calculating the distances. More details on computing these parameters is in _build_collision_model.

kinematics_parser: curobo._src.robot.parser.parser_base.RobotParser | None = None

Parser to load kinematics from URDF or USD files. This is used to load kinematics representation of the robot. This is created using RobotParser. USD is an experimental feature and might not work for all robots.

generator_config: curobo._src.robot.loader.kinematics_loader_cfg.KinematicsLoaderCfg | None = None

Generator config used to create this robot kinematics model.

get_joint_limits()

Get limits of actuated joints of the robot.

Returns:

Joint limits of the robot’s actuated joints.

Return type:

JointLimits

static from_basic_urdf(
urdf_path,
base_link,
tool_frames,
device_cfg=DeviceCfg(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32),
)

Load a cuda robot model from only urdf. This does not support collision queries.

Parameters:

  • urdf_path (str) – Path of urdf file.

  • base_link (str) – Name of base link.

  • tool_frames (List[str]) – List of tool frames to compute poses for. If None, all links are computed.

  • device_cfg (DeviceCfg) – Device to load robot model. Defaults to DeviceCfg().

Returns:

cuda robot model configuration.

Return type:

KinematicsCfg

static from_content_path(
content_path,
tool_frames=None,
device_cfg=DeviceCfg(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32),
**kwargs,
)

Load robot from Contentpath containing paths to robot description files.

Parameters:

  • content_path (ContentPath) – Path to robot configuration files.

  • tool_frames (Optional[List[str]]) – List of tool frames to compute poses for. If None, all links are computed.

  • device_cfg (DeviceCfg) – Device to load robot model, defaults to cuda:0.

  • kwargs (Any)

Returns:

cuda robot model configuration.

Return type:

KinematicsCfg

static from_robot_yaml_file(
file_path,
tool_frames=None,
device_cfg=DeviceCfg(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32),
urdf_path=None,
**kwargs,
)

Load robot from a yaml file that is in cuRobo’s format (tut_robot_configuration).

Parameters:

  • file_path (Union[str, Dict]) – Path to robot configuration file (yml or xrdf).

  • tool_frames (Optional[List[str]]) – List of tool frames to compute poses for. If None, all links are computed.

  • device_cfg (DeviceCfg) – Device to load robot model, defaults to cuda:0.

  • urdf_path (Optional[str]) – Path to urdf file. This is required when loading a xrdf file.

  • kwargs (Any)

Returns:

cuda robot model configuration.

Return type:

KinematicsCfg

static from_config_file(
file_path,
tool_frames=None,
device_cfg=DeviceCfg(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32),
urdf_path=None,
)

Load robot from a yaml file that is in cuRobo’s format (tut_robot_configuration).

Parameters:

  • file_path (Union[str, Dict]) – Path to robot configuration file (yml or xrdf).

  • tool_frames (Optional[List[str]]) – List of tool frames to compute poses for. If None, all links are computed.

  • device_cfg (DeviceCfg) – Device to load robot model, defaults to cuda:0.

  • urdf_path (Optional[str]) – Path to urdf file. This is required when loading a xrdf file.

Returns:

cuda robot model configuration.

Return type:

KinematicsCfg

static from_data_dict(
data_dict,
tool_frames=None,
device_cfg=DeviceCfg(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32),
)

Load robot from a dictionary containing data for KinematicsLoaderCfg.

:tut_robot_configuration discusses the data required to load a robot.

Parameters:

  • data_dict (Dict[str, Any]) – Input dictionary containing robot configuration.

  • device_cfg (DeviceCfg) – Device to load robot model, defaults to cuda:0.

  • tool_frames (List[str] | None)

Returns:

cuda robot model configuration.

Return type:

KinematicsCfg

static from_config(config)

Create a robot model configuration from a generator configuration.

Parameters:

config (KinematicsLoaderCfg) – Input robot generator configuration.

Returns:

robot model configuration.

Return type:

KinematicsCfg

property cspace: curobo._src.robot.types.cspace_params.CSpaceParams

Get cspace parameters of the robot.

property dof: int

Get the number of actuated joints (degrees of freedom) of the robot

__init__(
device_cfg,
tool_frames,
kinematics_config,
self_collision_config=None,
kinematics_parser=None,
generator_config=None,
)
Parameters:

  • device_cfg (curobo._src.types.device_cfg.DeviceCfg)

  • tool_frames (List[str])

  • kinematics_config (curobo._src.robot.types.kinematics_params.KinematicsParams)

  • self_collision_config (curobo._src.robot.types.self_collision_params.SelfCollisionKinematicsCfg | None)

  • kinematics_parser (curobo._src.robot.parser.parser_base.RobotParser | None)

  • generator_config (curobo._src.robot.loader.kinematics_loader_cfg.KinematicsLoaderCfg | None)

Return type:

None

class KinematicsState(
tool_poses=None,
tool_jacobians=None,
robot_spheres=None,
robot_com=None,
robot_collision_geometry=None,
)

Bases: object

Kinematic state of robot.

All tensor fields preserve the [batch, horizon, ...] shape from FK. No flattening is done; shapes flow end-to-end from input to output.

Parameters:
tool_poses: curobo._src.types.tool_pose.ToolPose | None = None

Link poses [batch, horizon, num_links, 3/4].

tool_jacobians: torch.Tensor | None = None

Jacobians [batch, horizon, num_links, 6, dof].

robot_spheres: torch.Tensor | None = None

Collision spheres [batch, horizon, num_spheres, 4] (x, y, z, r).

robot_com: torch.Tensor | None = None

Center of mass [batch, horizon, 4] (xyz=global CoM, w=total mass).

robot_collision_geometry: curobo._src.robot.types.collision_geometry.RobotCollisionGeometry | None = None

Static collision geometry descriptor.

property tool_frames: List[str]

Collision spheres [batch, horizon, num_spheres, 4].

Return type:

Tensor

clone()
detach()
copy_(other)
Parameters:

other (curobo._src.robot.kinematics.kinematics_state.KinematicsState)

__init__(
tool_poses=None,
tool_jacobians=None,
robot_spheres=None,
robot_com=None,
robot_collision_geometry=None,
)
Parameters:
Return type:

None