"""Bimanual exoskeleton scene-camera calibration for the YAM dual-arm rig. Sibling of :file:`exo_calibrate.py` — that script handles ONE arm at a time (``--arm right`` or ``--arm left``). This one solves the bimanual problem: given ONE scene camera looking at BOTH YAM arms simultaneously, detect both base ArUco boards in the same frame and recover BOTH ``T_cam_in_arm_base`` extrinsics in one calibration session. Why a separate script? ---------------------- The single-arm version makes several decisions that pin it to one arm: * opens ``YAM_BASE_ONLY_CONFIG`` (arm 1, board IDs 217–225) only * spins up ``RobotController`` with one leader + one follower * builds a single-arm mujoco model (``exo_cfg.xml``) * writes a single ``cameras[scene_cam_name]`` record to ``CALIBRATION_FILE`` The bimanual rig needs all of those generalised: * DETECT both boards every frame — arm 1's ``larger_coarse_board`` (IDs 217–225) and arm 2's ``larger_coarse_board_arm2`` (IDs 226–234). Both boards are 3×3 grids of the same physical 93.5 mm edge length, so the same ``yam_base_board_v2.stl`` mount fits either arm; only the marker IDs printed on the board distinguish them. A single ``cv2.aruco .detectMarkers`` pass returns ALL ids in the frame; ``do_est_aruco_pose`` filters per-board via ``getBoardObjectAndImagePoints`` (id ∩ board ids), so we just call it twice with the cached corners. * TELEOP both arms simultaneously — ``RobotController(use_right_leader= True, use_left_leader=True, use_right_follower=True, use_left_follower= True)``. Both followers stream proprio, and we drive both rendered arms off them so the user can swing each leader independently and watch both rendered overlays track in real time. * RENDER both arms in one mujoco scene. We use ``Demos.sim_yam_bimanual.build_bimanual_xml`` to emit a wrapper XML containing one copy of yam.xml's defaults/assets plus two arm body subtrees (arm 2's names are ``arm2_``-prefixed to avoid collisions), plus the exo + ArUco mocap bodies from BOTH configs. The arms sit at world y = ∓margin/2 in the bimanual XML, so we have to do one frame shift to express the camera in the bimanual-world frame (see below). Frame conventions — read this once ---------------------------------- Per-arm ArUco detection gives ``T_aruco_in_cam``; chaining through ``link_to_aruco_transform(link_cfg)`` (the rigid board→link offset baked into the printed board's mount) gives ``T_arm_base_in_cam``, and inverting yields ``T_cam_in_arm{i}_base``. This is what raiden ultimately stores per arm. For RENDERING, we need the camera in the *bimanual-world* frame because the mujoco model places arm 1 at ``(0, -margin/2, 0)`` and arm 2 at ``(0, +margin/2, 0)``. The relation is T_cam_in_world = T_arm1_in_world @ T_cam_in_arm1_base = T_arm2_in_world @ T_cam_in_arm2_base where ``T_armN_in_world`` is a pure translation along world Y. The two expressions must agree (up to detection noise) — if they diverge by more than a few mm, the rig moved between detections or one of the boards is poorly fit. We pick arm 1's calibration as the canonical render pose (documented choice — first board wins; arm 2 is the consistency check) and log the divergence as a diagnostic scalar. Per-arm calibration save ------------------------ The existing ``_save_calibration`` writes ``cal["cameras"][scene_cam_name] = {...}``: keyed by camera, NOT by arm. Calling it twice with the same ``scene_cam_name`` would clobber arm 1's record when arm 2 saves. We're told NOT to modify ``_save_calibration``, so the workaround here is to call it twice with DERIVED camera keys: cal["cameras"][f"{scene_cam_name}__arm1"] ← arm 1 extrinsics cal["cameras"][f"{scene_cam_name}__arm2"] ← arm 2 extrinsics Downstream raiden code that reads ``cameras["scene_camera"]`` will need to know about the ``__arm1`` / ``__arm2`` suffix convention. That's intentional: there is no canonical scene-camera-in-base for a bimanual rig (the question depends on WHICH base you're standing in), so the caller has to pick. The single-arm script and the bimanual script can both live in the same calibration file without collision. Leader/follower mapping ----------------------- Arm 1 (mujoco y = -margin/2) ← driven by ``robot_controller.follower_r`` Arm 2 (mujoco y = +margin/2) ← driven by ``robot_controller.follower_l`` This mirrors raiden's "right arm = arm 1, left arm = arm 2" convention that the dataset recorder uses. If you swap the CAN cabling you'll have to swap the mapping here too. Lock policy (v1) ---------------- A single yellow-button press toggles pose-lock for BOTH arms together. This is the simplest thing that gives you the teleop-on-frozen-pose workflow the single-arm version supports. A v2 would split the lock per arm (e.g. yellow on the right leader locks arm 1 only); leave as a TODO. Keyboard: ``y`` → save BOTH arms' calibrations to the same scene_cam_name key family (``__arm1``, ``__arm2``); ``q`` → quit without saving. Usage: python -m raiden.calibration.exo_calibrate_bimanual python -m raiden.calibration.exo_calibrate_bimanual --margin 0.7 --resolution HD1080 python -m raiden.calibration.exo_calibrate_bimanual --no-teleop # render-only """ import os # MUJOCO_GL must be set BEFORE any module that imports mujoco. Repeat here # (it's harmless if already set by the sibling import) because users may # run this script directly without exo_calibrate.py being loaded first. os.environ.setdefault("MUJOCO_GL", "egl") os.environ.setdefault("PYOPENGL_PLATFORM", "egl") import copy import json import re import sys import threading # Mutex serialising MuJoCo render calls across capture threads — multiple # capture threads each own a mujoco.Renderer + EGL context but the EGL # display itself is process-global; concurrent eglMakeCurrent raises # EGL_BAD_ACCESS. Serialising the render block costs ~half the per-cam # frame rate (well within budget for calibration), eliminating the race. _MJ_RENDER_LOCK = threading.Lock() import sys as _sys, traceback as _tb def _thread_exc(args): print("[THREAD EXCEPTION in", args.thread.name, "]", flush=True) _tb.print_exception(args.exc_type, args.exc_value, args.exc_traceback, file=_sys.stderr) threading.excepthook = _thread_exc import time from datetime import datetime from pathlib import Path from typing import Optional, Tuple, List from urllib.parse import quote import cv2 import mujoco import numpy as np # Self-locate the same way exo_calibrate.py does so we can import # third_party/exo_redo helpers and the build_bimanual_xml emitter. _RAIDEN_ROOT = Path(__file__).resolve().parents[2] _EXO_DIR = _RAIDEN_ROOT / "third_party" / "exo_redo" if str(_EXO_DIR) not in sys.path: sys.path.insert(0, str(_EXO_DIR)) # Reuse every helper from the single-arm module — do NOT duplicate. Anything # we'd otherwise re-implement (ZED open, JPEG-encode-for-rerun, render-with- # off-center-K warp, multi-frame pose pooling, save_calibration, rerun init, # _PoseHistory, _DetectionState) comes straight from there. from raiden.calibration.exo_calibrate_fidex import ( _CACHED_RENDERER, # noqa: F401 — referenced via name only when debugging _DetectionState, _PoseHistory, _compose_overlay, _draw_aruco_plane, _grab_bgr, _init_rerun, _open_zed_native, _render_with_intrinsics, _save_calibration, _scene_serial_from_config, _solve_multiframe_pose, _to_rerun_jpeg, ) # --------------------------------------------------------------------------- # Bimanual-specific helpers # --------------------------------------------------------------------------- # ArUco ID partitioning. These ranges are baked into the printed boards # (see ExoConfigs/panda_exo.py:33,53). If you mint a third board you'll # need to extend this map. _ARM1_IDS = set(range(217, 217 + 9)) # larger_coarse_board (arm 1) _ARM2_IDS = set(range(226, 226 + 9)) # larger_coarse_board_arm2 (arm 2) def _partition_corners(corners_all, ids_all): """Split a single ``cv2.aruco.detectMarkers`` result into per-arm ``(corners, ids, rejected)`` tuples that ``do_est_aruco_pose`` can consume via ``corners_est=``. Returns ``(arm1_corners_est, arm2_corners_est)`` where each is either a ``(corners_subset, ids_subset, [])`` tuple or ``None`` if no marker from that arm's ID range was detected. ``ids_subset`` is shaped ``(N, 1)`` (matching cv2's convention) so ``getBoardObjectAndImage Points`` doesn't choke. Why the partitioning matters: with one detect call returning ALL ids, passing the full result to each per-arm ``do_est_aruco_pose`` would still work (``getBoardObjectAndImagePoints`` filters by id ∩ board .getIds()), but we split anyway so we get clean per-arm corner/id arrays for our own diagnostics. It also avoids re-running detection twice on HD1080, which would otherwise cost ~30 ms × 2. """ if ids_all is None: return None, None ids_flat = ids_all.flatten() arm1_mask = np.array([int(i) in _ARM1_IDS for i in ids_flat]) arm2_mask = np.array([int(i) in _ARM2_IDS for i in ids_flat]) def _make(mask): if not mask.any(): return None # corners_all is a tuple/list of (1,4,2) arrays — index by mask. sub_corners = [corners_all[i] for i in np.where(mask)[0]] sub_ids = ids_all[mask].reshape(-1, 1) return (sub_corners, sub_ids, []) return _make(arm1_mask), _make(arm2_mask) def _T_translate_y(dy: float) -> np.ndarray: """4x4 transform that translates along world Y by ``dy``. Used to lift a per-arm ``T_cam_in_arm_base`` into the bimanual-world frame, where arm 1 sits at world y = -margin/2 and arm 2 at world y = +margin/2. """ T = np.eye(4) T[1, 3] = dy return T def _save_bimanual_calibration( scene_cam_name: str, K: np.ndarray, dist: np.ndarray, T_cam_in_arm1_base: Optional[np.ndarray], T_cam_in_arm2_base: Optional[np.ndarray], calibration_path: Path, resolution: str, ) -> None: """Save BOTH arm calibrations to ``CALIBRATION_FILE``. We do NOT modify ``_save_calibration``. Instead we call it twice with DERIVED scene_cam_name keys (``..__arm1``, ``..__arm2``) so the two records don't collide. The arm_label inside each record (``right`` for arm 1, ``left`` for arm 2) matches raiden's bimanual convention. Either of the two T's may be ``None`` if that board was never detected during the session — we save whichever ones we have. """ saved_any = False # Write THREE entries into `cameras`: # - {scene_cam_name}__arm1 (right_arm_base reference) # - {scene_cam_name}__arm2 (left_arm_base reference) # - {scene_cam_name} (plain key, == __arm2 content — what # LiveVisualizer reads; skips __armN # via its own filter) # This matches Shun's "store both arm poses" convention so we never # rely on a separate bimanual_transform to recover the missing arm's # frame. if T_cam_in_arm1_base is not None: _save_calibration( scene_cam_name=f"{scene_cam_name}__arm1", K=K, dist=dist, T_cam_in_base=T_cam_in_arm1_base, calibration_path=calibration_path, arm_label="right", resolution=resolution, ) saved_any = True if T_cam_in_arm2_base is not None: _save_calibration( scene_cam_name=f"{scene_cam_name}__arm2", K=K, dist=dist, T_cam_in_base=T_cam_in_arm2_base, calibration_path=calibration_path, arm_label="left", resolution=resolution, ) _save_calibration( scene_cam_name=scene_cam_name, K=K, dist=dist, T_cam_in_base=T_cam_in_arm2_base, calibration_path=calibration_path, arm_label="left", resolution=resolution, ) saved_any = True if not saved_any: return # Post-pass: add bimanual_transform.right_base_to_left_base when both # arm solves are available, and inject image_size into the intrinsics # of every entry we just wrote so LiveVisualizer's point-cloud # unprojection scales fx/fy correctly. Read-modify-write the JSON # in-place — _save_calibration writes a per-camera record but doesn't # touch the sibling fields we need here. try: import json as _json from pathlib import Path as _Path _cal_path = _Path(calibration_path) _cal = _json.load(open(_cal_path)) if _cal_path.exists() else {} _cams = _cal.setdefault("cameras", {}) # image_size on every entry we touched this round. _RES_PX = {"HD720": [1280, 720], "HD1080": [1920, 1080], "HD2K": [2208, 1242]} _img_size = _RES_PX.get(resolution, [1920, 1080]) for _name in (scene_cam_name, f"{scene_cam_name}__arm1", f"{scene_cam_name}__arm2"): if _name in _cams: _cams[_name].setdefault("intrinsics", {})["image_size"] = _img_size # bimanual_transform derived from this camera's two arm solves. # LiveVisualizer reads bimanual.get("right_base_to_left_base") and # stores its inverse as the right-arm world pose. The matrix we # store is therefore T_left_in_right; inverting yields # T_right_in_left (right arm in left/world). if T_cam_in_arm1_base is not None and T_cam_in_arm2_base is not None: _T_right_in_left = (T_cam_in_arm2_base @ np.linalg.inv(T_cam_in_arm1_base)) _T_left_in_right = np.linalg.inv(_T_right_in_left) _cal["bimanual_transform"] = { "right_base_to_left_base": _T_left_in_right.tolist(), "method": f"derived from {scene_cam_name} __arm1/__arm2 PnP pair", "translation_m_right_base_in_left_base": _T_right_in_left[:3, 3].tolist(), } print(f" • wrote bimanual_transform " f"(right_in_left t = {_T_right_in_left[:3, 3]})") _json.dump(_cal, open(_cal_path, "w"), indent=2) except Exception as _exc: print(f" ⚠ post-save JSON patch failed: {_exc}") if not saved_any: print(" ⚠ no valid detections to save (neither board ever fit)") # --------------------------------------------------------------------------- # Per-arm detection + history state # --------------------------------------------------------------------------- class _BimanualState: """Per-arm detection state holder, thread-safe. Wraps two ``_DetectionState`` snapshots (one per arm) so the main thread can ``snapshot()`` both at save time without racing the capture thread. ``pose_locked`` is shared across both arms (v1 lock policy — see module docstring TODO). """ def __init__(self): self.arm1 = _DetectionState() self.arm2 = _DetectionState() # ``stop_event`` is shared because the capture loop watches one # event — we mirror it onto each sub-state so callers using # state.arm1.stop_event also work, although today nothing does. self.stop_event = threading.Event() self.arm1.stop_event = self.stop_event self.arm2.stop_event = self.stop_event # Shared lock across BOTH arms for v1. TODO: per-arm locking. self.pose_locked = False # Per-arm frozen pose snapshots (only consulted while locked). self.locked_T_aruco_in_cam_arm1: Optional[np.ndarray] = None self.locked_T_aruco_in_cam_arm2: Optional[np.ndarray] = None # Per-arm pose histories (populated by capture loop after init). self.history_arm1: Optional[_PoseHistory] = None self.history_arm2: Optional[_PoseHistory] = None # ``accumulating`` gates whether per-frame ArUco detections get # *added* to the per-arm ``_PoseHistory`` queues. We start OFF # so the user can move the arms' grippers out of the way of the # base boards *before* recording: at start-up the grippers # typically occlude the boards and any frames pooled here would # be useless. Press ``c`` in the main input loop to clear both # histories AND flip this to True (i.e. start recording). Press # ``c`` again to pause without clearing. Live single-frame # detection + multi-frame solve over whatever's in the history # continue regardless — this only controls the *write* side. self.accumulating = False # --------------------------------------------------------------------------- # Capture / detect / log loop (bimanual) # --------------------------------------------------------------------------- def _capture_loop_bimanual( cam, K, dist, W, H, exo_cfg_arm1, exo_cfg_arm2, model, data, rr, state: _BimanualState, margin: float, solve_intrinsics: bool = False, history_size: int = 30, top_k: int = 10, robot_controller=None, arm1_qpos_idx=None, arm1_grip_qpos_idx=None, arm2_qpos_idx=None, arm2_grip_qpos_idx=None, viz_width: int = 1280, viz_hz: float = 10.0, jpeg_quality: int = 75, rerun_prefix: str = "scene", ): """Bimanual variant of ``_capture_loop`` — same shape, but with two boards, two histories, two follower→qpos mappings, and a render that composes both arms in a single mujoco scene. ╔══════════════════════════════════════════════════════════════════════╗ ║ Frame-of-reference reminder (do NOT confuse these): ║ ║ ║ ║ - T_aruco_in_cam{1,2} — board pose in camera frame (raw PnP out). ║ ║ - T_link_in_cam{1,2} — arm base pose in camera frame ║ ║ (= T_aruco_in_cam @ T_aruco_to_link). ║ ║ - T_cam_in_arm{1,2}_base — what we ultimately SAVE per arm. ║ ║ - T_cam_in_world — what we feed the bimanual RENDERER ║ ║ (= T_arm1_in_world @ T_cam_in_arm1_base)║ ║ ║ ║ We render using arm 1's calibration (documented choice). Arm 2's ║ ║ calibration is a consistency cross-check, logged as scalar diverg. ║ ╚══════════════════════════════════════════════════════════════════════╝ """ from ExoConfigs.exoskeleton import link_to_aruco_transform from exo_utils import ( do_est_aruco_pose, ARUCO_DICT, position_exoskeleton_meshes, get_link_poses_from_robot, ) from scipy.spatial.transform import Rotation as _R # MuJoCo body id for arm-2's root; cached once. ``build_bimanual_xml`` # injects this body at a fixed initial pos (y=+margin/2). We OVERWRITE # that pos+quat every frame from the live per-arm detection delta — # see the per-frame block below. arm2_body_id = model.body("arm2_arm").id # mujoco's EGL context is thread-bound; create one explicitly in THIS # daemon thread before any Renderer (mirror of the single-arm setup). try: _gl_ctx = mujoco.GLContext(W, H) _gl_ctx.make_current() print(f"[exo_calibrate_bimanual] daemon-thread GL context ({W}x{H})") except Exception as _exc: print(f"[exo_calibrate_bimanual] daemon GL context failed: {_exc}") _gl_ctx = None # Resolve the per-arm link configs and link→aruco transforms once. link_cfg_arm1 = exo_cfg_arm1.links["larger_coarse_board"] link_cfg_arm2 = exo_cfg_arm2.links["larger_coarse_board_arm2"] aruco_board_arm1 = exo_cfg_arm1.aruco_board_objects["larger_coarse_board"] aruco_board_arm2 = exo_cfg_arm2.aruco_board_objects["larger_coarse_board_arm2"] board_length_arm1 = link_cfg_arm1.board_length board_length_arm2 = link_cfg_arm2.board_length T_link_to_aruco_arm1 = link_to_aruco_transform(link_cfg_arm1) T_aruco_to_link_arm1 = np.linalg.inv(T_link_to_aruco_arm1) T_link_to_aruco_arm2 = link_to_aruco_transform(link_cfg_arm2) T_aruco_to_link_arm2 = np.linalg.inv(T_link_to_aruco_arm2) # Per-arm sliding-window histories (independent — each arm's board # might be visible in different frames). history_arm1 = _PoseHistory(max_size=history_size) history_arm2 = _PoseHistory(max_size=history_size) state.history_arm1 = history_arm1 state.history_arm2 = history_arm2 # World-frame transforms: arm 1 at y=-margin/2, arm 2 at y=+margin/2 # (matches build_bimanual_xml's arm-root injection). T_arm1_in_world = _T_translate_y(-margin / 2.0) T_arm2_in_world = _T_translate_y(+margin / 2.0) print(f"[exo_calibrate_bimanual] multi-frame robust pose: " f"history={history_size}, pool top-{top_k}; " f"solve_intrinsics={solve_intrinsics}; margin={margin:.3f} m") if robot_controller is not None: print(f"[exo_calibrate_bimanual] teleop enabled (BOTH arms); press " f"the YELLOW (top) leader button on EITHER leader to toggle " f"the shared pose-lock. v1 lock policy: both arms freeze " f"together. TODO: per-arm lock.") frame_idx = 0 last_log_t = 0.0 LOG_INTERVAL_S = 1.0 / max(viz_hz, 0.1) while not state.stop_event.is_set(): bgr = _grab_bgr(cam) if bgr is None: time.sleep(0.005) continue # SINGLE detectMarkers call per frame — split the result by id range. # We bypass do_est_aruco_pose's internal detect by passing corners_est=. try: gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY) corners_all, ids_all, rejected_all = cv2.aruco.detectMarkers( gray, ARUCO_DICT, parameters=cv2.aruco.DetectorParameters(), ) except Exception: corners_all, ids_all, rejected_all = ([], None, []) arm1_corners_est, arm2_corners_est = _partition_corners(corners_all, ids_all) # Per-arm pose solve — separate helper so we don't duplicate the # corner-vs-center-shift gymnastics twice in this function body. T_cam_in_arm1_base, T_link_arm1_in_cam, multi_resid_arm1, n_used_arm1 = ( _solve_one_arm( bgr, arm1_corners_est, aruco_board_arm1, board_length_arm1, T_aruco_to_link_arm1, K, dist, solve_intrinsics, history_arm1, top_k, pose_locked=state.pose_locked, locked_T_aruco_in_cam=state.locked_T_aruco_in_cam_arm1, accumulating=state.accumulating, )) T_cam_in_arm2_base, T_link_arm2_in_cam, multi_resid_arm2, n_used_arm2 = ( _solve_one_arm( bgr, arm2_corners_est, aruco_board_arm2, board_length_arm2, T_aruco_to_link_arm2, K, dist, solve_intrinsics, history_arm2, top_k, pose_locked=state.pose_locked, locked_T_aruco_in_cam=state.locked_T_aruco_in_cam_arm2, accumulating=state.accumulating, )) # Update per-arm DetectionState snapshots (for save-on-Y). if T_cam_in_arm1_base is not None: state.arm1.update(K, dist, T_cam_in_arm1_base) if T_cam_in_arm2_base is not None: state.arm2.update(K, dist, T_cam_in_arm2_base) # Yellow-button lock toggle. RobotController.check_button_press() # returns the first leader's press (raiden polls both); the single- # arm script treats any press as a toggle, so we do the same. if robot_controller is not None: if robot_controller.check_button_press() is not None: state.pose_locked = not state.pose_locked if state.pose_locked: # Snapshot WHATEVER each arm's current best pose is. # If an arm has no detection right now, its lock stays # None → solver falls through to live tracking for # that arm (graceful asymmetry, by design). state.locked_T_aruco_in_cam_arm1 = ( (T_link_arm1_in_cam @ np.linalg.inv(T_aruco_to_link_arm1)).copy() if T_link_arm1_in_cam is not None else None) state.locked_T_aruco_in_cam_arm2 = ( (T_link_arm2_in_cam @ np.linalg.inv(T_aruco_to_link_arm2)).copy() if T_link_arm2_in_cam is not None else None) print(f"\n[exo_calibrate_bimanual] POSE LOCKED — both " f"arms frozen. Drive either leader to compare " f"the rendered arms to reality. Yellow again " f"to unlock.") else: state.locked_T_aruco_in_cam_arm1 = None state.locked_T_aruco_in_cam_arm2 = None print(f"\n[exo_calibrate_bimanual] pose UNLOCKED " f"— tracking resumed on both arms.") # Drive both rendered arms from their respective followers. if robot_controller is not None: _drive_arm_qpos(robot_controller.follower_r, data, arm1_qpos_idx, arm1_grip_qpos_idx) _drive_arm_qpos(robot_controller.follower_l, data, arm2_qpos_idx, arm2_grip_qpos_idx) # ── Re-anchor arm 2 from the LIVE detection delta ───────────────────── # ``build_bimanual_xml`` compiles arm 2 at a fixed (0, +margin/2, 0). # That margin is a guess; the physical rig spacing isn't always # exactly ``--margin``. The CORRECT arm-2 placement in world frame # comes from the two per-arm board calibrations: # # T_arm2_in_arm1 = T_cam_in_arm1 @ inv(T_cam_in_arm2) # T_arm2_in_world = T_arm1_in_world @ T_arm2_in_arm1 # # MuJoCo's ``mjModel.body_pos`` / ``body_quat`` are writable on the # already-compiled model — overwriting them and calling mj_forward # propagates the new base pose through the kinematic tree (no XML # recompile needed). We then re-run ``position_exoskeleton_meshes`` # for arm 2 so the exo + ArUco mocap meshes follow. # # Only applied when BOTH arms detected this frame; on a missed # arm-2 frame we hold the last known body_pos/quat so the render # doesn't snap back to the default margin offset. if T_cam_in_arm1_base is not None and T_cam_in_arm2_base is not None: T_arm2_in_arm1 = T_cam_in_arm1_base @ np.linalg.inv(T_cam_in_arm2_base) T_arm2_in_world_now = T_arm1_in_world @ T_arm2_in_arm1 model.body_pos[arm2_body_id] = T_arm2_in_world_now[:3, 3] q_xyzw = _R.from_matrix(T_arm2_in_world_now[:3, :3]).as_quat() model.body_quat[arm2_body_id] = q_xyzw[[3, 0, 1, 2]] # mujoco = wxyz mujoco.mj_forward(model, data) # Arm 2's base just shifted — its exo + ArUco mocap meshes were # parented to the old body pose at startup, so re-attach them. # Arm 1's exo doesn't need updating (its body root is fixed at # y=-margin/2 by construction). We only re-position arm 2. if T_cam_in_arm1_base is not None and T_cam_in_arm2_base is not None: try: position_exoskeleton_meshes( exo_cfg_arm2, model, data, get_link_poses_from_robot(exo_cfg_arm2, model, data)) except Exception: # Don't kill the capture loop on a transient exo-attach # error — the next frame will retry. pass # ── 2D annotated frame ─────────────────────────────────────────────── # Draw markers for BOTH boards on a single image. We use cv2.aruco's # default annotator since we want one combined image, and add per-arm # board-rectangle outlines (yellow for arm 1, magenta for arm 2) plus # axes via cv2.drawFrameAxes when we have a pose. aruco_bgr = bgr.copy() if ids_all is not None: cv2.aruco.drawDetectedMarkers(aruco_bgr, corners_all, ids_all) if T_link_arm1_in_cam is not None: T_aruco_in_cam_arm1 = T_link_arm1_in_cam @ T_link_to_aruco_arm1 _draw_aruco_plane(aruco_bgr, T_aruco_in_cam_arm1, board_length_arm1, K, color=(0, 255, 255)) # yellow if T_link_arm2_in_cam is not None: T_aruco_in_cam_arm2 = T_link_arm2_in_cam @ T_link_to_aruco_arm2 _draw_aruco_plane(aruco_bgr, T_aruco_in_cam_arm2, board_length_arm2, K, color=(255, 0, 255)) # magenta # ── mujoco render in bimanual-world frame ───────────────────────────── # We use arm 1's calibration to anchor the render. Reason: arm 1 is # the first board and the dataset convention's "right" arm; if its # board ever fails, fall back to arm 2 + offset by margin along -y. overlay_bgr = aruco_bgr T_cam_in_world: Optional[np.ndarray] = None if T_cam_in_arm1_base is not None: T_cam_in_world = T_arm1_in_world @ T_cam_in_arm1_base elif T_cam_in_arm2_base is not None: T_cam_in_world = T_arm2_in_world @ T_cam_in_arm2_base if T_cam_in_world is not None: try: # _render_with_intrinsics wants T_link_in_cam where 'link' # is the body the rendering camera's pose is relative to — # i.e. world for the bimanual model. T_link_in_cam = inv # (T_cam_in_world). T_world_in_cam = np.linalg.inv(T_cam_in_world) with _MJ_RENDER_LOCK: rendered_rgb, seg_mask = _render_with_intrinsics( model, data, T_world_in_cam, K, W, H) overlay_rgb = _compose_overlay(bgr, rendered_rgb, seg_mask=seg_mask) overlay_bgr = cv2.cvtColor(overlay_rgb, cv2.COLOR_RGB2BGR) # Re-draw per-arm board planes on top of the alpha-composite # so the user can see how well the cv2 PnP pose matches the # rendered (mujoco-projected) board mesh. Green = cv2 truth. if T_link_arm1_in_cam is not None: _draw_aruco_plane( overlay_bgr, T_link_arm1_in_cam @ T_link_to_aruco_arm1, board_length_arm1, K, color=(0, 255, 0)) if T_link_arm2_in_cam is not None: _draw_aruco_plane( overlay_bgr, T_link_arm2_in_cam @ T_link_to_aruco_arm2, board_length_arm2, K, color=(0, 255, 0)) # Red contour of rendered silhouette. fg_mask = (seg_mask[:, :, 0] != -1).astype(np.uint8) * 255 contours, _ = cv2.findContours( fg_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cv2.drawContours(overlay_bgr, contours, -1, (0, 0, 255), 2, cv2.LINE_AA) except Exception: if not getattr(_capture_loop_bimanual, "_render_err_logged", False): import traceback print(f"\n[exo_calibrate_bimanual] mujoco render failed:") traceback.print_exc() print(f"[exo_calibrate_bimanual] falling back to ArUco-only overlay\n") _capture_loop_bimanual._render_err_logged = True # ── Status text + scalars ──────────────────────────────────────────── lock_str = "[LOCKED]" if state.pose_locked else "[tracking]" t1 = (T_cam_in_arm1_base[:3, 3] if T_cam_in_arm1_base is not None else np.full(3, np.nan)) t2 = (T_cam_in_arm2_base[:3, 3] if T_cam_in_arm2_base is not None else np.full(3, np.nan)) # Cross-arm consistency: project arm 2's cam-in-base into bimanual # world and compare to arm 1's projection. They SHOULD agree. consistency_mm = float("nan") if T_cam_in_arm1_base is not None and T_cam_in_arm2_base is not None: p1 = (T_arm1_in_world @ T_cam_in_arm1_base)[:3, 3] p2 = (T_arm2_in_world @ T_cam_in_arm2_base)[:3, 3] consistency_mm = float(np.linalg.norm(p1 - p2) * 1000) info_text = ( f"{lock_str} margin={margin:.3f}m\n" f" arm1: cam_in_base=({t1[0]:+.3f},{t1[1]:+.3f},{t1[2]:+.3f}) " f"{state.arm1.stability_str()} n_used={n_used_arm1} " f"multi_resid={multi_resid_arm1:.2f}px {history_arm1.stats()}\n" f" arm2: cam_in_base=({t2[0]:+.3f},{t2[1]:+.3f},{t2[2]:+.3f}) " f"{state.arm2.stability_str()} n_used={n_used_arm2} " f"multi_resid={multi_resid_arm2:.2f}px {history_arm2.stats()}\n" f" cross-arm consistency (mm): {consistency_mm:.1f} " f"(=|arm1_in_world − arm2_in_world|)" ) rr.set_time("step", sequence=frame_idx) if T_cam_in_arm1_base is not None: rr.log(f"{rerun_prefix}/arm1_cam_in_base_x", rr.Scalars(float(t1[0]))) rr.log(f"{rerun_prefix}/arm1_cam_in_base_y", rr.Scalars(float(t1[1]))) rr.log(f"{rerun_prefix}/arm1_cam_in_base_z", rr.Scalars(float(t1[2]))) if T_cam_in_arm2_base is not None: rr.log(f"{rerun_prefix}/arm2_cam_in_base_x", rr.Scalars(float(t2[0]))) rr.log(f"{rerun_prefix}/arm2_cam_in_base_y", rr.Scalars(float(t2[1]))) rr.log(f"{rerun_prefix}/arm2_cam_in_base_z", rr.Scalars(float(t2[2]))) if not np.isnan(consistency_mm): rr.log(f"{rerun_prefix}/cross_arm_consistency_mm", rr.Scalars(float(consistency_mm))) now = time.monotonic() if now - last_log_t >= LOG_INTERVAL_S: last_log_t = now rr.set_time("step", sequence=frame_idx) # JPEG-encode + resize before logging (lifted directly from the # single-arm script — same backlog avoidance argument). rr.log(f"{rerun_prefix}/rgb_aruco", _to_rerun_jpeg(aruco_bgr, target_w=viz_width, quality=jpeg_quality)) rr.log(f"{rerun_prefix}/rgb_overlay", _to_rerun_jpeg(overlay_bgr, target_w=viz_width, quality=jpeg_quality)) rr.log("info", rr.TextDocument(info_text)) frame_idx += 1 def _solve_one_arm(bgr, corners_est, aruco_board, board_length, T_aruco_to_link, K, dist, solve_intrinsics, history: _PoseHistory, top_k: int, pose_locked: bool, locked_T_aruco_in_cam: Optional[np.ndarray], accumulating: bool = True, ) -> Tuple[Optional[np.ndarray], Optional[np.ndarray], float, int]: """Per-arm pose solve. Returns: - ``T_cam_in_arm_base`` (4x4) or ``None`` if no detection - ``T_link_in_cam`` (4x4) or ``None`` (same condition; provided for the render path so we don't re-invert) - ``multi_resid`` (mean pooled reprojection residual, px; nan if single-frame fallback was used) - ``n_used`` (number of pooled frames in the multi-frame solve) Mirrors the single-arm capture loop's per-frame detection+pool+solve block, factored out so we can call it once per arm without copy-paste. Comments inside are deliberately heavy because the corner-vs-center-shift gymnastics here are subtle (the single-arm version's comment block goes into the same detail and you should read both side-by-side when modifying either). """ from exo_utils import do_est_aruco_pose, ARUCO_DICT if corners_est is None: return None, None, float("nan"), 0 try: result = do_est_aruco_pose( bgr, ARUCO_DICT, aruco_board, board_length, cameraMatrix=(None if solve_intrinsics else K), distCoeffs=dist, corners_est=corners_est, ) except Exception: return None, None, float("nan"), 0 if result == -1: return None, None, float("nan"), 0 K_this = np.asarray(result["cameraMatrix"], dtype=np.float64) dist_this = (np.asarray(result["distCoeffs"], dtype=np.float64).reshape(-1) if result.get("distCoeffs") is not None else dist) T_aruco_in_cam_single = result["est_aruco_pose"] # Undo do_est_aruco_pose's center-shift on the tvec so we can re-feed # corner-origin (obj, img) correspondences to the multi-frame solver. # See the long comment in exo_calibrate._capture_loop for the full # rationale; this is the SAME logic, just localised. rvec, tvec_shifted = result["rtvec"] R_mat = cv2.Rodrigues(rvec)[0] center_offset_board = np.array( [board_length / 2, board_length / 2, 0], dtype=np.float64) tvec_corner = tvec_shifted - R_mat.dot(center_offset_board) obj_cam, img_pts = result["obj_img_pts"] obj_board = (R_mat.T @ (obj_cam.T - tvec_corner.reshape(3, 1))).T proj_chk, _ = cv2.projectPoints(obj_board.astype(np.float32), rvec, tvec_corner, K_this, dist_this) residual_px = float(np.linalg.norm( proj_chk.reshape(-1, 2) - img_pts.reshape(-1, 2), axis=1).mean()) # Gate on BOTH the run-time start-recording toggle and the pose lock. # ``accumulating`` is what the ``c`` key in the main input loop flips # — we don't pool frames into the history until the user explicitly # opts in (otherwise the gripper-occluded boot frames poison the # multi-frame solve). ``pose_locked`` is the existing yellow-button # toggle and still freezes new additions while the user teleops to # visually verify the calibration without the pose drifting under them. if accumulating and not pose_locked: history.add(obj_board, img_pts, residual_px, single_frame_t=T_aruco_in_cam_single[:3, 3]) # Multi-frame pooled solve (or locked-pose fallback). obj_pool, img_pool, n_used, _ = history.top_k_concatenated(k=top_k) multi_resid = float("nan") if pose_locked and locked_T_aruco_in_cam is not None: T_aruco_in_cam = locked_T_aruco_in_cam else: T_aruco_in_cam = T_aruco_in_cam_single if obj_pool is not None and len(obj_pool) >= 4: mf, mf_resid = _solve_multiframe_pose( obj_pool, img_pool, K_this, dist_this, board_length) if mf is not None: T_aruco_in_cam = mf multi_resid = mf_resid T_link_in_cam = T_aruco_in_cam @ T_aruco_to_link T_cam_in_base = np.linalg.inv(T_link_in_cam) return T_cam_in_base, T_link_in_cam, multi_resid, n_used def _drive_arm_qpos(follower, data, arm_qpos_idx, grip_qpos_idx): """Pump the live follower proprio into the mujoco qpos slots for one arm. ``follower`` may be ``None`` (no init) — caller already gated on ``robot_controller is not None``, but the per-arm follower may still be ``None`` if ``use__follower=False`` (e.g. partial bimanual). """ if follower is None or not arm_qpos_idx: return try: q = follower.get_joint_pos() for idx, qv in zip(arm_qpos_idx, q[:len(arm_qpos_idx)]): data.qpos[idx] = float(qv) if grip_qpos_idx and len(q) >= 7: cmd = float(np.clip(q[6], 0.0, 1.0)) for qidx, sign, stroke in grip_qpos_idx: data.qpos[qidx] = sign * cmd * stroke except Exception: # Same swallow-and-continue as the single-arm script: a transient # CAN hiccup shouldn't kill the render thread. pass # --------------------------------------------------------------------------- # Joint qpos discovery # --------------------------------------------------------------------------- def _discover_arm_qpos(model, prefix: str = ""): """Locate (arm_qpos_idx, grip_qpos_idx) for one arm in the bimanual model. ``prefix`` is ``""`` for arm 1 (canonical YAM names) and ``"arm2_"`` for arm 2 (every body/joint/site name was prefixed by ``Demos.sim_yam_bimanual._prefix_names``). grip_qpos_idx is a list of (qpos_idx, sign, max_stroke_m), determined from the joint range (this handles BOTH the OPPOSITE-signed left_finger / right_finger XML and the SAME-signed joint7 / joint8 XML — see the single-arm version's comment block for the full story). """ arm_qpos_idx, grip_qpos_idx = [], [] for jn in ("joint1", "joint2", "joint3", "joint4", "joint5", "joint6"): jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, prefix + jn) if jid >= 0: arm_qpos_idx.append(int(model.jnt_qposadr[jid])) for jn in ("left_finger", "right_finger", "joint7", "joint8"): jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, prefix + jn) if jid >= 0: rng = model.jnt_range[jid] sign = +1.0 if abs(rng[1]) >= abs(rng[0]) else -1.0 max_stroke = float(max(abs(rng[0]), abs(rng[1]))) grip_qpos_idx.append((int(model.jnt_qposadr[jid]), sign, max_stroke)) return arm_qpos_idx, grip_qpos_idx # --------------------------------------------------------------------------- # Main entrypoint # --------------------------------------------------------------------------- def _capture_loop_bimanual_multicam( cam_handles, per_cam_states, exo_cfg_arm1, exo_cfg_arm2, model, data, rr, stop_event, margin: float, solve_intrinsics: bool = False, history_size: int = 30, top_k: int = 10, robot_controller=None, arm1_qpos_idx=None, arm1_grip_qpos_idx=None, arm2_qpos_idx=None, arm2_grip_qpos_idx=None, viz_width: int = 1280, viz_hz: float = 10.0, jpeg_quality: int = 75, ): """Single-thread bimanual capture across N scene cameras. Why single-thread? The previous design spawned ONE THREAD PER CAMERA, each with its own ``mujoco.MjModel`` clone and its own EGL renderer. MuJoCo's EGL context is process-global wrt ``eglMakeCurrent`` — two threads each calling ``GLContext.make_current()`` racing on the same EGLDisplay would segfault inside ``libEGL`` partway through a run. Serialising the renderer behind a lock fixed the crash but not the cost (each thread still cloned the model). This rewrite folds the cams into one daemon thread that: Pass 1 (per cam) — grab BGR, detect ArUco, per-arm pose solve. Pass 2 (once) — handle button-lock, drive followers, re-anchor arm 2 in mujoco from CAM 0's per-arm solves, ``mj_forward``, re-position arm 2 meshes. Pass 3 (per cam) — render overlay with THIS cam's intrinsics and extrinsic, compose, log under ``/...``. All cams share one ``mujoco.MjModel`` / ``MjData`` — the per-frame qpos / body_pos / body_quat writes happen ONCE per frame (Pass 2), and every per-cam render in Pass 3 reads from that single shared state. This is correct because (a) the followers' qpos is the same regardless of viewing cam, and (b) cam 0 is the documented anchor for arm-2 re-placement; the other cams just witness the rendered scene from a different intrinsic+extrinsic. """ from ExoConfigs.exoskeleton import link_to_aruco_transform from exo_utils import ( do_est_aruco_pose, ARUCO_DICT, position_exoskeleton_meshes, get_link_poses_from_robot, ) from scipy.spatial.transform import Rotation as _R # MuJoCo body id for arm-2's root; cached once. ``build_bimanual_xml`` # injects this body at a fixed initial pos (y=+margin/2). We OVERWRITE # that pos+quat every frame from CAM 0's per-arm detection delta. arm2_body_id = model.body("arm2_arm").id # Single EGL context for the WHOLE capture thread. Size it to the # largest cam so every per-cam render fits without re-creating. W_max = max(int(h["W"]) for h in cam_handles) H_max = max(int(h["H"]) for h in cam_handles) try: _gl_ctx = mujoco.GLContext(W_max, H_max) _gl_ctx.make_current() print(f"[exo_calibrate_bimanual] single-thread GL context " f"({W_max}x{H_max}) for {len(cam_handles)} cam(s)") except Exception as _exc: print(f"[exo_calibrate_bimanual] GL context failed: {_exc}") _gl_ctx = None # Resolve per-arm link configs + boards + transforms ONCE (shared). link_cfg_arm1 = exo_cfg_arm1.links["larger_coarse_board"] link_cfg_arm2 = exo_cfg_arm2.links["larger_coarse_board_arm2"] aruco_board_arm1 = exo_cfg_arm1.aruco_board_objects["larger_coarse_board"] aruco_board_arm2 = exo_cfg_arm2.aruco_board_objects["larger_coarse_board_arm2"] board_length_arm1 = link_cfg_arm1.board_length board_length_arm2 = link_cfg_arm2.board_length T_link_to_aruco_arm1 = link_to_aruco_transform(link_cfg_arm1) T_aruco_to_link_arm1 = np.linalg.inv(T_link_to_aruco_arm1) T_link_to_aruco_arm2 = link_to_aruco_transform(link_cfg_arm2) T_aruco_to_link_arm2 = np.linalg.inv(T_link_to_aruco_arm2) # World-frame transforms: arm 1 at y=-margin/2, arm 2 at y=+margin/2. T_arm1_in_world = _T_translate_y(-margin / 2.0) T_arm2_in_world = _T_translate_y(+margin / 2.0) # Per-cam sliding-window histories — each cam sees the boards from a # different angle so the histories MUST stay separate (a multi-frame # solve mixing corners from two viewpoints would be nonsense). per_cam_histories = [] for _idx, (_cam_name, _, _, _state) in enumerate(per_cam_states): h1 = _PoseHistory(max_size=history_size) h2 = _PoseHistory(max_size=history_size) _state.history_arm1 = h1 _state.history_arm2 = h2 per_cam_histories.append((h1, h2)) print(f"[exo_calibrate_bimanual] multi-frame robust pose: " f"history={history_size}, pool top-{top_k}; " f"solve_intrinsics={solve_intrinsics}; margin={margin:.3f} m; " f"cams={[h['name'] for h in cam_handles]}") if robot_controller is not None: print(f"[exo_calibrate_bimanual] teleop enabled (BOTH arms); " f"YELLOW leader button toggles the SHARED pose-lock " f"across ALL cams.") frame_idx = 0 last_log_t = 0.0 LOG_INTERVAL_S = 1.0 / max(viz_hz, 0.1) while not stop_event.is_set(): # ── PASS 1 ─ per-cam grab + detect + per-arm solve ────────────────── per_cam_results = [] any_bgr = False for cam_idx, h in enumerate(cam_handles): cam = h["cam"]; K = h["K"]; dist = h["dist"] bgr = _grab_bgr(cam) if bgr is None: per_cam_results.append(None) continue any_bgr = True try: gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY) corners_all, ids_all, rejected_all = cv2.aruco.detectMarkers( gray, ARUCO_DICT, parameters=cv2.aruco.DetectorParameters(), ) except Exception: corners_all, ids_all, rejected_all = ([], None, []) arm1_corners_est, arm2_corners_est = _partition_corners( corners_all, ids_all) cam_name, _, _, _state = per_cam_states[cam_idx] h1, h2 = per_cam_histories[cam_idx] T_cam_in_arm1_base, T_link_arm1_in_cam, multi_resid_arm1, n_used_arm1 = ( _solve_one_arm( bgr, arm1_corners_est, aruco_board_arm1, board_length_arm1, T_aruco_to_link_arm1, K, dist, solve_intrinsics, h1, top_k, pose_locked=_state.pose_locked, locked_T_aruco_in_cam=_state.locked_T_aruco_in_cam_arm1, accumulating=_state.accumulating, )) T_cam_in_arm2_base, T_link_arm2_in_cam, multi_resid_arm2, n_used_arm2 = ( _solve_one_arm( bgr, arm2_corners_est, aruco_board_arm2, board_length_arm2, T_aruco_to_link_arm2, K, dist, solve_intrinsics, h2, top_k, pose_locked=_state.pose_locked, locked_T_aruco_in_cam=_state.locked_T_aruco_in_cam_arm2, accumulating=_state.accumulating, )) if T_cam_in_arm1_base is not None: _state.arm1.update(K, dist, T_cam_in_arm1_base) if T_cam_in_arm2_base is not None: _state.arm2.update(K, dist, T_cam_in_arm2_base) # ── annotated 2D image (per cam) ──────────────────────────────── aruco_bgr = bgr.copy() if ids_all is not None: cv2.aruco.drawDetectedMarkers(aruco_bgr, corners_all, ids_all) if T_link_arm1_in_cam is not None: T_aruco_in_cam_arm1 = T_link_arm1_in_cam @ T_link_to_aruco_arm1 _draw_aruco_plane(aruco_bgr, T_aruco_in_cam_arm1, board_length_arm1, K, color=(0, 255, 255)) if T_link_arm2_in_cam is not None: T_aruco_in_cam_arm2 = T_link_arm2_in_cam @ T_link_to_aruco_arm2 _draw_aruco_plane(aruco_bgr, T_aruco_in_cam_arm2, board_length_arm2, K, color=(255, 0, 255)) per_cam_results.append(dict( cam_name=cam_name, K=K, dist=dist, W=int(h["W"]), H=int(h["H"]), bgr=bgr, aruco_bgr=aruco_bgr, corners_all=corners_all, ids_all=ids_all, T_cam_in_arm1_base=T_cam_in_arm1_base, T_cam_in_arm2_base=T_cam_in_arm2_base, T_link_arm1_in_cam=T_link_arm1_in_cam, T_link_arm2_in_cam=T_link_arm2_in_cam, multi_resid_arm1=multi_resid_arm1, multi_resid_arm2=multi_resid_arm2, n_used_arm1=n_used_arm1, n_used_arm2=n_used_arm2, history_arm1=h1, history_arm2=h2, state=_state, )) if not any_bgr: time.sleep(0.005) continue # ── PASS 2 ─ ONCE per frame, using CAM 0's solves ──────────────────── cam0 = per_cam_results[0] if per_cam_results and per_cam_results[0] else None T_cam0_in_arm1 = cam0["T_cam_in_arm1_base"] if cam0 is not None else None T_cam0_in_arm2 = cam0["T_cam_in_arm2_base"] if cam0 is not None else None T_link_arm1_in_cam0 = cam0["T_link_arm1_in_cam"] if cam0 is not None else None T_link_arm2_in_cam0 = cam0["T_link_arm2_in_cam"] if cam0 is not None else None if robot_controller is not None: if robot_controller.check_button_press() is not None: # Toggle pose-lock on EVERY state (shared lock policy). any_locked_after = None for (_n, _K, _d, _st) in per_cam_states: _st.pose_locked = not _st.pose_locked any_locked_after = _st.pose_locked if any_locked_after: # Each cam locks its OWN current best pose (its history # is what its multi-frame solve uses). cam 0's lock is # the one quoted in the print. for cam_idx, (_n, _K, _d, _st) in enumerate(per_cam_states): cr = per_cam_results[cam_idx] if cr is None: _st.locked_T_aruco_in_cam_arm1 = None _st.locked_T_aruco_in_cam_arm2 = None continue _st.locked_T_aruco_in_cam_arm1 = ( (cr["T_link_arm1_in_cam"] @ np.linalg.inv(T_aruco_to_link_arm1)).copy() if cr["T_link_arm1_in_cam"] is not None else None) _st.locked_T_aruco_in_cam_arm2 = ( (cr["T_link_arm2_in_cam"] @ np.linalg.inv(T_aruco_to_link_arm2)).copy() if cr["T_link_arm2_in_cam"] is not None else None) print(f"\n[exo_calibrate_bimanual] POSE LOCKED — both " f"arms frozen on ALL {len(per_cam_states)} cams.") else: for (_n, _K, _d, _st) in per_cam_states: _st.locked_T_aruco_in_cam_arm1 = None _st.locked_T_aruco_in_cam_arm2 = None print(f"\n[exo_calibrate_bimanual] pose UNLOCKED on " f"ALL cams.") _drive_arm_qpos(robot_controller.follower_r, data, arm1_qpos_idx, arm1_grip_qpos_idx) _drive_arm_qpos(robot_controller.follower_l, data, arm2_qpos_idx, arm2_grip_qpos_idx) # Re-anchor arm 2 from CAM 0's live delta (shared mujoco scene). if T_cam0_in_arm1 is not None and T_cam0_in_arm2 is not None: T_arm2_in_arm1 = T_cam0_in_arm1 @ np.linalg.inv(T_cam0_in_arm2) T_arm2_in_world_now = T_arm1_in_world @ T_arm2_in_arm1 model.body_pos[arm2_body_id] = T_arm2_in_world_now[:3, 3] q_xyzw = _R.from_matrix(T_arm2_in_world_now[:3, :3]).as_quat() model.body_quat[arm2_body_id] = q_xyzw[[3, 0, 1, 2]] mujoco.mj_forward(model, data) if T_cam0_in_arm1 is not None and T_cam0_in_arm2 is not None: try: position_exoskeleton_meshes( exo_cfg_arm2, model, data, get_link_poses_from_robot(exo_cfg_arm2, model, data)) except Exception: pass # ── PASS 3 ─ per-cam mujoco render + overlay + log ─────────────────── now = time.monotonic() do_image_log = (now - last_log_t) >= LOG_INTERVAL_S if do_image_log: last_log_t = now rr.set_time("step", sequence=frame_idx) info_sections = [] for cam_idx, cr in enumerate(per_cam_results): if cr is None: continue cam_name = cr["cam_name"] K = cr["K"]; W = cr["W"]; H = cr["H"] bgr = cr["bgr"]; aruco_bgr = cr["aruco_bgr"] T_cam_in_arm1_base = cr["T_cam_in_arm1_base"] T_cam_in_arm2_base = cr["T_cam_in_arm2_base"] T_link_arm1_in_cam = cr["T_link_arm1_in_cam"] T_link_arm2_in_cam = cr["T_link_arm2_in_cam"] _state = cr["state"] overlay_bgr = aruco_bgr T_cam_in_world = None if T_cam_in_arm1_base is not None: T_cam_in_world = T_arm1_in_world @ T_cam_in_arm1_base elif T_cam_in_arm2_base is not None: T_cam_in_world = T_arm2_in_world @ T_cam_in_arm2_base if T_cam_in_world is not None: try: T_world_in_cam = np.linalg.inv(T_cam_in_world) rendered_rgb, seg_mask = _render_with_intrinsics( model, data, T_world_in_cam, K, W, H) overlay_rgb = _compose_overlay( bgr, rendered_rgb, seg_mask=seg_mask) overlay_bgr = cv2.cvtColor(overlay_rgb, cv2.COLOR_RGB2BGR) if T_link_arm1_in_cam is not None: _draw_aruco_plane( overlay_bgr, T_link_arm1_in_cam @ T_link_to_aruco_arm1, board_length_arm1, K, color=(0, 255, 0)) if T_link_arm2_in_cam is not None: _draw_aruco_plane( overlay_bgr, T_link_arm2_in_cam @ T_link_to_aruco_arm2, board_length_arm2, K, color=(0, 255, 0)) fg_mask = (seg_mask[:, :, 0] != -1).astype(np.uint8) * 255 contours, _ = cv2.findContours( fg_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cv2.drawContours(overlay_bgr, contours, -1, (0, 0, 255), 2, cv2.LINE_AA) except Exception: if not getattr(_capture_loop_bimanual_multicam, "_render_err_logged", False): import traceback print(f"\n[exo_calibrate_bimanual] mujoco render " f"failed (cam {cam_name!r}):") traceback.print_exc() print(f"[exo_calibrate_bimanual] falling back to " f"ArUco-only overlay\n") _capture_loop_bimanual_multicam._render_err_logged = True # ── scalars + status ──────────────────────────────────────────── lock_str = "[LOCKED]" if _state.pose_locked else "[tracking]" t1 = (T_cam_in_arm1_base[:3, 3] if T_cam_in_arm1_base is not None else np.full(3, np.nan)) t2 = (T_cam_in_arm2_base[:3, 3] if T_cam_in_arm2_base is not None else np.full(3, np.nan)) consistency_mm = float("nan") if T_cam_in_arm1_base is not None and T_cam_in_arm2_base is not None: p1 = (T_arm1_in_world @ T_cam_in_arm1_base)[:3, 3] p2 = (T_arm2_in_world @ T_cam_in_arm2_base)[:3, 3] consistency_mm = float(np.linalg.norm(p1 - p2) * 1000) if T_cam_in_arm1_base is not None: rr.log(f"{cam_name}/arm1_cam_in_base_x", rr.Scalars(float(t1[0]))) rr.log(f"{cam_name}/arm1_cam_in_base_y", rr.Scalars(float(t1[1]))) rr.log(f"{cam_name}/arm1_cam_in_base_z", rr.Scalars(float(t1[2]))) if T_cam_in_arm2_base is not None: rr.log(f"{cam_name}/arm2_cam_in_base_x", rr.Scalars(float(t2[0]))) rr.log(f"{cam_name}/arm2_cam_in_base_y", rr.Scalars(float(t2[1]))) rr.log(f"{cam_name}/arm2_cam_in_base_z", rr.Scalars(float(t2[2]))) if not np.isnan(consistency_mm): rr.log(f"{cam_name}/cross_arm_consistency_mm", rr.Scalars(float(consistency_mm))) if do_image_log: rr.log(f"{cam_name}/rgb_aruco", _to_rerun_jpeg(aruco_bgr, target_w=viz_width, quality=jpeg_quality)) rr.log(f"{cam_name}/rgb_overlay", _to_rerun_jpeg(overlay_bgr, target_w=viz_width, quality=jpeg_quality)) info_sections.append( f"[{cam_name}] {lock_str} margin={margin:.3f}m\n" f" arm1: cam_in_base=({t1[0]:+.3f},{t1[1]:+.3f},{t1[2]:+.3f}) " f"{_state.arm1.stability_str()} n_used={cr['n_used_arm1']} " f"multi_resid={cr['multi_resid_arm1']:.2f}px " f"{cr['history_arm1'].stats()}\n" f" arm2: cam_in_base=({t2[0]:+.3f},{t2[1]:+.3f},{t2[2]:+.3f}) " f"{_state.arm2.stability_str()} n_used={cr['n_used_arm2']} " f"multi_resid={cr['multi_resid_arm2']:.2f}px " f"{cr['history_arm2'].stats()}\n" f" cross-arm consistency (mm): {consistency_mm:.1f}" ) if do_image_log and info_sections: rr.log("info", rr.TextDocument("\n\n".join(info_sections))) frame_idx += 1 def _discover_role_scene_cams(camera_config_file: str): """Return every camera key in ``camera.json`` whose ``role == "scene"``. russet's bimanual rig has BOTH a fixed opposite-facing scene camera AND an egocentric scene camera mounted between the arms. Both carry ``role: "scene"`` in the config; calibrating both at once is cheaper than running the script twice (one mujoco model load, one input loop) and lets the operator verify cross-camera consistency by eye in the same session. Falls back to a single-element ``["scene_camera"]`` list if the JSON can't be parsed (bad path / missing roles / etc.), matching the pre-patch behaviour. """ import json as _json from pathlib import Path as _Path try: with open(camera_config_file) as _f: cfg = _json.load(_f) except Exception as _exc: print(f" ⚠ could not read camera config ({_exc}); " f"defaulting to ['scene_camera']") return ["scene_camera"] found = [name for name, entry in cfg.items() if isinstance(entry, dict) and entry.get("role") == "scene"] if not found: print(f" ⚠ no role=scene cameras in {camera_config_file}; " f"defaulting to ['scene_camera']") return ["scene_camera"] return found def run_exo_calibrate_bimanual( scene_cam_name: str = "scene_camera", scene_cam_names: "Optional[List[str]]" = None, auto_discover_scene_cams: bool = False, camera_config_file: Optional[str] = None, calibration_out: Optional[str] = None, vis_port: int = 9092, resolution: str = "HD1080", solve_intrinsics: bool = False, history_size: int = 30, top_k: int = 10, teleop: bool = True, margin: float = 0.7, viz_width: int = 1280, viz_hz: float = 10.0, jpeg_quality: int = 75, ) -> None: """End-to-end bimanual scene-camera calibration. Heavy raiden imports are deferred until inside this function, matching the single-arm version's lazy-import policy — keeps module import cheap for tools that just want the helpers. """ from raiden._config import CAMERA_CONFIG, CALIBRATION_FILE from ExoConfigs.yam_exo import ( YAM_BASE_ONLY_CONFIG, YAM_BASE_ONLY_CONFIG_ARM2) # v2-reverse arm-2 board: physical mount mirrors the board across the # x-axis (the new yam_base_board_v2_reverse_clearance_fixed.stl). Swap # the exo mesh and flip the sign of the aruco offset x so the in-cam # board pose computed from the printed marker IDs lines up with the # actual mounted board on the rig. _ARM2_V2_STL = str(Path(_EXO_DIR) / "so100_blender_testings" / "yam_base_board_v2_reverse_clearance_fixed.stl") for _link in YAM_BASE_ONLY_CONFIG_ARM2.links.values(): _link.exo_mesh_path = _ARM2_V2_STL _old = np.asarray(_link.aruco_offset_pos, dtype=np.float64).copy() _link.aruco_offset_pos = np.array([-_old[0], _old[1], _old[2]]) print(f" ✓ arm2 v2-reverse patch: STL=" f"{Path(_ARM2_V2_STL).name}, " f"aruco_offset_pos x sign flipped " f"({_old[0]:+.2f}->{-_old[0]:+.2f})") from exo_utils import get_link_poses_from_robot, position_exoskeleton_meshes # Build wrapper that emits the bimanual mujoco XML in-process. from Demos.sim_yam_bimanual import build_bimanual_xml camera_config_file = camera_config_file or CAMERA_CONFIG calibration_out = Path(calibration_out or CALIBRATION_FILE) # cd into exo_redo so the XML's relative texture/mesh paths resolve. # Same trick the single-arm script uses; the bimanual XML emits # ABSOLUTE meshdir/texturedir but several plane meshes are # blender-relative, so the cwd switch is required. os.chdir(str(_EXO_DIR)) # Build the bimanual XML — NO background include (we composite the # render over a real camera frame; the floor/skybox would muddy the # alpha mask exactly as in the single-arm path). xml_str = build_bimanual_xml(margin=margin, include_background=False) model = mujoco.MjModel.from_xml_string(xml_str) data = mujoco.MjData(model) mujoco.mj_forward(model, data) # Mount BOTH arms' exo + ArUco mocap meshes. For arm 2 we deepcopy # the module-level config so we can rebind ``pybullet_name`` to the # prefixed body name ``arm2_arm`` without mutating shared state (the # canonical bimanual-sim pattern; see sim_yam_bimanual.main). exo_cfg_arm1 = YAM_BASE_ONLY_CONFIG exo_cfg_arm2 = copy.deepcopy(YAM_BASE_ONLY_CONFIG_ARM2) for link_cfg in exo_cfg_arm2.links.values(): link_cfg.pybullet_name = "arm2_arm" position_exoskeleton_meshes( exo_cfg_arm1, model, data, get_link_poses_from_robot(exo_cfg_arm1, model, data)) position_exoskeleton_meshes( exo_cfg_arm2, model, data, get_link_poses_from_robot(exo_cfg_arm2, model, data)) print(f" ✓ Built bimanual mujoco model ({model.nbody} bodies, " f"margin={margin:.3f} m)") # Resolve which scene cameras to open. Auto-discover wins; else use # the explicit list; else the legacy single name. This is the only # spot where the single-cam → multi-cam fan-out happens; everything # downstream operates on the resulting list. if auto_discover_scene_cams: cam_names = _discover_role_scene_cams(camera_config_file) elif scene_cam_names: cam_names = list(scene_cam_names) else: cam_names = [scene_cam_name] print(f" ✓ calibrating {len(cam_names)} scene camera(s): {cam_names}") # Open the ZED for each scene camera at the requested resolution. # We keep all opened cameras in a parallel list (`cam_handles`) so # the per-thread spawn loop below can iterate uniformly. K/dist are # PER camera (different intrinsics for the egocentric vs opposite # cams), so they live alongside each cam handle. cam_handles = [] for _name in cam_names: _serial = _scene_serial_from_config(camera_config_file, _name) _cam, _K, _dist, _W, _H = _open_zed_native(_serial, resolution) cam_handles.append({ "name": _name, "cam": _cam, "K": _K, "dist": _dist, "W": _W, "H": _H, }) # IMPORTANT: don't pre-create the mujoco renderer in main thread — # mujoco's EGL context is thread-bound; the daemon thread creates # its own. (Same caveat as the single-arm script.) # Discover per-arm qpos indices (arm 1 = unprefixed, arm 2 = "arm2_"). arm1_qpos_idx, arm1_grip_qpos_idx = _discover_arm_qpos(model, prefix="") arm2_qpos_idx, arm2_grip_qpos_idx = _discover_arm_qpos(model, prefix="arm2_") print(f" ✓ arm1 qpos: arm={arm1_qpos_idx}, grip={arm1_grip_qpos_idx}") print(f" ✓ arm2 qpos: arm={arm2_qpos_idx}, grip={arm2_grip_qpos_idx}") # Optional teleop on BOTH arms. The single-arm script gates on # arm==right/left for each flag; here we want all four flags True. robot_controller = None if teleop: try: from raiden.robot.controller import RobotController print("\nInitialising bimanual teleop " "(left + right leaders + followers)...") robot_controller = RobotController( use_right_leader=True, use_left_leader=True, use_right_follower=True, use_left_follower=True, ) # setup_for_teleop_recording() internally runs the can-check # AND robot init — same single-call rule as the single-arm path. robot_controller.setup_for_teleop_recording() robot_controller.enable_estop() robot_controller.start_teleoperation() print(" ✓ Teleop active on BOTH arms — move either leader, " "both rendered arms track their respective followers.") except Exception as e: print(f" ⚠ Teleop init failed ({e}); continuing without it. " "Use --no-teleop to suppress.") robot_controller = None rr = _init_rerun(vis_port) # One state + thread + model/data clone per camera. The capture loop # mutates ``model.body_pos[arm2_body_id]`` in place every frame to # re-anchor arm 2 from the live detection, so two cams sharing one # model would race; cloning is cheap (~50 MB and a single XML re-parse). # Only the FIRST thread is given the robot_controller — that thread # owns the yellow-button pose-lock toggle. Other threads track live. # One daemon thread owns ALL cams + the shared mujoco scene + EGL ctx. # This eliminates the eglMakeCurrent race that crashed the previous # per-cam-thread design: only this thread ever touches the GL context. per_cam_states = [] for _h in cam_handles: _state = _BimanualState() per_cam_states.append((_h["name"], _h["K"], _h["dist"], _state)) _shared_stop = per_cam_states[0][3].stop_event # Mirror the shared stop_event onto every state so anyone watching # state.stop_event sees the same flag. for (_n, _K, _d, _st) in per_cam_states[1:]: _st.stop_event = _shared_stop _st.arm1.stop_event = _shared_stop _st.arm2.stop_event = _shared_stop _thread = threading.Thread( target=_capture_loop_bimanual_multicam, args=(cam_handles, per_cam_states, exo_cfg_arm1, exo_cfg_arm2, model, data, rr, _shared_stop, margin), kwargs=dict( solve_intrinsics=solve_intrinsics, history_size=history_size, top_k=top_k, robot_controller=robot_controller, arm1_qpos_idx=arm1_qpos_idx, arm1_grip_qpos_idx=arm1_grip_qpos_idx, arm2_qpos_idx=arm2_qpos_idx, arm2_grip_qpos_idx=arm2_grip_qpos_idx, viz_width=viz_width, viz_hz=viz_hz, jpeg_quality=jpeg_quality, ), daemon=True, name="exo-capture-bimanual-multicam", ) threads = [_thread] _thread.start() print(f" ✓ spawned SINGLE capture thread for " f"{[h['name'] for h in cam_handles]} (single EGL ctx)") print("\nWatch the Rerun viewer in your browser, then return here.") print(" c → clear both arm histories AND start (or pause) recording.") print(" Use this AFTER moving the arms' grippers out of the way") print(" of their base boards — the boot frames are usually") print(" occluded and would poison the multi-frame solve.") print(" y → save the latest detection (BOTH arms)") print(" q → quit without saving\n") try: while True: try: ans = input("[c]=start/clear [y]=save [q]=quit > " ).strip().lower() except (EOFError, KeyboardInterrupt): ans = "q" if ans == "q": print("Quitting without saving.") break if ans == "c": # Toggle recording on EVERY camera's state at once. On ON-edge # we wipe each cam's per-arm pose histories so accumulation # restarts from a clean slate; on OFF-edge we PRESERVE # whatever's already pooled. _new = not per_cam_states[0][3].accumulating for _, _, _, _st in per_cam_states: _st.accumulating = _new if _new: if _st.history_arm1 is not None: with _st.history_arm1._lock: _st.history_arm1.entries = [] if _st.history_arm2 is not None: with _st.history_arm2._lock: _st.history_arm2.entries = [] if _new: print(f" ▶ recording ON across {len(per_cam_states)} " f"cam(s) — cleared histories, pooling fresh detections.") else: print(f" ⏸ recording PAUSED across " f"{len(per_cam_states)} cam(s) (histories preserved).") continue if ans == "y": # Save per-camera calibration records. Each camera writes # two records under the keys "__arm1" / "__arm2" # (the existing convention from the single-cam path); the # cross-camera collisions don't happen because the names # are different per camera (scene_camera vs ego_camera). _saved_any = False for _name, _K_c, _dist_c, _st in per_cam_states: _snap1 = _st.arm1.snapshot() _snap2 = _st.arm2.snapshot() if _snap1 is None and _snap2 is None: print(f" ⚠ [{_name}] no detections — skipping") continue _T1 = _snap1[2] if _snap1 is not None else None _T2 = _snap2[2] if _snap2 is not None else None _save_bimanual_calibration( scene_cam_name=_name, K=_K_c, dist=_dist_c, T_cam_in_arm1_base=_T1, T_cam_in_arm2_base=_T2, calibration_path=calibration_out, resolution=resolution, ) _saved_any = True if not _saved_any: print(" no valid detections on ANY camera — " "point cams at boards and retry") continue break print(f" unknown input {ans!r} — use 'c', 'y', or 'q'") finally: for _, _, _, _st in per_cam_states: _st.stop_event.set() for _thread in threads: _thread.join(timeout=2.0) if robot_controller is not None: for fn in ("stop_teleoperation", "return_to_home", "shutdown"): try: getattr(robot_controller, fn)() except Exception: pass for _h in cam_handles: try: _h["cam"].close() except Exception: pass def main(): import argparse p = argparse.ArgumentParser( description="Bimanual scene-camera calibration via dual exoskeleton " "ArUco boards (arm 1 IDs 217–225, arm 2 IDs 226–234).") p.add_argument("--scene_cam_name", default="scene_camera", help="Single scene-camera key in raiden's camera.json. " "Ignored when --auto_discover_scene_cams or --scene_cam_names is set.") p.add_argument("--scene_cam_names", default="", help="Comma-separated list of scene-camera keys (overrides --scene_cam_name).") p.add_argument("--auto_discover_scene_cams", action="store_true", help="Auto-pick every camera whose role==\"scene\" in camera.json. " "Overrides both --scene_cam_name and --scene_cam_names.") p.add_argument("--camera_config", default=None) p.add_argument("--out", default=None, help="Output calibration JSON path (defaults to raiden's " "CALIBRATION_FILE).") p.add_argument("--vis_port", type=int, default=9092) p.add_argument("--resolution", default="HD1080", choices=["HD720", "HD1080", "HD2K"], help="ZED resolution (default HD1080 — needed for the 3x3 board)") p.add_argument("--solve_intrinsics", action="store_true", help="Per-frame jointly solve focal length with pose " "(cameraMatrix=None). Default off — use ZED factory K.") p.add_argument("--history_size", type=int, default=30, help="Sliding-window size for the per-arm multi-frame pose pool.") p.add_argument("--top_k", type=int, default=10, help="Per-arm pool size for multi-frame solvePnP.") p.add_argument("--teleop", action=argparse.BooleanOptionalAction, default=True, help="Enable leader-follower teleop on BOTH arms. Use " "--no-teleop to disable (e.g. for offline visual-only " "calibration without the followers actually moving).") p.add_argument("--margin", type=float, default=0.7, help="Lateral distance between the two YAM arm bases (m). " "MUST match the physical rig — used to lift the per-arm " "camera pose into the bimanual mujoco render frame.") p.add_argument("--viz_width", type=int, default=1280, help="Rerun-side resize width before JPEG-encoding.") p.add_argument("--viz_hz", type=float, default=10.0, help="Rerun image-log rate cap (Hz). Per-frame scalar logs " "(cam_in_base_{x,y,z}) are NOT throttled.") p.add_argument("--jpeg_quality", type=int, default=75, help="JPEG quality used when wrapping image logs for Rerun.") args = p.parse_args() run_exo_calibrate_bimanual( scene_cam_name=args.scene_cam_name, scene_cam_names=[n.strip() for n in args.scene_cam_names.split(",") if n.strip()] or None, auto_discover_scene_cams=args.auto_discover_scene_cams, camera_config_file=args.camera_config, calibration_out=args.out, vis_port=args.vis_port, resolution=args.resolution, solve_intrinsics=args.solve_intrinsics, history_size=args.history_size, top_k=args.top_k, teleop=args.teleop, margin=args.margin, viz_width=args.viz_width, viz_hz=args.viz_hz, jpeg_quality=args.jpeg_quality, ) if __name__ == "__main__": main()