Record LinkerHand actions in sim2real datasets

Author: BotRunner64

AGENTS.md

@@ -148,7 +148,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos
 - Optional LinkerHand control uses `hands.enabled=true`, `hands.driver=linkerhand_l6|linkerhand_o6`, and `hands.mode=gripper|vr_hand_pose`; default is disabled
 - Optional Pico sim2real LeRobot v3 recording uses `--config-name sim2real_record` or `recording.enabled=true`; it requires `input.provider=pico4`, `input.video.enabled=true`, `input.video.source=realsense`, an interactive terminal, and the `recording` extra
 - Recording is manual only: terminal `R` starts an episode, `S` saves, `D` discards the active episode, and `Q` shuts down; `STANDING`, `MOCAP`, `ARMS`, and paused mocap are recordable
-- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, `observation.mode(1)`, and `action(36)`; RealSense capture lives in `pico_input` through the normal `input.video` path
+- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, `observation.mode(1)`, `action(36)`, and `action.hand(12)`; RealSense capture lives in `pico_input` through the normal `input.video` path
 - `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` for Pico grip/trigger open-close control and supports LinkerHand L6 and O6
 - `vr_hand_pose` mode reuses `Pico4InputProvider.get_hand_snapshot()` and somehand 0.2.0 public `somehand.api` for continuous Pico hand-pose retargeting; do not start a second `PicoBridge` for hand control
 - Teleopit owns Pico 26-joint hand-state to 21-landmark conversion; do not import `somehand.pico_input`

README.md

@@ -98,8 +98,9 @@ python scripts/run/run_sim2real.py --config-name sim2real_record \
 Recording uses the terminal controls `R` start, `S` save, `D` discard, and `Q`
 shutdown. `STANDING`, `MOCAP`, `ARMS`, and paused mocap can be recorded. The
 dataset schema is `observation.images.d435i_rgb` video at 30 Hz,
-`observation.state(68)`, `observation.mode(1)`, and `action(36)` as the aligned
-reference qpos sent to the policy path.
+`observation.state(68)`, `observation.mode(1)`, `action(36)` as the aligned
+reference qpos sent to the policy path, and `action.hand(12)` as the latest
+LinkerHand left/right 6D pose commands.
 
 ## Documentation
 
@@ -112,7 +113,7 @@ Full docs at **[BotRunner64.github.io/Teleopit](https://BotRunner64.github.io/Te
 - Added Pico sim2real `ARMS` mode: Pico/controller `B` toggles between whole-body `MOCAP` and stand-pose body/legs with live retargeted arms.
 - Bumped Pico input support to pico-bridge 0.2.1 and its corrected tracking pose semantics.
 - Added optional LinkerHand L6 sim2real modes under `hands.*`: `gripper` from Pico grip/trigger and `vr_hand_pose` from Pico hand pose through somehand 0.2.0 public API.
-- Added manual Pico sim2real LeRobot v3 recording with RealSense D435i RGB video, 68D robot state, mode labels, and 36D reference-qpos action labels.
+- Added manual Pico sim2real LeRobot v3 recording with RealSense D435i RGB video, 68D robot state, mode labels, 36D reference-qpos action labels, and 12D LinkerHand pose action labels.
 - Added LinkerHand O6 support for Pico `gripper` mode with an O6-specific grasp pose.
 - Set LinkerHand L6 `vr_hand_pose` control to maximum speed while keeping `gripper` at the configured default speed.
 - Switched default `vr_hand_pose` to a low-latency somehand path with 60 Hz hand retargeting and reduced smoothing.

docs/docs/configuration/config-reference.md

@@ -186,13 +186,16 @@ observation.images.d435i_rgb   video [480,640,3] uint8
 observation.state              float32[68]
 observation.mode               float32[1]
 action                         float32[36]
+action.hand                    float32[12]
 ```
 
 `observation.state` is ordered as `joint_pos(29)`, `joint_vel(29)`,
 `base_quat_wxyz(4)`, `base_ang_vel(3)`, and `projected_gravity(3)`.
 `observation.mode` is a numeric categorical: `standing=0`, `mocap=1`,
 `arms=2`, and `pause=3`. `action` is the current reference qpos:
 `root_pos(3) + root_quat_wxyz(4) + joint_pos(29)`.
+`action.hand` is the latest LinkerHand command from the hand worker:
+`left_pose(6) + right_pose(6)`, using the SDK's 0-255 pose values.
 
 ## Critical: `default_dof_pos`
 

docs/docs/tutorials/pico-sim2real.md

@@ -117,7 +117,7 @@ Terminal controls are `R` start episode, `S` save, `D` discard, and `Q`
 shutdown. `STANDING`, `MOCAP`, `ARMS`, and paused mocap can be recorded;
 saved episodes cannot be discarded afterward. The v1 schema records
 `observation.images.d435i_rgb`, `observation.state(68)`,
-`observation.mode(1)`, and `action(36)` at 30 Hz.
+`observation.mode(1)`, `action(36)`, and `action.hand(12)` at 30 Hz.
 
 ## Operator Flow
 

docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md

@@ -203,10 +203,13 @@ observation.images.d435i_rgb   video [480,640,3] uint8
 observation.state              float32[68]
 observation.mode               float32[1]
 action                         float32[36]
+action.hand                    float32[12]
 ```
 
 `observation.state` 的顺序是 `joint_pos(29)`、`joint_vel(29)`、
 `base_quat_wxyz(4)`、`base_ang_vel(3)` 和 `projected_gravity(3)`。
 `observation.mode` 是数值类别：`standing=0`、`mocap=1`、
 `arms=2`、`pause=3`。`action` 是当前 reference qpos：
 `root_pos(3) + root_quat_wxyz(4) + joint_pos(29)`。
+`action.hand` 是手部 worker 最新的 LinkerHand 命令：
+`left_pose(6) + right_pose(6)`，使用 SDK 的 0-255 pose 数值。

docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md

@@ -112,7 +112,7 @@ python scripts/run/run_sim2real.py \
 终端控制为：`R` 开始 episode，`S` 保存，`D` 丢弃，`Q` 关闭。可以录制
 `STANDING`、`MOCAP`、`ARMS` 和暂停状态的 mocap；已经保存的 episode 不支持再丢弃。
 v1 schema 以 30 Hz 记录 `observation.images.d435i_rgb`、`observation.state(68)`、
-`observation.mode(1)` 和 `action(36)`。
+`observation.mode(1)`、`action(36)` 和 `action.hand(12)`。
 
 ## 操作流程
 

teleopit/recording/lerobot_v3.py

@@ -19,9 +19,11 @@
 STATE_KEY = "observation.state"
 MODE_KEY = "observation.mode"
 ACTION_KEY = "action"
+HAND_ACTION_KEY = "action.hand"
 STATE_DIM = 68
 MODE_DIM = 1
 ACTION_DIM = FULL_QPOS_DIM
+HAND_ACTION_DIM = 12
 DEFAULT_IMAGE_SHAPE = (480, 640, 3)
 MODE_CODES = {
     "standing": 0,
@@ -41,6 +43,8 @@ class RecordingSchema:
     mode_dim: int = MODE_DIM
     action_key: str = ACTION_KEY
     action_dim: int = ACTION_DIM
+    hand_action_key: str = HAND_ACTION_KEY
+    hand_action_dim: int = HAND_ACTION_DIM
 
 
 def build_recording_schema(camera_cfg: Any) -> RecordingSchema:
@@ -74,6 +78,11 @@ def lerobot_features(schema: RecordingSchema) -> dict[str, dict[str, object]]:
             "shape": (schema.action_dim,),
             "names": ["action"],
         },
+        schema.hand_action_key: {
+            "dtype": "float32",
+            "shape": (schema.hand_action_dim,),
+            "names": ["hand_action"],
+        },
     }
 
 
@@ -114,6 +123,16 @@ def modality_sidecar(schema: RecordingSchema) -> dict[str, object]:
                     "joint_pos": [7, 36],
                 },
             },
+            schema.hand_action_key: {
+                "type": "low_dim",
+                "shape": [schema.hand_action_dim],
+                "dtype": "float32",
+                "units": "linkerhand_uint8_pose",
+                "slices": {
+                    "left_pose": [0, 6],
+                    "right_pose": [6, 12],
+                },
+            },
         },
     }
 
@@ -149,6 +168,19 @@ def normalize_action_reference_qpos(reference_qpos: object) -> np.ndarray:
     return action
 
 
+def normalize_hand_action(left_pose: object, right_pose: object) -> np.ndarray:
+    left = np.asarray(left_pose, dtype=np.float32).reshape(-1)
+    right = np.asarray(right_pose, dtype=np.float32).reshape(-1)
+    if left.shape[0] != 6:
+        raise ValueError(f"recording left hand pose must be 6D, got {left.shape[0]}")
+    if right.shape[0] != 6:
+        raise ValueError(f"recording right hand pose must be 6D, got {right.shape[0]}")
+    action = np.concatenate([left, right], dtype=np.float32)
+    if action.shape[0] != HAND_ACTION_DIM:
+        raise ValueError(f"recording action.hand must be {HAND_ACTION_DIM}D, got {action.shape[0]}")
+    return action
+
+
 def build_mode_observation(mode: str) -> np.ndarray:
     normalized = str(mode).strip().lower()
     if normalized not in MODE_CODES:
@@ -228,6 +260,7 @@ def add_frame(
         state: np.ndarray,
         mode: np.ndarray,
         action: np.ndarray,
+        hand_action: np.ndarray,
         task: str,
     ) -> None:
         if not self._active:
@@ -238,18 +271,22 @@ def add_frame(
         state_arr = np.asarray(state, dtype=np.float32).reshape(-1)
         mode_arr = np.asarray(mode, dtype=np.float32).reshape(-1)
         action_arr = np.asarray(action, dtype=np.float32).reshape(-1)
+        hand_action_arr = np.asarray(hand_action, dtype=np.float32).reshape(-1)
         if state_arr.shape[0] != self._schema.state_dim:
             raise ValueError(f"{self._schema.state_key} must be {self._schema.state_dim}D")
         if mode_arr.shape[0] != self._schema.mode_dim:
             raise ValueError(f"{self._schema.mode_key} must be {self._schema.mode_dim}D")
         if action_arr.shape[0] != self._schema.action_dim:
             raise ValueError(f"{self._schema.action_key} must be {self._schema.action_dim}D")
+        if hand_action_arr.shape[0] != self._schema.hand_action_dim:
+            raise ValueError(f"{self._schema.hand_action_key} must be {self._schema.hand_action_dim}D")
         self._dataset.add_frame(
             {
                 self._schema.image_key: image_arr,
                 self._schema.state_key: state_arr,
                 self._schema.mode_key: mode_arr,
                 self._schema.action_key: action_arr,
+                self._schema.hand_action_key: hand_action_arr,
                 "task": str(task),
             }
         )

teleopit/sim2real/hands/worker.py

@@ -2,71 +2,107 @@
 
 import logging
 import time
-from typing import Any
+from typing import Any, Sequence
 
 from teleopit.runtime.common import cfg_get
-from teleopit.sim2real.hands.base import HandDevice, HandInputMapper
+from teleopit.sim2real.hands.base import HandDevice, HandInputMapper, HandPoseCommand
 from teleopit.sim2real.hands.linkerhand_l6 import build_linkerhand_l6
 from teleopit.sim2real.hands.linkerhand_o6 import build_linkerhand_o6
 
 logger = logging.getLogger(__name__)
 
 
 class HandRuntime:
-    def __init__(self, device: HandDevice, mapper: HandInputMapper):
+    def __init__(
+        self,
+        device: HandDevice,
+        mapper: HandInputMapper,
+        *,
+        open_commands: Sequence[HandPoseCommand] = (),
+    ):
         self._device = device
         self._mapper = mapper
         self.enabled = True
         self._failed = False
+        self._open_commands = tuple(open_commands)
 
-    def start(self) -> None:
+    def start(self) -> tuple[HandPoseCommand, ...]:
         try:
             self._device.connect()
             self._mapper.start()
+            return self._open_pose_commands("startup")
         except Exception:
             try:
                 self._device.close()
             finally:
                 raise
 
-    def tick(self, *, controller_snapshot: object | None, hand_snapshot: object | None, active: bool, now_s: float | None = None) -> None:
+    def tick(
+        self,
+        *,
+        controller_snapshot: object | None,
+        hand_snapshot: object | None,
+        active: bool,
+        now_s: float | None = None,
+    ) -> tuple[HandPoseCommand, ...]:
         if self._failed:
-            return
+            return ()
         now = time.monotonic() if now_s is None else float(now_s)
         try:
-            for command in self._mapper.map(
+            commands = self._mapper.map(
                 controller_snapshot=controller_snapshot,
                 hand_snapshot=hand_snapshot,
                 active=active,
                 now_s=now,
-            ):
+            )
+            sent: list[HandPoseCommand] = []
+            for command in commands:
                 self._device.send_pose(command.side, command.pose, force=command.force, reason=command.reason)
+                sent.append(command)
+            return tuple(sent)
         except Exception:
             self._failed = True
             logger.exception("Hand runtime failed; disabling hand control")
             try:
                 self._device.open_all(force=True, reason="failure")
             except Exception:
                 logger.exception("Failed to open hand after hand runtime failure")
+                return ()
+            return self._open_pose_commands("failure")
 
-    def close(self) -> None:
+    def close(self) -> tuple[HandPoseCommand, ...]:
         try:
             self._mapper.close()
         finally:
             self._device.close()
+        return self._open_pose_commands("shutdown")
+
+    def _open_pose_commands(self, reason: str) -> tuple[HandPoseCommand, ...]:
+        return tuple(
+            HandPoseCommand(command.side, command.pose, True, reason)
+            for command in self._open_commands
+        )
 
 
 class DisabledHandRuntime:
     enabled = False
 
-    def start(self) -> None:
-        pass
+    def start(self) -> tuple[HandPoseCommand, ...]:
+        return ()
 
-    def tick(self, *, controller_snapshot: object | None, hand_snapshot: object | None, active: bool, now_s: float | None = None) -> None:
+    def tick(
+        self,
+        *,
+        controller_snapshot: object | None,
+        hand_snapshot: object | None,
+        active: bool,
+        now_s: float | None = None,
+    ) -> tuple[HandPoseCommand, ...]:
         del controller_snapshot, hand_snapshot, active, now_s
+        return ()
 
-    def close(self) -> None:
-        pass
+    def close(self) -> tuple[HandPoseCommand, ...]:
+        return ()
 
 
 def build_hand_runtime(cfg: Any) -> HandRuntime | DisabledHandRuntime:
@@ -80,4 +116,13 @@ def build_hand_runtime(cfg: Any) -> HandRuntime | DisabledHandRuntime:
         device, mapper = build_linkerhand_o6(cfg)
     else:
         raise ValueError(f"Unsupported hands.driver={driver!r}; supported drivers: linkerhand_l6, linkerhand_o6")
-    return HandRuntime(device, mapper)
+    return HandRuntime(device, mapper, open_commands=_open_commands_from_device(device))
+
+
+def _open_commands_from_device(device: HandDevice) -> tuple[HandPoseCommand, ...]:
+    config = getattr(device, "config", None)
+    sides = tuple(str(side).strip().lower() for side in getattr(config, "sides", ()))
+    open_pose = tuple(int(value) for value in getattr(config, "open_pose", ()))
+    if not sides or len(open_pose) != 6:
+        return ()
+    return tuple(HandPoseCommand(side, open_pose, True, "open") for side in sides)

teleopit/sim2real/mp/ipc.py

@@ -12,6 +12,7 @@
 
 BODY_TOPIC = "body"
 HAND_TOPIC = "hand"
+HAND_COMMAND_TOPIC = "hand_command"
 CONTROLLER_TOPIC = "controller"
 CONTROL_EVENTS_TOPIC = "control_events"
 REFERENCE_TOPIC = "reference"
@@ -26,6 +27,7 @@
 class Sim2RealIpcEndpoints:
     body_pub: str
     hand_pub: str
+    hand_command_pub: str
     controller_pub: str
     control_events_pub: str
     reference_pub: str
@@ -43,15 +45,16 @@ def default_endpoints(*, host: str = "127.0.0.1", base_port: int = 39700) -> Sim
     return Sim2RealIpcEndpoints(
         body_pub=f"{prefix}{base_port}",
         hand_pub=f"{prefix}{base_port + 1}",
-        controller_pub=f"{prefix}{base_port + 2}",
-        control_events_pub=f"{prefix}{base_port + 3}",
-        reference_pub=f"{prefix}{base_port + 4}",
-        mode_pub=f"{prefix}{base_port + 5}",
-        video_pub=f"{prefix}{base_port + 6}",
-        record_pub=f"{prefix}{base_port + 7}",
-        health_pub=f"{prefix}{base_port + 8}",
-        command_pub=f"{prefix}{base_port + 9}",
-        reference_command_pub=f"{prefix}{base_port + 10}",
+        hand_command_pub=f"{prefix}{base_port + 2}",
+        controller_pub=f"{prefix}{base_port + 3}",
+        control_events_pub=f"{prefix}{base_port + 4}",
+        reference_pub=f"{prefix}{base_port + 5}",
+        mode_pub=f"{prefix}{base_port + 6}",
+        video_pub=f"{prefix}{base_port + 7}",
+        record_pub=f"{prefix}{base_port + 8}",
+        health_pub=f"{prefix}{base_port + 9}",
+        command_pub=f"{prefix}{base_port + 10}",
+        reference_command_pub=f"{prefix}{base_port + 11}",
     )
 
 

teleopit/sim2real/mp/messages.py

@@ -71,6 +71,17 @@ class RecordStepPacket:
     seq: int
 
 
+@dataclass(frozen=True)
+class HandCommandPacket:
+    timestamp_s: float
+    driver: str
+    mode: str
+    active: bool
+    left_pose: Float64Array
+    right_pose: Float64Array
+    seq: int
+
+
 @dataclass(frozen=True)
 class HealthPacket:
     worker: str