Support Closed-loop IK in Isaac Teleop as a retargeter

Type: Feature / RFC
Area: Isaac Teleop (isaaclab_teleop, isaacteleop), Controllers (isaaclab.controllers)
Related robot: SO-101 (Isaac-Stack-Cube-SO101-IK-Abs-v0, Isaac-Stack-Cube-SO101-v0)

Summary

Add a closed-loop inverse-kinematics retargeter to Isaac Teleop: a pipeline node that converts an
XR controller target pose into robot joint targets by running IK against a self-contained
kinematic model, seeded each step from the measured joint state. The goal is one IK
implementation reused across simulation and real-hardware teleoperation, rather than today's split
where IK only exists as a simulation-only environment action term.

This requires one new framework capability that does not exist yet: a way to feed the robot's
current joint positions (and, for a model that needs it, the model-derived Jacobian) back into a
retargeter each step.

Existing Implementation

• Close loop IK lives in Isaac Lab only (left)
• An open loop IK can be easily implemented in Isaac Teleop (right), but a closed loop IK will require a new API to pass robot joint states back into Isaac Teleop.

Motivation

Today IK for teleop lives in the environment action term
(DifferentialInverseKinematicsAction): the retargeter emits a task-space EE pose and the env solves
IK inside env.step(). This is clean and robust in simulation because it reads the ground-truth
Jacobian straight from the physics engine (PhysX get_jacobians()), evaluated at the true current
configuration, with zero duplicated kinematics.

But that path cannot be reused on real hardware: there is no env.step() and no simulator
Jacobian on a physical SO-101. As a result, a real-robot deployment would need a second,
independently written IK path — exactly the duplication we want to avoid. Driving the IK from a
retargeter that owns a portable kinematic model lets the same retargeting artifact run in sim and on
hardware, which also gives:

• Sim/real parity — identical joint commands from identical controller input.
• Joint-space deterministic replay — recorded demonstrations replay from the controller stream
  alone, independent of physics gains / solver iteration counts / dt.
• A single place to maintain IK behavior for SO-101 teleop.

Background — where the "ground-truth Jacobian" comes from

The differential IK controller solves Δq = J⁺ · (x_des − x_curr), then returns q_curr + Δq
(isaaclab/controllers/differential_ik.py:149-175, 198-237). It needs four things each step:
the desired pose, the current EE pose, the current joint positions, and the Jacobian
J(q) — the geometry-derived ∂FK/∂q evaluated at the current configuration.

• In simulation, J is read from the engine:
  task_space_actions.py:140-141 → Articulation.data.body_link_jacobian_w →
  (PhysX backend) a passthrough of _root_view.get_jacobians()
  (isaaclab_physx/.../articulation_data.py:881-899; contract in
  base_articulation_data.py:677-700). It is "ground truth" because PhysX already maintains the exact
  kinematic tree of the simulated articulation, so there is no separate model and no drift — but it
  only exists in sim.
• Off-sim / for reuse, the Jacobian must instead come from a portable kinematic model: a URDF
  parsed into forward kinematics and differentiated. Isaac Lab already does this for PinkIKController,
  which builds a Pinocchio model from URDF (or converts USD→URDF) and computes the Jacobian itself,
  independent of the simulator:
  • controllers/pink_ik/pink_kinematics_configuration.py:
    RobotWrapper.BuildFromURDF(...), pin.computeJointJacobians(model, data, q),
    pin.getFrameJacobian(...).
  • controllers/pink_ik/pink_ik.py:85-102 (URDF/USD resolution),
    compute(curr_joint_pos, ...) → pink_configuration.update(q) — i.e. it is fed the measured
    current joint positions every step, and is consumed as an env action term
    (envs/mdp/actions/pink_task_space_actions.py).

So a model-based Jacobian needs (1) the model (URDF → J(q) function) and (2) the current
configuration q to evaluate it at — from the articulation in sim, from joint encoders on hardware.
"Closed-loop" means feeding (2) back into the solver each step.

Note for SO-101 specifically: position-only IK on a 5-DOF arm has a clean closed-form analytic
solution (shoulder_pan = atan2(y, x) + planar 3R law-of-cosines) that needs no Jacobian at all.
That remains the simplest option for SO-101 and should be offered as a non-iterative solver behind the
same retargeter interface. The closed-loop / Jacobian path in this ticket is the general capability
(needed for redundant/higher-DOF arms and for differential tracking), and the framework feedback
channel it requires is the load-bearing new piece either way.

Problem statement — the framework gap

Retargeters are leaf nodes: _compute_fn(inputs, outputs, context) sees only its declared
inputs plus a ComputeContext (graph_time, execution_events), with no back-reference to the
env/articulation (base_retargeter.py). input_spec() only exposes device/controller input types —
there is no channel for robot joint state or a Jacobian.

The single existing ingress for sim-derived tensors is a ValueInput leaf, filled by
isaaclab_teleop/session_lifecycle.py:_build_external_inputs() — which is hard-coded to recognize
only world_T_anchor; any other external leaf logs "IsaacTeleopDevice does not know how to provide
its inputs" and is dropped (session_lifecycle.py:778-793).

Therefore, closed-loop IK in a retargeter is not expressible today: there is no supported way to
deliver the measured joint positions (or a sim Jacobian) into a retargeter each step.

Proposed approach

1. Joint-state feedback channel (the core new capability)

Add a first-class, well-known external input that delivers the controlled robot's current joint
positions to the pipeline each step, analogous to how world_T_anchor is delivered today:

• Define a well-known leaf name (e.g. robot_joint_state) and a tensor type for an ordered joint
  vector.
• Extend IsaacTeleopDevice / session_lifecycle._build_external_inputs() to populate it from the
  articulation each advance() in sim (Articulation.data.joint_pos for the controlled joints, in a
  documented joint order).
• Document the one-step latency: the joint state delivered at step t reflects step t−1
  (pipelined execution). For teleop rates and small per-frame motion this is acceptable; call it out
  explicitly.
• Keep the channel optional: stateless / closed-form retargeters that don't need feedback simply
  don't declare it.

On real hardware, the same leaf is populated from joint encoders by the hardware integration layer —
the retargeter code is unchanged.

Scope decision to make in review: whether to also support delivering a sim Jacobian over a similar
channel (cheap in sim, undefined on hardware), or to always compute the Jacobian from the
retargeter's own model (uniform sim/real, at the cost of carrying the model). Recommendation:
model-derived Jacobian for sim/real uniformity; the joint-state channel is the only mandatory new
input.

2. Closed-loop IK retargeter

A new retargeter (e.g. SO101IkRetargeter) that:

• Declares inputs: the transformed controller grip pose (target) and the optional
  robot_joint_state feedback.
• Owns a portable kinematic model of the controlled chain — reuse the PinkKinematicsConfiguration
  / Pinocchio pattern, or a hand-verified analytic model for simple arms — sourced from the same
  URDF/USD the env loads, so there is a single source of truth.
• Computes FK + Jacobian from the model at the fed-back q, runs differential IK
  (reusing DifferentialIKController math where practical), and emits joint targets that drive the
  joint-position task (Isaac-Stack-Cube-SO101-v0) — no env-side IK action term.
• Offers a stateless closed-form solver variant for SO-101 position-only IK (no Jacobian, no
  feedback required) behind the same output contract.

3. Frame contract

Solve in the robot base frame: set target_frame_prim_path to the robot base
(isaac_teleop_cfg.py:154-173) so the controller grip pose arrives in the frame the kinematic model is
rooted in. Target the same EE body the env IK targets (gripper link) for parity.

Acceptance criteria

• A retargeter can declare a dependency on the controlled robot's current joint positions, and the
  device populates it each step in sim (from the articulation) with a documented joint order and
  latency.
• A closed-loop IK retargeter drives Isaac-Stack-Cube-SO101-v0 end-to-end in sim, producing
  stacking behavior comparable to Isaac-Stack-Cube-SO101-IK-Abs-v0.
• The Jacobian/FK used by the retargeter is model-derived (URDF/Pinocchio or analytic),
  requiring no physics-engine call — i.e. the same code path is viable on hardware.
• A sim-free conformance test asserts the retargeter's FK matches the articulation FK across
  sampled configurations, and that joint names/order/limits match the USD/SO101_CFG. (This is the
  guard against model-vs-USD drift.)
• No change to the existing IK-Abs task's behavior; the new path is additive and opt-in.

Non-goals

• Replacing or deprecating Isaac-Stack-Cube-SO101-IK-Abs-v0 — it remains the recommended path when an
  env.step() is available.
• Full 6-DOF orientation tracking on the 5-DOF SO-101 (position-only + direct wrist_roll, as today).
• Real-hardware driver integration itself — this ticket establishes the sim-side capability and the
  reusable IK; the hardware encoder→robot_joint_state adapter is a follow-up.
• A new heavyweight dependency where avoidable: for SO-101 prefer analytic IK; only pull
  Pinocchio/Pink when a general model-based Jacobian is actually required.

Risks & open questions

• Model ↔ USD ↔ real calibration drift. The retargeter now owns a kinematic model that must match
  both the sim USD and the physical arm. Mitigation: single-source the model from the USD/URDF and gate
  CI on the FK-conformance test.
• Feedback latency / cadence. Pipelined retargeting delivers one-step-stale joint state; quantify
  whether this degrades tracking at teleop rates, and whether a sync execution mode is needed for
  recording.
• Open vs closed loop on hardware. Closed-loop requires reliable encoder feedback at pipeline rate;
  define behavior when feedback is missing/stale (e.g. fall back to last-commanded q or to the
  stateless solver).
• Scope of the feedback channel. Joint positions only, or also velocities / a Jacobian? (See scope
  decision above.)
• Determinism. A feedback-seeded iterative solver is history-dependent; decide how that interacts
  with deterministic replay vs the stateless closed-form variant.

References (code)

• IK math: source/isaaclab/isaaclab/controllers/differential_ik.py:149-237
• Sim Jacobian (env path): source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py:140-150, 206-207
• Jacobian backend: source/isaaclab_physx/.../assets/articulation/articulation_data.py:881-899;
  contract source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py:677-700
• Model-based IK precedent (Pinocchio, no simulator): source/isaaclab/isaaclab/controllers/pink_ik/pink_kinematics_configuration.py,
  .../pink_ik/pink_ik.py, source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py
• Retargeter contract: /code/Teleop/src/core/retargeting_engine/python/interface/base_retargeter.py
• External-input choke point: source/isaaclab_teleop/isaaclab_teleop/session_lifecycle.py:778-793
• Base-frame rebase: source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_cfg.py:154-173
• SO-101 tasks: source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/ (stack_ik_abs_env_cfg.py,
  stack_joint_pos_env_cfg.py, roll_retargeter.py, gripper_retargeter.py)

Start with the retargeter contract in /code/Teleop/src/core/retargeting_engine/python/interface/base_retargeter.py and the external-input path in source/isaaclab_teleop/isaaclab_teleop/session_lifecycle.py:778-793. Read the differential IK and Pink IK references in source/isaaclab/isaaclab/controllers/ before settling the feedback and model approach. Done means the sim feedback channel, opt-in retargeter, FK-conformance test, and SO-101 end-to-end behavior satisfy the listed acceptance criteria without changing IK-Abs.