feat: add generic joint-space teleop device (SO-101 leader)

CMakeLists.txt

@@ -165,6 +165,7 @@ if(BUILD_PLUGINS)
 
     add_subdirectory(src/plugins/controller_synthetic_hands)
     add_subdirectory(src/plugins/generic_3axis_pedal)
+    add_subdirectory(src/plugins/so101_leader)
     add_subdirectory(src/plugins/manus)
     add_subdirectory(src/plugins/haptikos)
     if(BUILD_PLUGIN_OAK_CAMERA)

docs/source/device/index.rst

@@ -19,6 +19,7 @@ See the `Plugins directory <https://github.com/NVIDIA/IsaacTeleop/tree/main/src/
 
    trackers
    add_device
+   joint_space
    body_tracking
    haptic_feedback
    manus

docs/source/device/joint_space.rst

@@ -0,0 +1,130 @@
+.. SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+.. SPDX-License-Identifier: Apache-2.0
+
+Generic Joint-Space Device
+==========================
+
+A reusable device path for any **joint-encoder source** -- leader arms, exoskeletons, haptic
+gloves, or other articulated input devices. A device streams a name-keyed ``JointStateOutput``
+FlatBuffer over the OpenXR tensor transport; one schema, one tracker, one source, and one
+retargeter serve them all, so adding a new joint-space device is just a new **plugin** plus a
+small **config**.
+
+The **SO-101 leader arm** (`TheRobotStudio SO-ARM100 <https://github.com/TheRobotStudio/SO-ARM100>`_,
+6 Feetech STS3215 bus servos) is the reference instance.
+
+At a glance
+-----------
+
+.. list-table::
+   :header-rows: 1
+   :widths: 18 82
+
+   * - Layer
+     - Component
+   * - Schema
+     - :code-file:`src/core/schema/fbs/joint_state.fbs` -- ``JointState`` (name + position +
+       optional velocity/effort) and ``JointStateOutput`` (a vector of joints + ``device_id``).
+   * - Plugin
+     - :code-dir:`src/plugins/so101_leader` -- pushes ``JointStateOutput`` via ``SchemaPusher``.
+       Reads the FEETECH STS3215 servos over serial (``FeetechBus``); synthetic fallback when no
+       device path is given.
+   * - Tracker
+     - ``JointStateTracker`` (facade) with live (``LiveJointStateTrackerImpl``) and MCAP-replay
+       (``ReplayJointStateTrackerImpl``) backends, registered in the live/replay factories.
+   * - Source
+     - ``JointStateSource`` (``IDeviceIOSource``) -- converts the FlatBuffer into a name-keyed
+       group of joint positions for the retargeting graph.
+   * - Retargeter
+     - ``JointStateRetargeter`` -- ``joint`` (mirror) or ``ee_pose`` (URDF FK) mode. See
+       :doc:`/references/retargeting/joint_space`.
+
+Data schema
+-----------
+
+Joints are modeled as **name -> value** records so consumers read them by name, independent of
+wire order:
+
+.. code-block:: idl
+   :class: code-100col
+
+   table JointState {
+     name: string (id: 0, key);   // e.g. "shoulder_pan", "gripper"
+     position: float (id: 1);     // [rad] revolute, [m] prismatic
+     velocity: float (id: 2);     // optional (JointStateOutput.has_velocity)
+     effort: float (id: 3);       // optional (JointStateOutput.has_effort)
+     valid: bool = true (id: 4);
+   }
+
+   table JointStateOutput {
+     joints: [JointState] (id: 0);
+     device_id: string (id: 1);
+     has_velocity: bool (id: 2);
+     has_effort: bool (id: 3);
+     ee_pose: Pose (id: 4);       // RESERVED: device-side FK; not consumed yet
+     ee_pose_valid: bool (id: 5);
+   }
+
+The gripper is just another named DOF (conventionally ``"gripper"``). ``velocity``, ``effort``,
+and ``ee_pose`` are optional/reserved: the reference plugin and ``JointStateSource`` populate and
+surface joint **positions** only.
+
+The SO-101 leader plugin
+------------------------
+
+``so101_leader`` reads the six SO-101 servos (``shoulder_pan, shoulder_lift, elbow_flex,
+wrist_flex, wrist_roll, gripper``) and pushes them to a tensor collection. With a serial device
+path it talks to the FEETECH STS3215 bus servos directly via ``FeetechBus`` -- the same SMS/STS
+wire protocol the FEETECH SCServo SDK / LeRobot's ``FeetechMotorsBus`` use, with no SDK dependency:
+it disables torque (so the leader can be back-driven) and reads ``Present_Position`` each frame,
+converting ticks to radians with per-joint calibration. With no device path it falls back to a
+**synthetic** trajectory so the pipeline runs hardware-free (CI and the headless example).
+
+.. code-block:: bash
+
+   # Synthetic backend (no hardware), default collection id "so101_leader":
+   ./install/plugins/so101_leader/so101_leader_plugin
+
+   # Real SO-101 leader on a serial port (Linux), optional calibration file:
+   ./install/plugins/so101_leader/so101_leader_plugin /dev/ttyACM0 so101_leader so101_leader.calib
+
+See the :code-file:`plugin README <src/plugins/so101_leader/README.md>` for hardware setup
+(unique servo ids, gear removal, back-driving) and the calibration file format.
+
+The consumer side creates a ``JointStateSource(name=..., collection_id="so101_leader",
+joint_names=[...])`` on the same ``collection_id``; ``TeleopSession`` discovers and polls the
+``JointStateTracker`` each frame.
+
+Record and replay
+-----------------
+
+The live tracker records to MCAP, and ``ReplayJointStateTrackerImpl`` replays it back with no
+OpenXR runtime, so a recorded session drives the retargeting graph headlessly:
+
+.. code-block:: python
+
+   from isaacteleop.deviceio import McapRecordingConfig, McapReplayConfig
+   from isaacteleop.teleop_session_manager import SessionMode, TeleopSession, TeleopSessionConfig
+
+   # Record (live): TeleopSessionConfig(..., mcap_config=McapRecordingConfig("leader.mcap"))
+   # Replay (headless): TeleopSessionConfig(..., mode=SessionMode.REPLAY,
+   #                                        mcap_config=McapReplayConfig("leader.mcap"))
+
+Add another joint-space device
+------------------------------
+
+Reuse everything above by writing only:
+
+#. A **plugin** that reads your hardware and fills ``JointStateOutput`` (positions; optionally
+   velocity/effort), modeled on :code-dir:`src/plugins/so101_leader`.
+#. A **config**: a ``collection_id``, the device joint names, and -- for ``ee_pose`` mode -- a URDF
+   and end-effector link.
+
+The schema, ``JointStateTracker``, ``JointStateSource``, and ``JointStateRetargeter`` are unchanged.
+
+.. seealso::
+
+   :doc:`add_device` -- the general four-step device-plugin recipe (foot-pedal reference).
+
+   :doc:`/references/retargeting/joint_space` -- the ``JointStateRetargeter`` (joint / EE modes),
+   the end-to-end example, and validation.

docs/source/references/retargeting/index.rst

@@ -11,6 +11,8 @@ Source Nodes
 * ``HandsSource`` -- provides hand tracking data (left/right, 26 joints each).
 * ``ControllersSource`` -- provides motion controller data (grip pose, trigger, thumbstick, etc.).
 * ``Generic3AxisPedalSource`` -- provides 3-axis foot pedal data (left/right pedals, rudder).
+* ``JointStateSource`` -- provides name-keyed joint positions from a generic joint-space device
+  (leader arm, exoskeleton, ...). See :doc:`joint_space`.
 * ``FullBodySource`` -- provides full-body pose (e.g. Pico tracking).
 
 Available Retargeters
@@ -41,6 +43,14 @@ Available Retargeters
    ``hand_side`` (``"left"`` or ``"right"``), ``gripper_close_meters``, ``gripper_open_meters``,
    and ``controller_threshold`` for trigger-based closing.
 
+.. dropdown:: JointStateRetargeter
+
+   Maps a name-keyed joint-state input (from ``JointStateSource``) to an action for a generic
+   joint-space device -- leader arm, exoskeleton, etc. Two modes via ``JointStateRetargeterConfig``:
+   ``"joint"`` (lossless leader -> follower mirror with optional per-joint affine; no extra deps)
+   and ``"ee_pose"`` (URDF forward kinematics -> 7D EE pose + gripper, requires ``pinocchio``).
+   See :doc:`joint_space` for the full setup, modes, and the SO-101 example.
+
 .. dropdown:: DexHandRetargeter / DexBiManualRetargeter
 
    Accurate hand tracking retargeter using the ``dex-retargeting`` library. It maps full hand
@@ -315,3 +325,4 @@ and :doc:`Contributing Guide <../../getting_started/contributing>` for details.
    :caption: Retargeter setup guides
 
    sharpa
+   joint_space

docs/source/references/retargeting/joint_space.rst

@@ -0,0 +1,128 @@
+.. SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+.. SPDX-License-Identifier: Apache-2.0
+
+Retargeter: Joint-Space Device
+==============================
+
+``JointStateRetargeter`` maps a name-keyed joint-state input (from
+:doc:`/device/joint_space`'s ``JointStateSource``) onto an Isaac Lab action, in one of two modes.
+It is the generic retargeter for leader arms, exoskeletons, and other joint-encoder devices; the
+SO-101 leader arm is the reference instance.
+
+At a glance
+-----------
+
+.. list-table::
+   :header-rows: 1
+   :widths: 16 30 54
+
+   * - Mode
+     - Output
+     - Use
+   * - ``joint``
+     - one float per target joint (``joint_targets``)
+     - Lossless leader -> follower mirror for same-kinematics teleoperation. Name remap + optional
+       per-joint affine. No extra dependencies.
+   * - ``ee_pose``
+     - 7-D ``ee_pose`` ``[x,y,z,qx,qy,qz,qw]`` + ``gripper_command``
+     - Task-space / cross-embodiment teleoperation via URDF forward kinematics. Requires
+       ``pinocchio`` (the ``[retargeters]`` extra).
+
+``joint`` mode
+--------------
+
+Each target joint is filled from a device joint (by name) with an optional affine
+``offset + sign * scale * value``. Defaults are an identity mirror. ``JointStateRetargeterConfig``:
+
+* ``device_joints`` -- ordered device DOF names (must match the source's ``joint_names`` order).
+* ``target_joints`` -- robot joint names to emit (defaults to ``device_joints``).
+* ``joint_map`` -- ``{device_name: target_name}`` overrides; ``scale`` / ``offset`` / ``sign`` --
+  per-target affine.
+
+``ee_pose`` mode
+----------------
+
+Forward-kinematics the device joints through a URDF and emit the end-effector pose plus a gripper
+command. Config: ``urdf_path``, ``ee_link``, ``gripper_joint`` (and optional ``gripper_open`` /
+``gripper_close`` to emit normalized closedness in ``[0, 1]`` instead of the raw value).
+
+* FK uses ``pinocchio`` (imported lazily; ``joint`` mode never needs it). Install via
+  ``pip install 'isaacteleop[retargeters]'``.
+* Assumes a fixed-base model of single-DOF joints (the common leader-arm / exoskeleton case).
+* The schema's device ``ee_pose`` field is **not** consumed yet -- FK is always computed from the
+  joint positions.
+* ``clutch=True`` rebases the EE around an origin captured on the first ``RUNNING`` frame so
+  engaging teleop does not jump the robot; when the optional ``robot_ee_pos`` input (the live
+  ``world_T_ee``) is connected, the latched home is the robot's current end-effector.
+
+.. note::
+
+   The ``joints`` input is read positionally in ``device_joints`` order, so the upstream source's
+   ``joint_names`` must list the same names in the same order. A name mismatch is rejected by the
+   graph's type check at ``connect`` time.
+
+Use it from Python
+------------------
+
+A pipeline builder returns an ``OutputCombiner`` with a single ``"action"`` key (the layout your
+environment's action space expects):
+
+.. code-block:: python
+
+   from isaacteleop.retargeting_engine.deviceio_source_nodes import JointStateSource
+   from isaacteleop.retargeting_engine.interface import OutputCombiner
+   from isaacteleop.retargeters import (
+       JointStateRetargeter,
+       JointStateRetargeterConfig,
+       TensorReorderer,
+   )
+
+   SO101_JOINTS = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]
+
+   def build_so101_joint_pipeline():
+       source = JointStateSource(name="leader", collection_id="so101_leader", joint_names=SO101_JOINTS)
+       retargeter = JointStateRetargeter(
+           name="leader",
+           mode="joint",
+           config=JointStateRetargeterConfig(device_joints=SO101_JOINTS, target_joints=SO101_JOINTS),
+       )
+       head = retargeter.connect({JointStateRetargeter.JOINTS: source.output(JointStateSource.JOINTS)})
+       reorderer = TensorReorderer(
+           input_config={"joint_targets": SO101_JOINTS},
+           output_order=SO101_JOINTS,
+           name="action_reorderer",
+           input_types={"joint_targets": "scalar"},
+       )
+       connected = reorderer.connect({"joint_targets": head.output("joint_targets")})
+       return OutputCombiner({"action": connected.output("output")})
+
+For ``ee_pose`` mode, build the retargeter with ``mode="ee_pose"`` + a ``urdf_path`` / ``ee_link``
+and flatten ``ee_pose`` + ``gripper_command`` into the env's task-space action layout.
+
+Run the example
+---------------
+
+The repo ships ``examples/teleop/python/joint_space_device_example.py``:
+
+.. code-block:: console
+
+   # Consumes the so101_leader plugin over OpenXR (source cloudxr.env first):
+   $ python joint_space_device_example.py --launch-plugin --mode joint --frames 8
+   $ python joint_space_device_example.py --launch-plugin --mode ee --urdf so101_new_calib.urdf
+
+Validate
+--------
+
+Sim-free unit tests cover both modes (joint affine/remap/hold/reset, EE forward kinematics, clutch
+rebasing, and the flattened action width/order):
+
+.. code-block:: console
+
+   $ ctest --test-dir build -R 'retargeting_test_joint_state' --output-on-failure
+
+.. seealso::
+
+   :doc:`/device/joint_space` -- the schema, ``JointStateTracker``, ``JointStateSource``, the
+   SO-101 plugin, and MCAP record/replay.
+
+   :doc:`index` -- the broader retargeting interface and pipeline-builder pattern.