Quickstart: single-world IK

Quickstart: single-world IK#

This page walks through a minimal differential IK loop on the Franka Panda — the mink-warp equivalent of Mink’s getting-started example.

Setup#

Load the model and create a batched configuration with nworld=1:

import mujoco
import mink_warp as mw

model = mujoco.MjModel.from_xml_path("franka_emika_panda/mjx_scene.xml")
cfg = mw.Configuration(model, nworld=1)
cfg.update_from_keyframe("home")

Configuration wraps MuJoCo Warp state and exposes FK, Jacobians, and integration on device.

Target pose#

Use host SE3 / SO3 for targets (same composition rules as Mink):

import numpy as np
from mink_warp import SE3, SO3

ee = cfg.get_transform_frame_to_world_se3("attachment_site", "site")
target = (
    SE3.from_translation(np.array([0.0, -0.4, -0.2])) @ ee
    @ SE3.from_rotation(SO3.exp(np.array([0.0, -np.pi / 2, 0.0])))
)

Tasks#

frame = mw.FrameTask(
    "attachment_site", "site",
    position_cost=1.0, orientation_cost=1.0, gain=0.5,
)
frame.set_target(target)
posture = mw.PostureTask(model, cost=1e-2)
posture.set_target_from_configuration(cfg)

IK loop#

Each step: solve for velocity, then integrate:

dt = 1.0 / 60.0
for _ in range(120):
    v = mw.solve_ik(cfg, [frame, posture], dt)
    cfg.integrate_inplace(v, dt)

Or use a reusable solver with integration built in:

solver = mw.DLSSolver(cfg)
solver.solve_and_integrate([frame, posture], dt, iterations=120)

Complete example#

"""Single-world IK loop used in the Sphinx quickstart tutorial."""

from __future__ import annotations

import mujoco
import numpy as np

import mink_warp as mw
from mink_warp import SE3, SO3

model = mujoco.MjModel.from_xml_path("franka_emika_panda/mjx_scene.xml")
cfg = mw.Configuration(model, nworld=1)
cfg.update_from_keyframe("home")

ee_pose = cfg.get_transform_frame_to_world_se3("attachment_site", "site")
translation = np.array([0.0, -0.4, -0.2])
rotation = SO3.exp(np.array([0.0, -np.pi / 2, 0.0]))
target = SE3.from_translation(translation) @ ee_pose
target = target @ SE3.from_rotation(rotation)

duration, fps = 2.0, 60
n_frames = int(duration * fps)
gain = 1.0 - 0.01 ** (1.0 / n_frames)

frame = mw.FrameTask(
    frame_name="attachment_site",
    frame_type="site",
    position_cost=1.0,
    orientation_cost=1.0,
    gain=gain,
)
frame.set_target(target)
posture = mw.PostureTask(model, cost=1e-2)
posture.set_target_from_configuration(cfg)

dt = 1.0 / fps
for _ in range(n_frames):
    v = mw.solve_ik(cfg, [frame, posture], dt)
    cfg.integrate_inplace(v, dt)

final = cfg.get_transform_frame_to_world_se3("attachment_site", "site")
print(
    f"Position error: {np.linalg.norm(final.translation() - target.translation()):.2e} m"
)

Next steps#