feat(wuji): Genesis-native deploy + sim2real toolchain

examples/wuji/pyproject.toml

@@ -6,6 +6,16 @@ readme = "README.md"
 requires-python = ">=3.12"
 dependencies = ["genelab"]
 
+[project.optional-dependencies]
+# Deploy core (real2sim + policy control). Pure-software pieces tested headlessly.
+deploy = ["onnxruntime", "pyzmq", "scipy"]
+# Vision observer (Hikvision camera -> ZMQ). Hardware-side, not exercised in CI.
+# Also needs the Hikvision MVS SDK (system install, not pip) — see deploy/README.md.
+deploy-vision = ["opencv-contrib-python", "pupil-apriltags", "pyyaml"]
+# Real Wuji hand SDK (compiled wheel; physical hardware). Imported lazily, kept out
+# of `deploy` so the headless core stays binary-free. Pinned to match wuji-mjlab.
+deploy-hand = ["wujihandpy==1.5.1"]
+
 [project.entry-points."genelab.extensions"]
 genelab_wuji = "genelab_wuji.tasks:register"
 

examples/wuji/src/genelab_wuji/deploy/README.md

@@ -0,0 +1,108 @@
+# Wuji-hand deploy (Genesis-native)
+
+A Genesis-native port of the `wuji-mjlab/deploy/reorient` pipeline. Two deliverables:
+
+1. **real2sim** — reproduce the real cube's pose inside the Genesis sim.
+2. **policy deploy** — run an exported ONNX policy to control the (real or mock) hand.
+
+The pieces are decoupled via ZMQ (localhost):
+
+```
+ cube_world_observer ──cube pose (5555)──▶ play_real        (controls the hand)
+  (Hikvision camera)         │             toreal_viewer    (mirrors cube in sim)
+                             └──────────────▶
+ toreal_viewer ──goal (5556)──▶ play_real
+```
+
+## Architecture
+
+| Module | Responsibility | Tested |
+|---|---|---|
+| `frame_transform.py` | wxyz quat math + `cube_cam_to_tag` (camera→wrist-tag lift) | ✅ |
+| `real2sim.py` | `tag_pose_in_world`, `cube_pose_in_tag_to_world` (sim reproduction) | ✅ |
+| `zmq_bridge.py` | cube/goal pub-sub + xyzw↔wxyz + last-valid cache | ✅ |
+| `obs.py` | `DeployObsBuilder` (207-dim policy obs + 3-step history) | ✅ |
+| `action.py` | `ActionProcessor` (offset + clamp + EMA + warmup) | ✅ |
+| `onnx_policy.py` | `ONNXPolicy` (GeneLab metadata format) | ✅ |
+| `hand_driver.py` | `HandDriverBase` / `MockHandDriver` / `WujiHandDriver` | ✅ (mock) |
+| `controller.py` | `DeployController` (closed-loop step) | ✅ |
+| `camera_config.py` | Hikvision intrinsics/ROI/capture from `config/camera.yaml` | glue (hardware) |
+| `cube_geom.py` | cube_tags JSON resolution (`config/cube_tags.json`) | glue |
+| `scripts/hand_utils.py` | `check` (read-only bridge test) / `home` (3s ramp to grasp pose) | glue (hardware) |
+| `scripts/calib_check.py` | static calib viewer: live hand (encoders) + cube vs. digital twin | glue (hardware) |
+| `scripts/play_real.py` | deploy control loop + goal modes + success monitor + Genesis mirror (real/mock) | glue |
+| `scripts/toreal_viewer.py` | real2sim Genesis viewer | glue |
+| `scripts/cube_world_observer.py` | Hikvision camera → ArUco board + SO3 Kalman → ZMQ cube pose | glue (hardware) |
+
+The pure-software core is numpy-only and runs headlessly (no Genesis, no hardware),
+so all frame/obs/action/policy logic is unit-tested in `tests/test_examples_wuji_deploy_*.py`.
+
+### Key conventions
+
+- **Quaternions**: wxyz everywhere internally; the cube wire format is scipy xyzw and
+  is converted at the ZMQ boundary (`cube_pose_from_msg` / `cube_msg_from_pose`).
+- **Tag frame**: the observer reports the cube already in the wrist-AprilTag frame —
+  the exact frame the policy was trained on — so the deploy obs needs **no forward
+  kinematics**.
+- **6D goal error**: matches the GeneLab training encoding (`matrix_to_rotation_6d`,
+  first two matrix rows), pinned against the real training math in the tests.
+- **Joint order**: `finger1_joint1..4, finger2...` (= `wujihandpy` (5,4) row-major), so
+  no remap between policy and hardware.
+
+## Install
+
+```bash
+uv pip install -e 'examples/wuji[deploy]'                 # core (real2sim + control)
+uv pip install -e 'examples/wuji[deploy,deploy-vision]'   # + camera observer
+uv pip install -e 'examples/wuji[deploy,deploy-hand]'     # + real Wuji hand SDK (wujihandpy)
+```
+
+The cube observer also needs the **Hikvision MVS SDK** (system install, not pip — same
+as wuji-mjlab). Install from <https://www.hikrobotics.com> (default `/opt/MVS`) and source
+its environment before running the observer:
+
+```bash
+export MVCAM_COMMON_RUNENV=/opt/MVS/lib
+export LD_LIBRARY_PATH=/opt/MVS/lib/64:/opt/MVS/lib/32:$LD_LIBRARY_PATH
+# (or: source /opt/MVS/bin/set_env_path.sh /opt/MVS)
+# If MvImport lives elsewhere: export MVS_PYTHON_PATH=/path/to/dir/containing/MvImport
+```
+
+## Run
+
+```bash
+# 0) export a trained policy to ONNX
+genelab export Genelab-Reorient-Wuji-Hand-v0 PATH/model.pt --format onnx --out policy.onnx
+
+# 1) smoke-test the control loop, no hardware, no ZMQ, no viewer
+python -m genelab_wuji.deploy.scripts.play_real --ckpt policy.onnx --mock --no-zmq --no-viewer --steps 100
+
+# 1.5) bring up the real hand bridge (needs wujihandpy): check first, then home
+python -m genelab_wuji.deploy.scripts.hand_utils check   # READ-ONLY: connection + encoder sanity
+python -m genelab_wuji.deploy.scripts.hand_utils home    # 3s ease-in-out ramp to the grasp pose
+
+# 2) vision: detect the cube and publish its tag-frame pose on ZMQ:5555 (needs MVS env)
+python -m genelab_wuji.deploy.scripts.cube_world_observer --preview   # terminal A
+python -m genelab_wuji.deploy.scripts.toreal_viewer                   # terminal B (real2sim mirror)
+
+# 2.5) calibration check: home the hand, render live hand + observed cube in the twin
+python -m genelab_wuji.deploy.scripts.calib_check                     # (needs the observer running)
+
+# 3) drive the real hand from the live observer feed (Genesis mirror viewer on by default,
+#    showing the live hand + observed cube + goal; pass --no-viewer for headless).
+#    goal modes: --goal-mode random (uniform-SO3, resampled on success) |
+#                fixed --goal-quat w,x,y,z | external (goal from toreal_viewer ZMQ)
+python -m genelab_wuji.deploy.scripts.play_real --ckpt policy.onnx --real --goal-mode random
+```
+
+`play_real` mirrors the live hand (encoders) + observed cube + goal in a Genesis viewer
+by default (`--no-viewer` to disable). It reuses the same kinematic, physics-free refresh
+as `calib_check`, so the mirror just reflects reality. The control core itself is numpy-only
+and runs headlessly under `--no-viewer`.
+
+The cube observer is a faithful port of the production wuji-mjlab pipeline (Hikvision MVS
+capture, multi-face ArUco board fusion, SO3 Kalman + position low-pass + corner EMA, world
+auto-sampling, fast ROI, OpenCV preview). It publishes the cube pose in the wrist-tag frame
+in the exact same ZMQ schema GeneLab's `CubeReceiver` consumes. Tuning lives in
+`config/observer.yaml`; camera intrinsics/ROI in `config/camera.yaml`; cube tag layout in
+`config/cube_tags.json`. For a non-Hikvision camera, swap the MVS capture in `run()`.

examples/wuji/src/genelab_wuji/deploy/__init__.py

@@ -0,0 +1,8 @@
+"""Genesis-native deployment for the Wuji-hand reorientation policy.
+
+Ports the deployment pipeline from ``wuji-mjlab/deploy/reorient`` onto the GeneLab
+(Genesis) stack: real2sim cube-pose reproduction in sim and ONNX-policy control of
+the hand. The pure-numpy core (frame transforms, ZMQ bridge, obs assembly, ONNX
+wrapper, hand-driver abstraction) is simulator- and hardware-agnostic so it runs
+and tests headlessly; Genesis viewers and real hardware live in ``deploy.scripts``.
+"""

examples/wuji/src/genelab_wuji/deploy/action.py

@@ -0,0 +1,52 @@
+"""Action post-processing for deploy (numpy port of JointPositionOffsetEMAAction).
+
+The policy emits raw actions ~[-1, 1]; the joint target is
+``default + action_scale * clamp(action)``, clamped to joint limits, EMA-smoothed
+against the previous target, and held at the default pose for ``warmup_steps`` after
+each reset. Training-only terms (encoder_bias, action noise) are dropped.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+
+
+class ActionProcessor:
+    """Turn a raw policy action into a smoothed, limit-clamped joint target."""
+
+    def __init__(
+        self,
+        default_joint_pos: np.ndarray,
+        action_scale: float = 0.5,
+        ema_alpha: float = 0.5,
+        warmup_steps: int = 8,
+        joint_pos_limits: tuple[np.ndarray, np.ndarray] | None = None,
+    ) -> None:
+        self._default = np.asarray(default_joint_pos, dtype=float)
+        self.action_scale = action_scale
+        self.ema_alpha = ema_alpha
+        self.warmup_steps = warmup_steps
+        if joint_pos_limits is None:
+            self._lo = np.full_like(self._default, -np.inf)
+            self._hi = np.full_like(self._default, np.inf)
+        else:
+            self._lo = np.asarray(joint_pos_limits[0], dtype=float)
+            self._hi = np.asarray(joint_pos_limits[1], dtype=float)
+        self._prev_target = self._default.copy()
+        self._step = 0
+
+    def reset(self) -> None:
+        """Reset the EMA state and warmup counter (call on episode boundaries)."""
+        self._prev_target = self._default.copy()
+        self._step = 0
+
+    def process(self, action: np.ndarray) -> np.ndarray:
+        """Return the joint target for this control step (JOINT_NAMES_20 order)."""
+        action = np.clip(np.asarray(action, dtype=float), -1.0, 1.0)
+        raw_target = self._default + self.action_scale * action
+        raw_target = np.clip(raw_target, self._lo, self._hi)
+        smoothed = self.ema_alpha * raw_target + (1.0 - self.ema_alpha) * self._prev_target
+        target = self._default.copy() if self._step < self.warmup_steps else smoothed
+        self._prev_target = target.copy()
+        self._step += 1
+        return target

examples/wuji/src/genelab_wuji/deploy/camera_config.py

@@ -0,0 +1,188 @@
+#!/usr/bin/env python3
+# SPDX-License-Identifier: Apache-2.0
+# Copyright 2026 Wuji Technology Co., Ltd.
+# Ported into GeneLab from wuji-mjlab deploy/reorient/lib/camera_config.py
+"""Camera Configuration Loader.
+
+Centralized camera parameters from config/camera.yaml.
+Provides functions for loading camera intrinsics, distortion, and ROI settings.
+
+Example:
+    >>> from camera_config import get_camera_matrix, get_dist_coeffs
+    >>> K = get_camera_matrix()
+    >>> dist = get_dist_coeffs()
+"""
+
+from __future__ import annotations
+
+import os
+from typing import Any
+
+import numpy as np
+import yaml
+
+SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
+CONFIG_FILE = os.path.join(SCRIPT_DIR, "config", "camera.yaml")
+
+def load_camera_config(config_file: str | None = None) -> dict[str, Any]:
+    """Load camera configuration from YAML file.
+
+    Args:
+        config_file: Path to configuration file. If None, uses default.
+
+    Returns:
+        Camera configuration dictionary.
+
+    Raises:
+        FileNotFoundError: If configuration file doesn't exist.
+    """
+    if config_file is None:
+        config_file = CONFIG_FILE
+
+    if not os.path.exists(config_file):
+        raise FileNotFoundError(f"Camera config not found: {config_file}")
+
+    with open(config_file, 'r') as f:
+        cfg = yaml.safe_load(f)
+
+    return cfg
+
+
+def get_camera_matrix(cfg: dict[str, Any] | None = None) -> np.ndarray:
+    """Get camera intrinsic matrix K, adjusted for ROI offset.
+
+    When ROI is set, cx and cy are shifted by the ROI offset so that the
+    intrinsics remain valid for the cropped image.
+
+    Args:
+        cfg: Camera configuration dict. If None, loads from file.
+
+    Returns:
+        3x3 camera intrinsic matrix.
+    """
+    if cfg is None:
+        cfg = load_camera_config()
+
+    intr = cfg['intrinsics']
+    roi = cfg['roi']
+    K = np.array([
+        [intr['fx'], 0, intr['cx'] - roi['offset_x']],
+        [0, intr['fy'], intr['cy'] - roi['offset_y']],
+        [0, 0, 1]
+    ], dtype=np.float64)
+    return K
+
+
+def get_dist_coeffs(cfg: dict[str, Any] | None = None) -> np.ndarray:
+    """Get distortion coefficients.
+
+    Args:
+        cfg: Camera configuration dict. If None, loads from file.
+
+    Returns:
+        Distortion coefficients array [k1, k2, p1, p2, k3].
+    """
+    if cfg is None:
+        cfg = load_camera_config()
+
+    dist = cfg['distortion']
+    return np.array([
+        dist['k1'], dist['k2'], dist['p1'], dist['p2'], dist['k3']
+    ], dtype=np.float64)
+
+
+def get_roi(cfg: dict[str, Any] | None = None) -> tuple[int, int, int, int]:
+    """Get ROI parameters.
+
+    Args:
+        cfg: Camera configuration dict. If None, loads from file.
+
+    Returns:
+        Tuple of (offset_x, offset_y, width, height).
+    """
+    if cfg is None:
+        cfg = load_camera_config()
+
+    roi = cfg['roi']
+    return roi['offset_x'], roi['offset_y'], roi['width'], roi['height']
+
+
+def get_capture_settings(cfg: dict[str, Any] | None = None) -> dict[str, Any]:
+    """Get camera capture settings.
+
+    Args:
+        cfg: Camera configuration dict. If None, loads from file.
+
+    Returns:
+        Dictionary with exposure_time, gain, and frame_rate.
+    """
+    if cfg is None:
+        cfg = load_camera_config()
+
+    cap = cfg['capture']
+    return {
+        'exposure_time': cap['exposure_time'],
+        'gain': cap['gain'],
+        'frame_rate': cap.get('frame_rate', 0),
+    }
+
+
+def setup_camera_roi(cam: Any, cfg: dict[str, Any] | None = None) -> tuple[int, int]:
+    """Setup camera ROI from config.
+
+    Args:
+        cam: MvCamera instance.
+        cfg: Camera configuration dict. If None, loads from file.
+
+    Returns:
+        Tuple of (width, height) of the configured ROI.
+    """
+    offset_x, offset_y, width, height = get_roi(cfg)
+    cam.MV_CC_SetIntValueEx("OffsetX", offset_x)
+    cam.MV_CC_SetIntValueEx("OffsetY", offset_y)
+    cam.MV_CC_SetIntValueEx("Width", width)
+    cam.MV_CC_SetIntValueEx("Height", height)
+    print(f"Camera ROI: {width}x{height} @ ({offset_x}, {offset_y})")
+    return width, height
+
+
+def setup_camera_capture(cam: Any, cfg: dict[str, Any] | None = None) -> None:
+    """Setup camera capture settings from config.
+
+    Args:
+        cam: MvCamera instance.
+        cfg: Camera configuration dict. If None, loads from file.
+    """
+    settings = get_capture_settings(cfg)
+    cam.MV_CC_SetFloatValue("ExposureTime", settings['exposure_time'])
+    cam.MV_CC_SetFloatValue("Gain", settings['gain'])
+    # Frame rate: enable explicit control and set target
+    frame_rate = settings.get('frame_rate', 0)
+    if frame_rate and frame_rate > 0:
+        ret1 = cam.MV_CC_SetBoolValue("AcquisitionFrameRateEnable", True)
+        ret2 = cam.MV_CC_SetFloatValue("AcquisitionFrameRate", float(frame_rate))
+        # Read back actual resulting frame rate
+        from ctypes import c_float, byref
+        actual_fps = c_float(0)
+        ret3 = cam.MV_CC_GetFloatValue("ResultingFrameRate", actual_fps)
+        if ret3 == 0:
+            actual_str = f", actual={actual_fps.value:.1f}Hz"
+        else:
+            actual_str = ", actual=unknown"
+        print(f"Camera capture: exposure={settings['exposure_time']}us, gain={settings['gain']}, "
+              f"frame_rate={frame_rate}Hz (enable_ret=0x{ret1:X}, set_ret=0x{ret2:X}{actual_str})")
+    else:
+        print(f"Camera capture: exposure={settings['exposure_time']}us, gain={settings['gain']}, frame_rate=default")
+
+
+if __name__ == "__main__":
+    print("=" * 50)
+    print("Camera Config Test")
+    print("=" * 50)
+
+    cfg = load_camera_config()
+    print("\nCamera Config loaded:")
+    print(f"  ROI: {get_roi(cfg)}")
+    print(f"  K:\n{get_camera_matrix(cfg)}")
+    print(f"  Dist: {get_dist_coeffs(cfg)}")
+    print(f"  Capture: {get_capture_settings(cfg)}")