Add SO-101 support in Isaac Lab

[jiwenc-nv]

Description

Adds so101 stack env for teleop + imitation learning

Fixes # (issue)

Type of change

• New feature (non-breaking change which adds functionality)

Checklist

• I have read and understood the contribution guidelines
• I have run the pre-commit checks with ./isaaclab.sh --format
• I have made corresponding changes to the documentation
• My changes generate no new warnings
• I have added tests that prove my fix is effective or that my feature works
• I have updated the changelog and the corresponding version in the extension's config/extension.toml file
• I have added my name to the CONTRIBUTORS.md or my name already exists there

[isaaclab-review-bot]

🤖 Code Review — PR #5989: Add SO-101 Teleop

Latest review: Commit c8873693 (rebased onto latest develop)

---

Summary

This PR adds support for the TheRobotStudio SO-101 5-DOF follower arm to Isaac Lab, including:

• Asset configuration (SO101_CFG, SO101_HIGH_PD_CFG)
• Cube-stacking task environments (joint-position and IK-absolute)
• New: Reduced-task position+pitch differential IK controller (SO101PositionPitchIKController)
• Generalization of gripper MDP logic to support single-jaw grippers
• XR teleoperation support with IsaacTeleop

---

🔴 New Issue Requiring Attention

Test-implementation mismatch in test_so101_teleop_pipeline.py:

The pipeline builder _build_so101_stack_pipeline() produces a 6D action [pos_x, pos_y, pos_z, pitch, roll, gripper] (matching the docstring and SO101IkActionsCfg field order), but the tests still assert 5D:

# test_so101_teleop_pipeline.py, line 37:
assert action_type.shape == (5,), f"expected a 5D action, got shape {action_type.shape}"

# And line 52:
assert output_order == ["pos_x", "pos_y", "pos_z", "roll_value", "gripper_value"], ...

Expected assertions should be:

• shape == (6,) for the 6D action
• output_order == ["pos_x", "pos_y", "pos_z", "pitch_value", "roll_value", "gripper_value"] (with the correct pitch element label from SO101WristRetargeter)

Please update test_so101_teleop_pipeline.py to match the 6D action contract.

---

✅ Previous Issues Addressed

1. Yaw tuple typo fixed — "yaw": (-1.0, 1, 0) → "yaw": (-1.0, 1.0) ✓
2. Test coverage added — Comprehensive sim-free tests for the position+pitch IK controller ✓

---

🟢 Positive Aspects of New Changes

• Clean reduced-task IK design: The 4x4 [x, y, z, pitch] solve over 4 joints elegantly handles the 5-DOF arm's kinematic constraints
• Proper degenerate handling: Pitch row/error are zeroed when EE is directly above the base (radial reach < ε)
• xyzw quaternion convention documented: Clear docstrings noting IsaacLab's scalar-last convention
• Discriminating regression test: test_pitch_error_zero_for_non_identity_quat_xyzw catches quaternion convention bugs
• Batch-aware degeneracy mask: Per-environment handling is correctly implemented and tested

---

🟡 Non-Blocking Suggestions

1. Gripper limit margin (carried from previous review): _SO101_GRIPPER_OPEN = 1.745 is at USD limit (~100°). Consider small margin.

2. Table/Cube geometry (carried from previous review): Base StackEnvCfg table is at pos=[0.5, 0, 0] but SO-101 cubes at x ∈ [0.15, 0.30]. Verify in-sim.

---

Verdict: One blocking test inconsistency to fix. Otherwise, the position+pitch IK implementation is well-designed with excellent test coverage.

---

Changelog:

• 832734dc → c8873693: Major rewrite adding position+pitch reduced-task IK. Action changed from 5D to 6D. New SO101PositionPitchIKController, SO101PositionPitchIKAction, and comprehensive unit tests added. Tests need updating to match new 6D contract.

[greptile-apps]

Greptile Summary

This PR adds SO-101 5-DOF follower arm support to Isaac Lab, introducing two cube-stacking environments (Isaac-Stack-Cube-SO101-v0 and Isaac-Stack-Cube-SO101-IK-Abs-v0) along with XR teleoperation via a clutch-rebased full-SE3-pose IK pipeline. The MDP gripper observations and terminations are also generalized to support single-jaw grippers alongside the existing two-finger parallel gripper path.

• Robot asset & joint-pos env (so101.py, stack_joint_pos_env_cfg.py): Adds SO101_CFG / SO101_HIGH_PD_CFG with implicit actuators, seats the robot on a tabletop at _SO101_MOUNT_Z, and configures a binary gripper action with a 0.2 rad grasped-threshold.
• Full-pose SE3 IK stack (pose_ik_controller.py, pose_ik_action_term.py, pose_ik_action.py): Implements a custom SO101PoseIKController with soft orientation-row weighting, per-joint orientation masks (wrist-only orientation), manipulability-aware DLS damping, and null-space JLA — replacing the previous 4-row pitch+roll decomposition with a single 6-row pose solve over all 5 arm joints.
• Teleop pipeline (stack_ik_abs_env_cfg.py): Wires SO101ClutchRetargeter + SO101GripperRetargeter into an 8D action [pos_xyz, quat_xyzw, gripper], with all isaacteleop imports correctly deferred inside _build_so101_stack_pipeline() and the world→base rebase handled upstream via target_frame_prim_path.

Confidence Score: 5/5

This PR is safe to merge; all new code is well-tested and the core math has been verified by thorough sim-free unit tests.

The full-pose IK controller, action term, env configs, and teleop pipeline are all self-consistent and well-documented. The isaacteleop imports are correctly deferred inside _build_so101_stack_pipeline(), resolving the previous retargeter-import concern. Tests cover quaternion conventions, adaptive damping, JLA, orientation masking, and the 8D pipeline contract. The generalized gripper observations/terminations loop over all joint IDs using the same env.cfg.gripper_threshold that was already required before this PR, so existing Franka paths are unaffected. Only minor style suggestions were found.

No files require special attention — pose_ik_controller.py is the most complex addition but is well-exercised by the unit test suite.

Important Files Changed

Filename	Overview
source/isaaclab_assets/isaaclab_assets/robots/so101.py	New robot asset config for the SO-101 5-DOF arm; SO101_HIGH_PD_CFG is a deep copy with stiffened arm PD and gravity disabled — standard IsaacLab pattern, correctly isolated from SO101_CFG.
source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/pose_ik_controller.py	Custom 6-row SE3 DLS IK controller with soft orientation weights, per-joint orientation masks, adaptive damping, and null-space JLA; math is correct and all code paths are verified by the unit tests in test_so101_pose_ik.py.
source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/pose_ik_action_term.py	Runtime action term wrapping SO101PoseIKController; lazily injects joint limits on first apply_actions call and guards against unsupported clip usage — torch.eye created on every call could be cached.
source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/stack_ik_abs_env_cfg.py	IK-Abs env cfg with full-SE3-pose action and IsaacTeleop wiring; isaacteleop imports correctly deferred inside _build_so101_stack_pipeline(), top-level isaaclab_teleop import is intentional and documented as always-available.
source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/stack_joint_pos_env_cfg.py	Joint-position env cfg with seated robot placement, cube workspace sized to SO-101 reach (~0.3 m), and gripper open/close constants with a 0.2 rad grasped-threshold; well-commented.
source/isaaclab_tasks/isaaclab_tasks/contrib/stack/mdp/observations.py	Generalized gripper_pos, object_grasped, and object_stacked to support 1-joint grippers by looping over all gripper joint IDs; uses env.cfg.gripper_threshold which was already required before this PR.
source/isaaclab_tasks/isaaclab_tasks/contrib/stack/mdp/terminations.py	cubes_stacked termination generalized to loop over N gripper joints with parametric atol/rtol; consistent with the observation changes.
source/isaaclab_tasks/test/contrib/stack/test_so101_pose_ik.py	Comprehensive sim-free unit tests for the IK controller covering action_dim, set_command, orientation weighting, joint masking, adaptive damping, JLA, and quaternion convention — all passing against hand-built tensors.
source/isaaclab_tasks/test/contrib/test_so101_teleop_pipeline.py	Sim-free tests guard the 8D action contract, output element order, and clutch input-spec; correctly uses pytest.importorskip to skip the whole module when isaacteleop is absent, consistent with the updated 8D pipeline.

Class Diagram

%%{init: {'theme': 'neutral'}}%%
classDiagram
    class DifferentialIKControllerCfg {
        +command_type: str
        +use_relative_mode: bool
        +ik_method: str
    }
    class SO101PoseIKControllerCfg {
        +orientation_task_weight: float
        +orientation_joint_names: tuple
        +lambda_min: float
        +lambda_max: float
        +sigma_thresh: float
        +jla_gain: float
        +jla_margin: float
    }
    class DifferentialIKController {
        +action_dim: int
        +set_command()
        +compute()
    }
    class SO101PoseIKController {
        +action_dim: int
        +set_command()
        +compute()
        +_assemble_task()
        +_damped_least_squares()
        +_adaptive_lambda_sq()
        +_joint_limit_avoidance()
        +set_joint_pos_limits()
        +set_orientation_joint_mask()
    }
    class DifferentialInverseKinematicsActionCfg {
        +controller: DifferentialIKControllerCfg
        +scale: float
    }
    class SO101PoseIKActionCfg {
        +controller: SO101PoseIKControllerCfg
    }
    class DifferentialInverseKinematicsAction {
        +apply_actions()
    }
    class SO101PoseIKAction {
        +apply_actions()
        -_limits_injected: bool
    }
    DifferentialIKControllerCfg <|-- SO101PoseIKControllerCfg
    DifferentialIKController <|-- SO101PoseIKController
    DifferentialInverseKinematicsActionCfg <|-- SO101PoseIKActionCfg
    DifferentialInverseKinematicsAction <|-- SO101PoseIKAction
    SO101PoseIKActionCfg --> SO101PoseIKControllerCfg : controller
    SO101PoseIKAction --> SO101PoseIKController : _ik_controller

Loading

%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
classDiagram
    class DifferentialIKControllerCfg {
        +command_type: str
        +use_relative_mode: bool
        +ik_method: str
    }
    class SO101PoseIKControllerCfg {
        +orientation_task_weight: float
        +orientation_joint_names: tuple
        +lambda_min: float
        +lambda_max: float
        +sigma_thresh: float
        +jla_gain: float
        +jla_margin: float
    }
    class DifferentialIKController {
        +action_dim: int
        +set_command()
        +compute()
    }
    class SO101PoseIKController {
        +action_dim: int
        +set_command()
        +compute()
        +_assemble_task()
        +_damped_least_squares()
        +_adaptive_lambda_sq()
        +_joint_limit_avoidance()
        +set_joint_pos_limits()
        +set_orientation_joint_mask()
    }
    class DifferentialInverseKinematicsActionCfg {
        +controller: DifferentialIKControllerCfg
        +scale: float
    }
    class SO101PoseIKActionCfg {
        +controller: SO101PoseIKControllerCfg
    }
    class DifferentialInverseKinematicsAction {
        +apply_actions()
    }
    class SO101PoseIKAction {
        +apply_actions()
        -_limits_injected: bool
    }
    DifferentialIKControllerCfg <|-- SO101PoseIKControllerCfg
    DifferentialIKController <|-- SO101PoseIKController
    DifferentialInverseKinematicsActionCfg <|-- SO101PoseIKActionCfg
    DifferentialInverseKinematicsAction <|-- SO101PoseIKAction
    SO101PoseIKActionCfg --> SO101PoseIKControllerCfg : controller
    SO101PoseIKAction --> SO101PoseIKController : _ik_controller

Loading

_{Reviews (4): Last reviewed commit: "Update SO-101 teleop orientation offset ..." | Re-trigger Greptile}

[StafaH]

Hi @jiwenc-nv, thanks for the PR! Sorry for the delay, we started a refactor of isaaclab_tasks and the lift task refactor just landed so now would be a good time to merge this. For a task to be in core it needs to be maintained such that all physics backends converge with RL training using rsl_rl. Do you think you can add a lift variant for core, and then leave the stack variant in contrib?

[rwiltz]

@greptile

[rwiltz]

@greptile

[rwiltz]

@greptile