Implementation:ARISE Initiative Robosuite IKUtils
| Knowledge Sources | |
|---|---|
| Domains | Robotics, Inverse Kinematics |
| Last Updated | 2026-02-15 07:00 GMT |
Overview
The ik_utils module provides the IKSolver class, a damped least-squares inverse kinematics solver with nullspace control that computes joint position targets from Cartesian end-effector pose goals for multi-site robot configurations.
Description
The IKSolver class implements a Jacobian-based inverse kinematics solver designed for robots with multiple end-effector sites (e.g., bimanual humanoids). It supports absolute, relative, and relative-pose action representations, with configurable rotation representations (quaternion wxyz or axis-angle) and reference frames (world, base, or mobile base).
The solver uses a damped least-squares approach: given target positions and orientations for each end-effector site, it computes the site Jacobian, formulates a twist error (position error scaled by Kpos and orientation error scaled by Kori), and solves for joint velocity increments using the formula dq = J^T (J J^T + lambda^2 I)^{-1} twist. It then adds a nullspace component dq_null = (I - J^+ J) * Kn * (q0 - q) that biases the solution toward a preferred joint configuration q0 without affecting the task-space motion.
The computed joint velocities are clipped to a maximum magnitude (max_dq for general joints and max_dq_torso for torso joints specifically), integrated to produce desired joint angles, and then clipped to the model's joint range limits. The get_targets method supports reading target poses from mocap bodies or from pre-recorded pickle files, and transform_pose handles coordinate frame conversions between world, base, and mobile base frames.
Usage
Use this solver when implementing whole-body IK controllers for humanoid or bimanual robots. Instantiate with a MuJoCo model, data, and a robot configuration dictionary specifying joint names, end-effector sites, and nullspace gains. Call solve with target actions to get desired joint positions.
Code Reference
Source Location
- Repository: ARISE_Initiative_Robosuite
- File: robosuite/utils/ik_utils.py
Signature
def get_nullspace_gains(joint_names: List[str], weight_dict: Dict[str, float]) -> np.ndarray: ...
class IKSolver:
def __init__(
self,
model: mujoco.MjModel,
data: mujoco.MjData,
robot_config: Dict,
damping: float,
integration_dt: float,
max_dq: float,
max_dq_torso: float = 0.3,
input_type: Literal["keyboard", "mocap", "pkl"] = "keyboard",
debug: bool = False,
input_action_repr: Literal["absolute", "relative", "relative_pose"] = "absolute",
input_file: Optional[str] = None,
input_rotation_repr: Literal["quat_wxyz", "axis_angle"] = "axis_angle",
input_ref_frame: Literal["world", "base", "mobilebase0_base"] = "world",
): ...
def solve(self, target_action: np.ndarray, Kpos: float = 0.95, Kori: float = 0.95) -> np.ndarray: ...
def get_targets(self) -> tuple: ...
def forward_kinematics(self, qpos: np.ndarray) -> Dict[str, np.ndarray]: ...
def transform_pose(self, src_frame_pose, src_frame, dst_frame) -> np.ndarray: ...
def action_split_indexes(self) -> Dict[str, Tuple[int, int]]: ...
def get_controller_base_pose(self) -> np.ndarray: ...
Import
from robosuite.utils.ik_utils import IKSolver, get_nullspace_gains
I/O Contract
Inputs
| Name | Type | Required | Description |
|---|---|---|---|
| model | mujoco.MjModel | Yes | MuJoCo model instance (native, not wrapped) |
| data | mujoco.MjData | Yes | MuJoCo data instance (native, not wrapped) |
| robot_config | dict | Yes | Configuration with keys: "joint_names", "end_effector_sites", "nullspace_gains", optionally "mocap_bodies", "initial_keyframe" |
| damping | float | Yes | Damping factor for the damped least-squares pseudoinverse |
| integration_dt | float | Yes | Integration timestep for joint velocity to position conversion |
| max_dq | float | Yes | Maximum joint velocity magnitude per step |
| target_action (solve) | np.ndarray | Yes | Target action array of shape (num_sites * (pos_dim + rot_dim),) |
| Kpos (solve) | float | No | Position gain (default: 0.95) |
| Kori (solve) | float | No | Orientation gain (default: 0.95) |
Outputs
| Name | Type | Description |
|---|---|---|
| q_des (solve) | np.ndarray | Desired joint positions after IK solve, clipped to joint limits |
| target_pos, target_ori (get_targets) | tuple(np.ndarray, np.ndarray) | Target positions and orientations (wxyz quats) for each site |
| forward_kinematics output | Dict[str, np.ndarray] | Site name to 3D position mapping for given joint configuration |
| action_split_indexes output | Dict[str, Tuple[int, int]] | Maps "left"/"right" to (start, end) index tuples in the action vector |
Usage Examples
import mujoco
import numpy as np
from robosuite.utils.ik_utils import IKSolver
# Load a MuJoCo model
model = mujoco.MjModel.from_xml_path("path/to/robot.xml")
data = mujoco.MjData(model)
# Define robot configuration
robot_config = {
"joint_names": ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"],
"end_effector_sites": ["right_hand_site"],
"nullspace_gains": [1.0] * 7,
}
# Create IK solver
ik_solver = IKSolver(
model=model,
data=data,
robot_config=robot_config,
damping=1e-4,
integration_dt=0.1,
max_dq=1.0,
input_action_repr="absolute",
input_rotation_repr="axis_angle",
)
# Solve for a target pose (3 pos + 3 axis-angle = 6 dims per site)
target_action = np.array([0.5, 0.0, 0.5, 0.0, 0.0, 0.0])
q_desired = ik_solver.solve(target_action, Kpos=0.95, Kori=0.95)
print(f"Desired joint positions: {q_desired}")