Advanced and parallel IK

The inverse kinematics (IK) solver introduced in Inverse kinematics and motion planning extends in two directions that matter for real manipulation and for training at scale: solving for several end-effector links at once, and solving every environment of a parallel scene in a single call. This page covers both.

The runnable sources for this page are advanced_IK_multilink.py and batched_IK.py.

IK with multiple end-effector links

A single call to robot.inverse_kinematics() positions one link. When a task constrains several links at once (a gripper’s two fingertips, both hands of a dual-arm robot, or a closed kinematic loop), solve them jointly with robot.inverse_kinematics_multilink(). Solving each link separately would let later solutions undo earlier ones; the multi-link solver finds one joint configuration that satisfies every target together.

The call takes parallel lists: one target link, one position, and one orientation per entry.

left_finger = robot.get_link("left_finger")
right_finger = robot.get_link("right_finger")

q = robot.inverse_kinematics_multilink(
    links=[left_finger, right_finger],
    poss=[target_pos_left, target_pos_right],
    quats=[target_quat, target_quat],
    rot_mask=[False, False, True],  # only restrict direction of z-axis
)

Each target need not be a full 6-DoF pose. pos_mask and rot_mask are length-3 boolean masks that select which position axes and which rotation axes the solver has to satisfy; both default to [True, True, True]. Here rot_mask=[False, False, True] asks only that each fingertip’s z-axis align with the z-axis of target_quat, leaving its heading in the horizontal plane free. Masking out constraints you do not care about gives the solver more freedom and makes it more likely to converge.

Orientations follow the (w, x, y, z) quaternion convention, so target_quat = np.array([0, 1, 0, 0]) points the finger’s z-axis straight down.

This example is purely kinematic: it demonstrates the solver, not the physics. After solving, it writes the result directly to the robot and refreshes the display rather than stepping the simulation:

# Note that this IK is for visualization purposes, so here we do not call scene.step(), but only update the state and the visualizer
# In actual control applications, you should instead use robot.control_dofs_position() and scene.step()
robot.set_dofs_position(q)
scene.visualizer.update()

The target frames in the video are drawn with scene.draw_debug_frame() and moved each iteration with scene.update_debug_objects(). These markers live at the visualizer level and take no part in the simulation. See Interactive GUI and debugging for the debug-drawing API.

IK across parallel environments

When a scene is built with multiple environments, the same solver runs across all of them in one call, useful for generating demonstrations or driving policy rollouts without a Python loop over environments. The example spawns 16 environments and rotates each robot’s end-effector at a different angular speed.

The only structural change from single-environment IK is the batch dimension. Build the scene with n_envs, then give the solver batched targets whose leading dimension is n_envs:

n_envs = 16
scene.build(n_envs=n_envs, env_spacing=(1.0, 1.0))

target_quat = np.tile(np.array([0, 1, 0, 0]), [n_envs, 1])  # (n_envs, 4), pointing downwards
center = np.tile(np.array([0.4, -0.2, 0.25]), [n_envs, 1])
angular_speed = np.random.uniform(-10, 10, n_envs)
r = 0.1
ee_link = robot.get_link("hand")

Inside the loop, pos and quat carry the batch dimension and the solver returns one configuration per environment:

target_pos = np.zeros([n_envs, 3])
target_pos[:, 0] = center[:, 0] + np.cos(i / 360 * np.pi * angular_speed) * r
target_pos[:, 1] = center[:, 1] + np.sin(i / 360 * np.pi * angular_speed) * r
target_pos[:, 2] = center[:, 2]

q = robot.inverse_kinematics(
    link=ee_link,
    pos=target_pos,  # shape (n_envs, 3)
    quat=target_quat,  # shape (n_envs, 4)
    rot_mask=[False, False, True],  # for demo purpose: only restrict direction of z-axis
)

robot.set_qpos(q)
scene.step()

The returned q has shape ([n_envs,] n_dofs): batched when the scene has multiple environments, a plain (n_dofs,) vector otherwise. The same rule applies to the inputs: pos is ([n_envs,] 3) and quat is ([n_envs,] 4). The masks stay length-3 and are shared across the batch. inverse_kinematics_multilink batches the same way, with each entry of poss/quats carrying the leading n_envs dimension.

Tip

Parallel IK is most effective on a GPU backend, where all environments are solved together. See Parallel simulation for how batched environments run, and Control your robot for driving the solved configuration through the controllers instead of setting it directly.

Forward kinematics and the Jacobian

Inverse kinematics is built on two lower-level operations the same solver exposes directly, and both are useful on their own for analytic control. Forward kinematics is IK’s inverse: it maps a joint configuration to the resulting link poses. The Jacobian relates joint velocities to the end-effector’s spatial velocity, which is the quantity IK differentiates to take each step; it also underpins Jacobian-transpose control and manipulability analysis.

Both require an entity created with requires_jac_and_IK=True, which is the default for the MJCF and URDF robot morphs.

ee_link = robot.get_link("hand")

# Jacobian at the link origin: rows 0-2 are linear, rows 3-5 angular, one column per dof.
jacobian = robot.get_jacobian(link=ee_link)  # shape ([n_envs,] 6, n_dofs)

# Forward kinematics: joint configuration -> world-frame link poses.
qpos = robot.get_qpos()
links_pos, links_quat = robot.forward_kinematics(qpos)
# links_pos:  shape ([n_envs,] n_links, 3)   link-frame origins, world coordinates
# links_quat: shape ([n_envs,] n_links, 4)   orientations, (w, x, y, z)

get_jacobian takes an optional local_point (a length-3 point in the link’s local frame) to evaluate the Jacobian somewhere other than the link origin. forward_kinematics accepts qs_idx_local, links_idx_local, and envs_idx to compute a subset of the configuration, links, or environments; the returned poses are in the world frame, consistent with the world-frame qpos input.

Note

forward_kinematics reuses the IK solver’s internal buffers, which are allocated the first time you call inverse_kinematics on the entity. Solve IK at least once before calling it standalone.