feat(so101): add SO-101 cube-stack task with full-SE3 IK teleop

Add the TheRobotStudio SO-101 5-DOF arm and a cube-stacking task with
both joint-position and absolute task-space (IK) control, driven over XR
teleoperation.

- Asset: SO101_CFG and SO101_HIGH_PD_CFG (stiffer PD for IK tracking).
- Task: Isaac-Stack-Cube-SO101-v0 (joint-pos) and
  Isaac-Stack-Cube-SO101-IK-Abs-v0 (absolute pose IK), seated on the
  table and posed for top-down grasps within the arm's reach.
- Controller: a full-SE3-pose differential IK over all 5 arm joints. The
  3 linear task rows track position exactly while the 3 orientation rows
  are soft-weighted (scalar or per-axis, so wz=0 can drop the one DOF a
  5-DOF arm cannot serve). Kept well-conditioned via manipulability-aware
  damped least squares (adaptive lambda keyed off the full weighted task
  Jacobian) and null-space joint-limit avoidance.
- Action term: SO101PoseIKAction wiring the controller into a 7D
  [pos, quat_xyzw] command; teleop pipeline flattens to the 8D
  [pos, quat, gripper] action with a clutch-rebased, base-frame pose
  stream and an analog gripper.
- Generalize the stack gripper observations/terminations from
  parallel-only (2-joint) to single- or N-jaw grippers.
- Sim-free unit tests for the controller and the teleop pipeline wiring.

Author: jiwenc-nv

CONTRIBUTORS.md

@@ -110,6 +110,7 @@ Guidelines for modifications:
 * Jingzhou Liu
 * Jinqi Wei
 * Jinyeob Kim
+* Jiwen Cai
 * Johnson Sun
 * Juana Du
 * Kaixi Bao

source/isaaclab_assets/changelog.d/jiwenc-so-101.minor.rst

@@ -0,0 +1,6 @@
+Added
+^^^^^
+
+* Added :obj:`~isaaclab_assets.robots.so101.SO101_CFG` and
+  :obj:`~isaaclab_assets.robots.so101.SO101_HIGH_PD_CFG` configurations for the
+  TheRobotStudio SO-101 5-DOF follower arm.

source/isaaclab_assets/isaaclab_assets/robots/__init__.pyi

@@ -36,6 +36,8 @@ __all__ = [
     "RIDGEBACK_FRANKA_PANDA_CFG",
     "SAWYER_CFG",
     "SHADOW_HAND_CFG",
+    "SO101_CFG",
+    "SO101_HIGH_PD_CFG",
     "joint_parameter_lookup",
     "SPOT_CFG",
     "GO1_ACTUATOR_CFG",
@@ -85,6 +87,7 @@ from .quadcopter import CRAZYFLIE_CFG
 from .ridgeback_franka import RIDGEBACK_FRANKA_PANDA_CFG
 from .sawyer import SAWYER_CFG
 from .shadow_hand import SHADOW_HAND_CFG
+from .so101 import SO101_CFG, SO101_HIGH_PD_CFG
 from .spot import joint_parameter_lookup, SPOT_CFG
 from .unitree import (
     GO1_ACTUATOR_CFG,

source/isaaclab_assets/isaaclab_assets/robots/so101.py

@@ -0,0 +1,86 @@
+# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""Configuration for the TheRobotStudio SO-101 follower arm.
+
+The following configurations are available:
+
+* :obj:`SO101_CFG`: SO-101 5-DOF arm with a single-jaw gripper.
+* :obj:`SO101_HIGH_PD_CFG`: SO-101 with a stiffer PD controller for task-space (IK) tracking.
+
+The SO-101 is a low-cost 5-DOF arm (``shoulder_pan``, ``shoulder_lift``, ``elbow_flex``,
+``wrist_flex``, ``wrist_roll``) plus a single revolute ``gripper`` jaw. Because the arm has
+only 5 actuated DOF, it cannot achieve an arbitrary 6-DOF end-effector pose; task-space
+controllers should command the full pose but soft-weight the orientation rows so position is
+tracked exactly and orientation is best-effort (see the cube-stack IK-Abs task).
+
+Reference: https://github.com/TheRobotStudio/SO-ARM100
+Actuator gains follow the values tuned for simulation in LeIsaac.
+"""
+
+import isaaclab.sim as sim_utils
+from isaaclab.actuators import ImplicitActuatorCfg
+from isaaclab.assets.articulation import ArticulationCfg
+from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
+
+##
+# Configuration
+##
+
+SO101_CFG = ArticulationCfg(
+    spawn=sim_utils.UsdFileCfg(
+        usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/RobotStudio/so101_new_calib/so101_new_calib.usd",
+        activate_contact_sensors=False,
+        rigid_props=sim_utils.RigidBodyPropertiesCfg(
+            disable_gravity=False,
+            max_depenetration_velocity=5.0,
+        ),
+        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
+            enabled_self_collisions=True,
+            solver_position_iteration_count=8,
+            solver_velocity_iteration_count=0,
+            fix_root_link=True,
+        ),
+    ),
+    init_state=ArticulationCfg.InitialStateCfg(
+        joint_pos={
+            "shoulder_pan": 0.0,
+            "shoulder_lift": 0.0,
+            "elbow_flex": 0.0,
+            "wrist_flex": 0.0,
+            "wrist_roll": 0.0,
+            "gripper": 0.0,
+        },
+    ),
+    actuators={
+        "arm": ImplicitActuatorCfg(
+            joint_names_expr=["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"],
+            effort_limit_sim=10.0,
+            velocity_limit_sim=10.0,
+            stiffness=17.8,
+            damping=0.60,
+        ),
+        "gripper": ImplicitActuatorCfg(
+            joint_names_expr=["gripper"],
+            effort_limit_sim=10.0,
+            velocity_limit_sim=10.0,
+            stiffness=17.8,
+            damping=0.60,
+        ),
+    },
+    soft_joint_pos_limit_factor=1.0,
+)
+"""Configuration of the SO-101 follower arm with implicit actuators."""
+
+
+SO101_HIGH_PD_CFG = SO101_CFG.copy()
+SO101_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True
+SO101_HIGH_PD_CFG.actuators["arm"].stiffness = 400.0
+SO101_HIGH_PD_CFG.actuators["arm"].damping = 80.0
+"""Configuration of the SO-101 follower arm with stiffer PD control.
+
+This configuration is useful for task-space control using differential IK, where the
+default low-stiffness gains track end-effector targets poorly.
+"""

source/isaaclab_tasks/changelog.d/jiwenc-so-101.minor.rst

@@ -0,0 +1,43 @@
+Added
+^^^^^
+
+* Added ``Isaac-Stack-Cube-SO101-v0`` and ``Isaac-Stack-Cube-SO101-IK-Abs-v0`` cube-stacking
+  environments for the SO-101 5-DOF arm. The IK-Abs variant uses absolute task-space
+  differential inverse kinematics and is seated on the table.
+* Generalized the cube-stack MDP gripper observations and terminations to support single-jaw
+  grippers in addition to two-finger parallel grippers.
+* Added XR teleoperation for ``Isaac-Stack-Cube-SO101-IK-Abs-v0`` via an IsaacTeleop retargeting
+  pipeline built from the SO-101 retargeters in ``isaacteleop.retargeters``. Controller poses are
+  rebased into the robot base frame upstream via
+  :attr:`~isaaclab_teleop.IsaacTeleopCfg.target_frame_prim_path` (set to the seated, +90 deg-yawed
+  base prim), so the retargeters work directly in the IK command frame. End-effector motion is
+  clutch-rebased around the pose captured on engage: the clutch latches on the first running frame
+  (the headset "Play") and seeds its home from a static reset-origin (the gripper's pose in the
+  base frame at the seated init pose) so engaging with a steady controller does not move the arm,
+  then keeps a running home so a mid-task re-clutch resumes from the last commanded pose. The
+  gripper jaw tracks the controller trigger continuously (analog).
+
+Changed
+^^^^^^^
+
+* **Breaking:** Reworked the ``Isaac-Stack-Cube-SO101-IK-Abs-v0`` teleop control to command the
+  full end-effector SE3 pose. The arm action is now an 8-D
+  ``[pos_x, pos_y, pos_z, quat_x, quat_y, quat_z, quat_w, gripper]`` (orientation xyzw),
+  replacing the previous 6-D ``[pos_x, pos_y, pos_z, pitch, roll, gripper]``. A single
+  full-pose differential IK (``SO101PoseIKController``, ``command_type="pose"``) now solves all
+  **5** arm joints (``shoulder_pan``, ``shoulder_lift``, ``elbow_flex``, ``wrist_flex``,
+  ``wrist_roll``) over a 6-row task: 3 linear rows track position exactly (weight 1) and 3
+  orientation rows are soft-weighted by ``orientation_task_weight`` (default ``0.5``). This
+  replaces the reduced 4-row ``[x, y, z, pitch]`` IK over 4 joints plus a separate
+  ``wrist_roll`` action. The manipulability-aware damped least squares and null-space
+  joint-limit avoidance are retained (the damping ramp keys off the full weighted task Jacobian,
+  so it is coupled to the orientation weight).
+
+  Migration: the controller / action classes were renamed
+  ``SO101PositionPitchIK{Controller,ControllerCfg,Action,ActionCfg}`` ->
+  ``SO101PoseIK{...}`` (modules ``position_pitch_ik_{controller,action}.py`` ->
+  ``pose_ik_{controller,action}.py``), the cfg field ``pitch_task_weight`` ->
+  ``orientation_task_weight``, and the ``wrist_roll_action`` term was removed (``wrist_roll`` is
+  now solved by the IK). The SO-101 ``SO101WristRetargeter`` (and its ``SO101RollRetargeter``
+  alias) was removed from ``isaacteleop.retargeters``; the clutch retargeter now drives the full
+  pose with a fixed orientation calibration offset.

source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/__init__.py

@@ -0,0 +1,35 @@
+# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+import gymnasium as gym
+
+##
+# Register Gym environments.
+##
+
+##
+# Joint Position Control
+##
+
+gym.register(
+    id="Isaac-Stack-Cube-SO101-v0",
+    entry_point="isaaclab.envs:ManagerBasedRLEnv",
+    kwargs={
+        "env_cfg_entry_point": f"{__name__}.stack_joint_pos_env_cfg:SO101CubeStackEnvCfg",
+    },
+    disable_env_checker=True,
+)
+
+##
+# Inverse Kinematics - Absolute Pose Control
+##
+
+gym.register(
+    id="Isaac-Stack-Cube-SO101-IK-Abs-v0",
+    entry_point="isaaclab.envs:ManagerBasedRLEnv",
+    kwargs={
+        "env_cfg_entry_point": f"{__name__}.stack_ik_abs_env_cfg:SO101CubeStackEnvCfg",
+    },
+    disable_env_checker=True,
+)

source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/pose_ik_action.py

@@ -0,0 +1,82 @@
+# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""Action term wiring the SO-101 full-pose SE3 IK controller.
+
+Subclasses :class:`~isaaclab.envs.mdp.actions.task_space_actions.DifferentialInverseKinematicsAction`
+and swaps in :class:`SO101PoseIKController`. The base term already delegates ``action_dim`` to
+the controller and feeds it the full 6-row base-frame Jacobian, so the only override needed is
+the controller construction (and rebuilding the action buffers at the controller's 7-wide
+action dim).
+
+.. note::
+    :attr:`~isaaclab.envs.mdp.actions.actions_cfg.DifferentialInverseKinematicsActionCfg.clip`
+    is not supported for this subclass.  The base clips by action-dim axis using joint-name
+    keys, but this term's action layout is the task-space pose ``[pos_xyz, quat_xyzw]``, which
+    does not map to joint names.  Setting ``clip`` raises :exc:`NotImplementedError`.
+"""
+
+from __future__ import annotations
+
+from dataclasses import MISSING
+from typing import TYPE_CHECKING
+
+import torch
+
+from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg
+from isaaclab.envs.mdp.actions.task_space_actions import DifferentialInverseKinematicsAction
+from isaaclab.utils.configclass import configclass
+
+from .pose_ik_controller import (
+    SO101PoseIKController,
+    SO101PoseIKControllerCfg,
+)
+
+if TYPE_CHECKING:
+    from isaaclab.envs import ManagerBasedEnv
+
+
+class SO101PoseIKAction(DifferentialInverseKinematicsAction):
+    """IK action term that uses the SO-101 full-pose SE3 controller."""
+
+    cfg: SO101PoseIKActionCfg
+
+    def __init__(self, cfg: SO101PoseIKActionCfg, env: ManagerBasedEnv):
+        super().__init__(cfg, env)
+        # Replace the base position/pose controller with the SO-101 full-pose one.  Re-create the
+        # raw/processed action buffers and scale (action_dim is the controller's 7).
+        self._ik_controller = SO101PoseIKController(cfg=self.cfg.controller, num_envs=self.num_envs, device=self.device)
+        self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device)
+        self._processed_actions = torch.zeros_like(self._raw_actions)
+        self._scale = torch.zeros((self.num_envs, self.action_dim), device=self.device)
+        self._scale[:] = torch.tensor(self.cfg.scale, device=self.device)
+        # Action clipping is not supported for this term: the base clips by action-dim axis using
+        # joint-name keys, but this term's action layout is the task-space pose
+        # [pos_xyz, quat_xyzw], which does not map to joint names. Refuse rather than silently
+        # clip the wrong axis.
+        if self.cfg.clip is not None:
+            raise NotImplementedError(
+                "clip is not supported for SO101PoseIKAction (task-space [pos, quat] action "
+                "does not map to joint-name clip keys)."
+            )
+        # Joint limits are injected lazily on the first apply (asset data is populated by then)
+        # so the controller can do null-space joint-limit avoidance.
+        self._limits_injected = False
+
+    def apply_actions(self) -> None:
+        if not self._limits_injected:
+            # Limits are uniform across envs for the SO-101; env 0 is representative.
+            limits = self._asset.data.soft_joint_pos_limits.torch[0, self._joint_ids, :]
+            self._ik_controller.set_joint_pos_limits(limits[:, 0].clone(), limits[:, 1].clone())
+            self._limits_injected = True
+        super().apply_actions()
+
+
+@configclass
+class SO101PoseIKActionCfg(DifferentialInverseKinematicsActionCfg):
+    """Configuration for :class:`SO101PoseIKAction`."""
+
+    class_type: type[SO101PoseIKAction] = SO101PoseIKAction
+    controller: SO101PoseIKControllerCfg = MISSING