Robot

Base

class robokit.robo.robot.Robot(spec: RobotSpec, array_type: Literal['numpy'], compute_backend: Literal['pinocchio'] | None = ...)[source]
class robokit.robo.robot.Robot(spec: RobotSpec, array_type: Literal['torch'], compute_backend: Literal['torch', 'warp'] | None = ...)
class robokit.robo.robot.Robot(spec: RobotSpec, array_type: Literal['warp'], compute_backend: Literal['warp'] | None = ...)

Robot model with multiple backends.

Examples

>>> from robot_descriptions.loaders.yourdfpy import load_robot_description
>>> robot = Robot.load(load_robot_description("panda_description"), array_type="numpy")
>>> q = np.array([0.0, -1.2, 0.0, -2.0, 0.0, 1.0, 0.0, 0.0], dtype=np.float32)
>>> state = robot.state(q=q)
>>> state = robot.forward_kinematics(state)
>>> ee_pose = state.get_T_world_link(robot.link_names.index("panda_hand"))
>>> expected_pose = np.array([0.0608599, 0.0, 0.7637312, 0.0382045, 0.9192637, 0.3807715, 0.0922340])
>>> np.allclose(ee_pose.xyz_wxyz, expected_pose, atol=1e-4)
True

>>> import torch
>>> robot = Robot.load(load_robot_description("panda_description"), array_type="torch")
>>> q = torch.tensor([0.0, -1.2, 0.0, -2.0, 0.0, 1.0, 0.0, 0.0], dtype=torch.float32)
>>> state = robot.state(q=q)
>>> state = robot.forward_kinematics(state)
>>> ee_pose = state.get_T_world_link(robot.link_names.index("panda_hand"))
>>> torch.allclose(ee_pose.xyz_wxyz, torch.tensor([0.0608599, 0.0, 0.7637312, 0.0382045, 0.9192637, 0.3807715, 0.0922340], dtype=torch.float32), atol=1e-4)
True
class robokit.robo.robot.RobotState(spec: robokit.robo.robot_spec.RobotSpec)[source]

For fixed-base robots, returns only the joint Jacobian. For floating-base robots (when T_world_base is provided), returns both joint and base Jacobians.

NumpyRobot

NOTE: As of development time, pinocchio 3.7 does not handle mimic joints correctly. Therefore, mimic joints are processed manually in this file, with most logic implemented in numpy/python. This reduces efficiency (potentially 5x slower than pure pinocchio functions).

class robokit.robo.numpy_robot.NumpyRobot(spec: RobotSpec, array_type: Literal['numpy'], compute_backend: Literal['pinocchio'] | None = ...)[source]
class robokit.robo.numpy_robot.NumpyRobot(spec: RobotSpec, array_type: Literal['torch'], compute_backend: Literal['torch', 'warp'] | None = ...)
class robokit.robo.numpy_robot.NumpyRobot(spec: RobotSpec, array_type: Literal['warp'], compute_backend: Literal['warp'] | None = ...)
build_inverse_kinematics_optimizer(frame_names: str | Sequence[str], T_world_target: PinocchioSE3 | Sequence[PinocchioSE3], position_weight: float = 1.0, orientation_weight: float = 0.2, limit_weight: float = 2.0, optimizer_config: NumpyOptimizerConfig | None = None) → NumpyOptimizer[source]

Build an inverse kinematics optimizer for the robot.

Example

>>> from robokit.lie.pinocchio_se3 import PinocchioSE3
>>> from robot_descriptions.loaders.yourdfpy import load_robot_description
>>> robot = Robot.load(load_robot_description("ur10_description"), array_type="numpy")
>>> target_pose = PinocchioSE3(np.array([0.5, 0.3, 0.4, 1.0, 0.0, 0.0, 0.0]))
>>> ik_optimizer = robot.build_inverse_kinematics_optimizer("ee_link", target_pose)
>>> state = robot.state(q=robot.zero_q)
>>> state = ik_optimizer.solve([state])[0]
>>> state = robot.forward_kinematics(state)
>>> achieved_pose = state.get_T_world_link(robot.link_names.index("ee_link"))
>>> np.allclose(achieved_pose.xyz, target_pose.xyz, atol=1e-3)
True
>>> np.allclose(achieved_pose.quat_wxyz, target_pose.quat_wxyz, atol=1e-3)
True
compute_motion_subspace(state: NumpyRobotState) → NumpyRobotState[source]

Compute the joint Jacobians for the robot and store them in the state.

After calling this method, use state.get_link_jacobian() to retrieve Jacobians for specific links. When a floating base is provided, the returned Jacobian will have J_base prepended to J_joints (shape: [6, 6 + num_dofs]).

Example

>>> from robokit.lie.pinocchio_se3 import PinocchioSE3
>>> from robot_descriptions.loaders.yourdfpy import load_robot_description
>>> robot = NumpyRobot.load(load_robot_description("panda_description"), array_type="numpy")
>>> T_world_base = PinocchioSE3(np.array([1.0, 2.0, 3.0, 0.7071, 0.0, 0.7071, 0.0]))
>>> q = np.array([0.0, -1.2, 0.0, -2.0, 0.0, 1.0, 0.0, 0.0], dtype=np.float32)
>>> state = robot.state(q=q, T_world_base=T_world_base)
>>> state = robot.compute_motion_subspace(state)
>>> J = state.get_link_jacobian(robot.link_names.index("panda_hand"), "body")
>>> np.allclose(J[0, :6], np.array([0.693, 0.7071, 0.1405, -0.54, 0.5207, 0.043]), atol=1e-4)
True
>>> J = state.get_link_jacobian(robot.link_names.index("panda_hand"), "spatial")
>>> np.allclose(J[0, 6:], np.array([0.0001, -3., 1.8641, 3.2647, -1.4346, 3.0467, -0.3974, 0.]), atol=1e-3)
True
>>> np.allclose(J[0, :6], np.array([0.0001, 0., 1., -2., -3., 0.0001]), atol=1e-3)
True
forward_kinematics(state: NumpyRobotState) → NumpyRobotState[source]

Compute the forward kinematics of the robot for given joint positions statelessly. Returns a robot state containing the computed transforms of all links.

Example

>>> from robokit.lie.pinocchio_se3 import PinocchioSE3
>>> from robot_descriptions.loaders.yourdfpy import load_robot_description
>>> robot = Robot.load(load_robot_description("panda_description"), array_type="numpy")
>>> q = np.array([0.0, -1.2, 0.0, -2.0, 0.0, 1.0, 0.0, 0.0], dtype=np.float32)
>>> state = robot.state(q=q)
>>> state = robot.forward_kinematics(state)
>>> link_pose = state.get_T_world_link(robot.link_names.index("panda_hand"))
>>> expected_pose = PinocchioSE3(np.array([0.0608599, 0.0, 0.7637312, 0.0382045, 0.9192637, 0.3807715, 0.0922340]))
>>> np.allclose(link_pose.as_matrix(), expected_pose.as_matrix(), atol=1e-4)
True
>>> T_world_base = PinocchioSE3(np.array([1.0, 2.0, 3.0, 0.7071, 0.0, 0.7071, 0.0]))
>>> state = robot.state(q=q, T_world_base=T_world_base)
>>> state = robot.forward_kinematics(state)
>>> link_pose = state.get_T_world_link(robot.link_names.index("panda_hand"))
>>> expected_pose = PinocchioSE3(np.array([1.76371783, 2.0, 2.93915576, -0.24222432, 0.71524162, 0.29626278, -0.58478125]))
>>> np.allclose(link_pose.as_matrix(), expected_pose.as_matrix(), atol=1e-4)
True
class robokit.robo.numpy_robot.NumpyRobotState(spec: robokit.robo.robot_spec.RobotSpec, _pin_model: pinocchio.pinocchio_pywrap_default.Model, _pin_data: pinocchio.pinocchio_pywrap_default.Data, _pin_aux: robokit.robo.numpy_robot.PinocchioAuxiliaryMapping, _pin_q: numpy.ndarray | None = None, _q: numpy.ndarray | None = None, _T_world_base: robokit.lie.pinocchio_se3.PinocchioSE3 | None = None)[source]

For fixed-base robots, returns only the joint Jacobian. For floating-base robots (when T_world_base is provided), returns both joint and base Jacobians.

get_motion_subspace() → Float[ndarray, '6 num_joints'][source]

Returns the stack of all motion subspace expressed in the world frame.

class robokit.robo.numpy_robot.PinocchioAuxiliaryMapping(pin_link_frame_ids: List[int], pin_to_spec_actuated_joint_indices: numpy.ndarray | None = None, pin_to_spec_nonfixed_joint_indices: numpy.ndarray | None = None, pin_mimic_multipliers: numpy.ndarray | None = None, pin_mimic_offsets: numpy.ndarray | None = None, pin_to_spec_jac_map: numpy.ndarray | None = None)[source]

Frame IDs in the Pinocchio model.

pin_mimic_multipliers: ndarray | None = None

Multipliers for mimic joints in Pinocchio ordering.

pin_mimic_offsets: ndarray | None = None

Offsets for mimic joints in Pinocchio ordering.

pin_to_spec_actuated_joint_indices: ndarray | None = None

Mapping from Pinocchio actuated joint indices to spec actuated joint indices.

pin_to_spec_jac_map: ndarray | None = None

Mapping matrix for converting Jacobians from Pinocchio to spec ordering.

pin_to_spec_nonfixed_joint_indices: ndarray | None = None

Mapping from Pinocchio actuated joint indices to spec non-fixed joint indices.

TorchRobot

WarpRobot