"""Live BIMANUAL wrist + scene extrinsics verification in Rerun. Sibling of :file:`wrist_verify_rerun.py` -- that script verifies ONE arm's wrist + scene extrinsics. This one does the bimanual version: ONE shared scene camera + TWO wrist cameras + bimanual mujoco model + teleop on BOTH arms. We DO NOT modify the single-arm script; everything bimanual lives in this file. Streamed Rerun panels --------------------- scene/rgb_aruco scene cam + aruco markers + BOTH per-arm board outlines (arm 1 = yellow, arm 2 = magenta) + per-arm aruco axis frames. scene/rgb_overlay scene cam + bimanual mujoco mesh overlay (BOTH arms + BOTH base boards rendered) + red silhouette. Cross-checks T_base_scene_cam for BOTH arms and that the base+exo meshes line up. wrist_arm1/rgb_overlay right wrist cam + arm 1's mesh overlay (uses T_ee_cam_arm1 from the saved calibration to pose the rendering camera). wrist_arm2/rgb_overlay left wrist cam + arm 2's mesh overlay (uses T_ee_cam_arm2). wrist_arm1/rgb_dots right wrist cam + uniform pixel grid drawn as FILLED RED dots. wrist_arm2/rgb_dots left wrist cam + uniform pixel grid drawn as FILLED BLUE dots. scene/rgb_dots scene cam + OPEN circles at the same physical floor points back-projected from each wrist's pixel grid (RED open circles ↔ arm 1 wrist; BLUE open circles ↔ arm 2 wrist). Colour convention ----------------- Arm 1 (right) is RED. Arm 2 (left) is BLUE. Every per-arm marker in this script (dots, log lines, scalar paths) consistently uses that mapping. The two-colour scheme is the user's explicit ask -- one-glance arm identification on the scene-cam panel where both grids share the image. Math reminder ------------- The single-arm script computes, per frame, for one arm: T_base_ee = FK(live_q) # arm's own base T_base_wrist = T_base_ee @ T_ee_wrist # wrist cam in arm base T_link_in_wristcam = inv(T_base_wrist) # render target T_link_in_scenecam = inv(T_base_scene) # render target For the bimanual case we run that math TWICE (independently per arm) using each arm's own: - `T_ee_cam_arm{i}` (from the bimanual wrist calibration JSON) - `T_cam_in_arm{i}_base` (live-resolved from the scene cam each startup; *not* read from the saved calibration -- the scene cam gets bumped between sessions, so we re-solve at boot just like the single-arm script does). For the SCENE overlay we render the bimanual mujoco model from the shared scene camera. Following the convention of exo_calibrate_bimanual, the render is anchored to arm 1's calibration (canonical choice) and we overlay BOTH arms in one mujoco scene. Arm 2's body_pos / body_quat is re-anchored every frame from the live per-arm scene-cam detection delta (exactly the same trick exo_calibrate_bimanual uses around its ``T_arm2_in_arm1 = T_cam_in_arm1 @ inv(T_cam_in_arm2)`` block). For the WRIST overlays we render the SAME bimanual mujoco model from the per-arm wrist cam pose. Per-arm wrist FK gives us ``T_base_wrist_arm{i}``; we lift it into the bimanual-world frame by left-multiplying with ``T_arm{i}_in_world = translate_y(∓margin/2)``, then invert to get ``T_world_in_wristcam``. For the DOT GRID on the scene cam, we run the single-arm script's floor-back-projection math TWICE: once per arm. Each wrist's pixel grid is back-projected to z=0 in THAT ARM'S BASE frame, then we transform those base-frame floor points into the scene-cam frame using THAT ARM'S ``T_cam_in_arm{i}_base``. Crucially we DON'T re-anchor through the bimanual world frame for the dots -- the dots are an arm-by-arm consistency check, not a render, so each arm's three extrinsics (wrist→ee, scene→that-arm's-base, FK) close their own loop independently. Failure modes that the single-arm script handles, all preserved here: - z=0 back-projection with negative depth or wildly off-screen scene pixels: drop those dots gracefully. Each arm does this check on its own. - One wrist's grid being entirely off-screen (e.g. wrist not pointing at the floor): show grey dots on the wrist panel and skip the scene-side open circles. Same per-arm. - Scene-cam recal at startup needs BOTH boards in view (we recal per-arm independently). If a board is missing we fail loudly pointing at the calibration script that produces it. CLI / config ------------ ``--scene_cam`` / ``--right_wrist_cam`` / ``--left_wrist_cam`` name the camera keys in raiden's camera.json. The wrist-calibration JSON (default ``~/.config/raiden/wrist_calibration_result_bimanual.json``) is produced by ``wrist_calibrate_bimanual.py``; the scene-calibration JSON (default raiden ``CALIBRATION_FILE``) is produced by ``exo_calibrate_bimanual.py``. Both are read at startup; the scene extrinsic is then *replaced* by a live aruco recal because the scene cam gets nudged between sessions -- the saved scene extrinsic is only inspected to confirm both ``__arm1`` / ``__arm2`` records exist (a load-time sanity check). Press the YELLOW leader button on EITHER leader to end + route BOTH arms back HOME via the shared INTERMEDIATE waypoint (sequenced, arm 1 then arm 2 -- same readability+safety argument the bimanual wrist calibration uses). """ import os os.environ.setdefault("MUJOCO_GL", "egl") os.environ.setdefault("PYOPENGL_PLATFORM", "egl") import argparse import copy import json import re import sys import threading import time from dataclasses import dataclass from pathlib import Path from typing import Optional, Tuple import cv2 import mujoco import numpy as np # Self-locate exo_redo the same way the single-arm verify does so this # script works from any sshfs mount. Lives at # raiden_fork/raiden/calibration/wrist_verify_rerun_bimanual.py. _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 helpers from the single-arm scene-cam module -- DO NOT duplicate. from raiden.calibration.exo_calibrate_fidex import ( _scene_serial_from_config, _open_zed_native, _grab_bgr, _init_rerun, _draw_aruco_plane, _to_rerun_jpeg, _render_with_intrinsics, _compose_overlay, _PoseHistory, _solve_multiframe_pose, ) # And the bimanual scene-cam partition helper (no need to redefine). from raiden.calibration.exo_calibrate_bimanual_fidex import ( _partition_corners, _T_translate_y, ) # Default file locations (overridable via CLI). _DEFAULT_CAM_CONFIG = Path.home() / ".config" / "raiden" / "camera.json" _DEFAULT_SCENE_CAL = Path.home() / ".config" / "raiden" / "calibration_results.json" _DEFAULT_WRIST_CAL_BIMANUAL = ( Path.home() / ".config" / "raiden" / "wrist_calibration_result_bimanual.json" ) # Per-arm colours. BGR triples (OpenCV native). # arm 1 (right) -> RED (0, 0, 255) # arm 2 (left) -> BLUE (255, 0, 0) # Greyed-out (off-screen / no-depth) dots are GREY -- matches the # single-arm script's convention so the operator's eye picks them out # the same way. _ARM1_COLOR = (0, 0, 255) # red _ARM2_COLOR = (255, 0, 0) # blue _GRAY = (140, 140, 140) # Symmetric "fold inward" waypoint for both arms (lifted from the # bimanual wrist calibration, which lifted it from the single-arm # wrist_calibrate / wrist_verify). Keep these in sync if those constants # ever change. INTERMEDIATE_POSE_RIGHT = np.array( [0.0097, 1.9213, 1.4582, -0.4324, 0.5880, -0.1459, 0.9923], dtype=np.float64, ) INTERMEDIATE_POSE_LEFT = INTERMEDIATE_POSE_RIGHT.copy() HOME_POSE = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64) @dataclass class _State: """Daemon-side flags. ``stop`` is the only mutable knob the main thread flips -- everything else is read-only after thread start. """ stop: bool = False # --------------------------------------------------------------------------- # JSON loaders (bimanual schema) # --------------------------------------------------------------------------- def _load_wrist_extrinsic(path: Path, cam_name: str) -> np.ndarray: """Pull a 4x4 ``T_ee_cam`` from the bimanual wrist calibration JSON. Schema (set by wrist_calibrate_bimanual._save_bimanual_calibration_result): cal["cameras"][]["extrinsics"] = { "rotation_matrix": [[..3x3..]], "translation_vector": [.., .., ..], "reference_frame": "_grasp_site", ... } The per-camera record's ``arm`` field is the canonical "right"/"left" label; we don't enforce it here because callers pass the camera name explicitly. Raises FileNotFoundError / KeyError with a directive error message pointing at the calibration script the user is missing. """ if not path.exists(): raise FileNotFoundError( f"Bimanual wrist calibration JSON not found at {path}.\n" f" Run wrist_calibrate_bimanual.py first:\n" f" python -m raiden.calibration.wrist_calibrate_bimanual" ) with open(path) as f: cal = json.load(f) try: ext = cal["cameras"][cam_name]["extrinsics"] except KeyError as e: avail = ", ".join((cal.get("cameras") or {}).keys()) or "(none)" raise KeyError( f"Wrist camera key {cam_name!r} missing from {path}.\n" f" Available camera keys: {avail}\n" f" Re-run wrist_calibrate_bimanual.py with the correct\n" f" --right_wrist_cam / --left_wrist_cam flags." ) from e T = np.eye(4) T[:3, :3] = np.array(ext["rotation_matrix"], dtype=np.float64) T[:3, 3] = np.array(ext["translation_vector"], dtype=np.float64) return T def _check_scene_calibration_present(path: Path, scene_cam_name: str) -> None: """Sanity-check that raiden's CALIBRATION_FILE has BOTH per-arm records keyed by ``{scene_cam_name}__arm1`` / ``__arm2`` (the schema exo_calibrate_bimanual writes). We DO NOT use these extrinsics directly -- we re-solve them live from aruco at startup (the scene cam gets bumped between sessions). But we do verify both records exist as a load-time sanity check so the user knows immediately if they forgot to run the bimanual scene-cam calibration. """ if not path.exists(): raise FileNotFoundError( f"Scene calibration JSON not found at {path}.\n" f" Run exo_calibrate_bimanual.py first:\n" f" python -m raiden.calibration.exo_calibrate_bimanual" ) with open(path) as f: cal = json.load(f) cams = (cal.get("cameras") or {}) missing = [] for suffix in ("__arm1", "__arm2"): if f"{scene_cam_name}{suffix}" not in cams: missing.append(f"{scene_cam_name}{suffix}") if missing: avail = ", ".join(cams.keys()) or "(none)" raise KeyError( f"Scene calibration {path} missing bimanual record(s): " f"{missing}.\n" f" Available camera keys: {avail}\n" f" Re-run exo_calibrate_bimanual.py to populate both\n" f" __arm1 and __arm2 records." ) # --------------------------------------------------------------------------- # Live scene-cam pose recovery (per arm, BOTH boards in the same frame) # --------------------------------------------------------------------------- def _live_calibrate_scene_cam_bimanual( scene_cam, K_s, dist_s, exo_cfg_arm1, exo_cfg_arm2, n_frames: int = 30, top_k: int = 10, ) -> Tuple[np.ndarray, np.ndarray]: """Solve BOTH per-arm scene-cam poses from a single live capture. Returns ``(T_cam_in_arm1_base, T_cam_in_arm2_base)``. Both arms are solved independently from THE SAME captured frames -- one ``cv2.aruco.detectMarkers`` per frame, partitioned by id-range into arm-1 (217-225) and arm-2 (226-234) subsets, each fed to its own ``do_est_aruco_pose`` and pooled into its own ``_PoseHistory``. Mirrors the single-arm ``_live_calibrate_scene_cam_in_base`` from wrist_verify_rerun.py -- same per-frame center-shift undo, same multi-frame pool, same final ``T_aruco_in_cam @ T_aruco_to_link`` chaining -- just doubled up for the two arms. We raise loudly if either arm fails to accumulate enough detections; the user should point the scene cam so BOTH boards are visible (the rig is designed around this constraint). """ from ExoConfigs.exoskeleton import link_to_aruco_transform from exo_utils import do_est_aruco_pose, ARUCO_DICT # Resolve per-arm board configs 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) history_arm1 = _PoseHistory(max_size=n_frames) history_arm2 = _PoseHistory(max_size=n_frames) aruco_params = cv2.aruco.DetectorParameters() attempts = 0 n_arm1_seen = 0 n_arm2_seen = 0 print(f"[live-recal scene] collecting ~{n_frames} frames per arm " f"(boards: 217-225 arm1, 226-234 arm2)...") t0 = time.monotonic() # Time-bound the collection so a stalled board doesn't hang the script. # 20 s is generous -- the single-arm version uses 10 s for one arm. while (n_arm1_seen < n_frames or n_arm2_seen < n_frames) and ( time.monotonic() - t0) < 20.0: bgr = _grab_bgr(scene_cam) if bgr is None: time.sleep(0.01) continue attempts += 1 gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY) try: corners_all, ids_all, _ = cv2.aruco.detectMarkers( gray, ARUCO_DICT, parameters=aruco_params) except Exception: corners_all, ids_all = ([], None) arm1_est, arm2_est = _partition_corners(corners_all, ids_all) for (corners_est, history, aruco_board, board_length, T_aruco_to_link, seen_counter_name) in ( (arm1_est, history_arm1, aruco_board_arm1, board_length_arm1, T_aruco_to_link_arm1, "arm1"), (arm2_est, history_arm2, aruco_board_arm2, board_length_arm2, T_aruco_to_link_arm2, "arm2"), ): if corners_est is None: continue try: r = do_est_aruco_pose( bgr, ARUCO_DICT, aruco_board, board_length, cameraMatrix=K_s, distCoeffs=dist_s, corners_est=corners_est, ) except Exception: continue if r == -1: continue rvec, tvec_shifted = r["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 = r["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_s, dist_s) resid = float(np.linalg.norm( proj_chk.reshape(-1, 2) - img_pts.reshape(-1, 2), axis=1).mean()) T_aruco_in_cam_single = r["est_aruco_pose"] history.add(obj_board, img_pts, resid, single_frame_t=T_aruco_in_cam_single[:3, 3]) if seen_counter_name == "arm1": n_arm1_seen += 1 else: n_arm2_seen += 1 if attempts % 10 == 0: print(f" attempt {attempts:>3}: arm1_seen={n_arm1_seen} " f"arm2_seen={n_arm2_seen} (target {n_frames} each)") def _solve_one(history, T_aruco_to_link, board_length, name): obj_pool, img_pool, n_used, mean_resid = ( history.top_k_concatenated(k=top_k)) if obj_pool is None or len(obj_pool) < 4: raise RuntimeError( f"live scene-cam recal ({name}): only {n_used} pooled " f"frames after {attempts} attempts -- aim the scene cam " f"at the {name} exo board (id-range mismatch?)" ) T_aruco_in_cam, mf_resid = _solve_multiframe_pose( obj_pool, img_pool, K_s, dist_s, board_length) if T_aruco_in_cam is None: raise RuntimeError( f"live scene-cam recal ({name}): multi-frame solve failed" ) T_link_in_cam = T_aruco_in_cam @ T_aruco_to_link T_cam_in_base = np.linalg.inv(T_link_in_cam) print(f"[live-recal scene/{name}] OK used={n_used} frames, " f"avg_single_resid={mean_resid:.2f}px, " f"multi_resid={mf_resid:.2f}px") print(f"[live-recal scene/{name}] cam_in_base xyz=(" f"{T_cam_in_base[0,3]:+.3f}, {T_cam_in_base[1,3]:+.3f}, " f"{T_cam_in_base[2,3]:+.3f})") return T_cam_in_base T_cam_in_arm1_base = _solve_one( history_arm1, T_aruco_to_link_arm1, board_length_arm1, "arm1") T_cam_in_arm2_base = _solve_one( history_arm2, T_aruco_to_link_arm2, board_length_arm2, "arm2") return T_cam_in_arm1_base, T_cam_in_arm2_base # --------------------------------------------------------------------------- # FK (shared between arms -- both YAMs have identical morphology) # --------------------------------------------------------------------------- def _fk_base_to_ee(joint_state_7: np.ndarray) -> np.ndarray: """``T_base_ee`` from a YAM follower's 7-vec joint state. Shared kinematics object between both arms (the bimanual variant differs only in world placement, irrelevant to per-arm FK in the arm's OWN base frame). Lifted verbatim from the single-arm verify script. """ from i2rt.robots.kinematics import Kinematics from raiden._xml_paths import get_yam_4310_linear_xml_path global _KIN_CACHE try: kin = _KIN_CACHE # type: ignore[name-defined] except NameError: kin = Kinematics(get_yam_4310_linear_xml_path(), site_name="grasp_site") globals()["_KIN_CACHE"] = kin q = np.zeros(8, dtype=np.float64) q[:6] = joint_state_7[:6] return np.asarray(kin.fk(q), dtype=np.float64) def _smooth_move(robot, target_7, max_vel=0.25, min_s=0.5, steps=100): """Smooth-move one follower to a 7-vec joint target. Identical to the single-arm helper -- duplicated rather than imported because wrist_verify_rerun.py defines it module-locally and we don't want to take a runtime dep on it (the hard "don't modify any other file" rule means we can't refactor the helper into a shared module). """ from raiden.robot.controller import smooth_move_joints current = np.asarray(robot.get_joint_pos(), dtype=np.float64) target = np.asarray(target_7, dtype=np.float64) delta = float(np.max(np.abs(target - current))) t_s = max(min_s, delta / max(max_vel, 1e-6)) print(f" smooth_move dmax={delta:.3f} -> {t_s:.2f}s") smooth_move_joints(robot, target, time_interval_s=t_s, steps=steps) # --------------------------------------------------------------------------- # qpos discovery (per-arm prefix lookup -- matches exo_calibrate_bimanual) # --------------------------------------------------------------------------- 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, ``"arm2_"`` for arm 2 (Demos.sim_yam_bimanual._prefix_names prefixed every joint/body/ site for arm 2). Lifted from exo_calibrate_bimanual._discover_arm_qpos -- duplicated here to keep this module standalone (single-file constraint, see module docstring). """ 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 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. Swallow-and-continue on a transient CAN hiccup -- a momentary stall shouldn't kill the daemon. Returns the live 7-vec joint state (or ``None`` on failure). Both arms call this each frame in the bimanual capture loop. """ if follower is None or not arm_qpos_idx: return None 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 return np.asarray(q, dtype=np.float64) except Exception: return None # --------------------------------------------------------------------------- # Per-arm dot-grid floor projection (the headline viz) # --------------------------------------------------------------------------- def _build_wrist_dot_grid(W_w: int, H_w: int, K_w: np.ndarray, T_base_wrist: np.ndarray, grid_step_px: int, pad_frac: float = 0.15 ) -> Tuple[np.ndarray, np.ndarray]: """Generate a uniform pixel grid on the wrist image, then back- project each pixel onto the z=0 plane in the arm's BASE frame. Returns ``(valid_uv, pts_3d_in_base)`` where ``valid_uv`` is shape ``(N, 2)`` (pixel coords that hit the floor with positive depth) and ``pts_3d_in_base`` is shape ``(N, 3)`` (the same indices, expressed in the arm's base frame). Math (lifted verbatim from wrist_verify_rerun.py): For each pixel (u, v), build the ray direction in CAM frame: d_cam = [(u-cx)/fx, (v-cy)/fy, 1] Rotate into base frame: d_base = R_base_wrist @ d_cam. Origin in base frame: O = T_base_wrist[:3, 3]. Intersect with z=0: t = -O[z] / d_base[z], drop if t<=0 or |d_base[z]| < eps (parallel to the floor / pointing up). Floor point in base: pt_base = O + t * d_base. ``grid_step_px`` controls the spacing of the pixel grid -- we pick ``ceil((W*(1-2*pad)) / step)`` columns and similarly for rows. """ pad_x = W_w * pad_frac pad_y = H_w * pad_frac n_cols = max(2, int(np.ceil((W_w - 2 * pad_x) / max(grid_step_px, 1))) + 1) n_rows = max(2, int(np.ceil((H_w - 2 * pad_y) / max(grid_step_px, 1))) + 1) us = np.linspace(pad_x, W_w - pad_x, n_cols) vs = np.linspace(pad_y, H_w - pad_y, n_rows) uu, vv = np.meshgrid(us, vs) grid_uv = np.stack([uu.flatten(), vv.flatten()], axis=-1) fx_w, fy_w = K_w[0, 0], K_w[1, 1] cx_w, cy_w = K_w[0, 2], K_w[1, 2] O = T_base_wrist[:3, 3] R_bw = T_base_wrist[:3, :3] valid_uv = [] pts_3d = [] for u, v in grid_uv: d_cam = np.array([(u - cx_w) / fx_w, (v - cy_w) / fy_w, 1.0]) d_base = R_bw @ d_cam if abs(d_base[2]) < 1e-6: continue t = -O[2] / d_base[2] if t <= 0: continue valid_uv.append([u, v]) pts_3d.append(O + t * d_base) return ( np.array(valid_uv) if valid_uv else np.zeros((0, 2)), np.array(pts_3d) if pts_3d else np.zeros((0, 3)), ) def _project_to_scene(pts_3d_in_base: np.ndarray, T_cam_in_base: np.ndarray, K_s: np.ndarray, W_s: int, H_s: int ) -> Tuple[np.ndarray, np.ndarray]: """Project base-frame 3D points into the scene cam's pixel space. Returns ``(uv_scene, in_bounds_mask)``. ``uv_scene`` is shape ``(N, 2)`` (NaN-safe even on points behind the camera); the bool mask is True where the point is in front of the camera AND inside the scene image. Uses ``T_scene_base = inv(T_cam_in_base)`` -- i.e. the same transform direction the single-arm verify script uses. Each arm's dots use THAT ARM'S own ``T_cam_in_armN_base`` so the loop closes per-arm independently (no bimanual-world reframing needed for the dots themselves). """ if len(pts_3d_in_base) == 0: return np.zeros((0, 2)), np.zeros((0,), dtype=bool) T_scene_base = np.linalg.inv(T_cam_in_base) P_h = np.c_[pts_3d_in_base, np.ones(len(pts_3d_in_base))] P_sc = (T_scene_base @ P_h.T).T[:, :3] front = P_sc[:, 2] > 0.05 # ignore points within 5 cm of the cam plane z_safe = np.where(front, P_sc[:, 2], 1.0) us_s = K_s[0, 0] * P_sc[:, 0] / z_safe + K_s[0, 2] vs_s = K_s[1, 1] * P_sc[:, 1] / z_safe + K_s[1, 2] in_bounds = (front & (us_s >= 0) & (us_s < W_s) & (vs_s >= 0) & (vs_s < H_s)) return np.stack([us_s, vs_s], axis=-1), in_bounds def _draw_dot_pair_on_panels( wrist_canvas, scene_canvas, valid_uv: np.ndarray, scene_uv: np.ndarray, in_bounds: np.ndarray, color: Tuple[int, int, int], dot_radius_px: int, label_prefix: str = "", ): """Draw the FILLED dots on the wrist panel and matching OPEN circles on the scene panel. ``in_bounds[i] = False`` greys out that dot on the wrist panel and SKIPS the open circle on the scene panel -- a back-projection that lands off-screen is no use to the user as a visual consistency cue. ``label_prefix`` (e.g. ``"R"``, ``"L"``) is prepended to the dot index so the bimanual scene panel can disambiguate which arm a given numbered circle came from when the two grids overlap. Drawn order: wrist filled dot + index, scene open circle + index. Index labels are drawn with a white halo (thick white draw, thin coloured draw on top) so they're legible against any background. """ if len(valid_uv) == 0: return open_radius = dot_radius_px + 4 # scene open circles slightly larger for i, ((u, v), ok) in enumerate(zip(valid_uv, in_bounds)): col = color if ok else _GRAY u_int, v_int = int(round(u)), int(round(v)) cv2.circle(wrist_canvas, (u_int, v_int), dot_radius_px, col, -1, cv2.LINE_AA) label = f"{label_prefix}{i}" cv2.putText(wrist_canvas, label, (u_int + dot_radius_px + 2, v_int - dot_radius_px - 2), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 3, cv2.LINE_AA) cv2.putText(wrist_canvas, label, (u_int + dot_radius_px + 2, v_int - dot_radius_px - 2), cv2.FONT_HERSHEY_SIMPLEX, 0.6, col, 1, cv2.LINE_AA) if ok: us_int = int(round(scene_uv[i, 0])) vs_int = int(round(scene_uv[i, 1])) cv2.circle(scene_canvas, (us_int, vs_int), open_radius, color, 2, cv2.LINE_AA) cv2.putText(scene_canvas, label, (us_int + open_radius + 2, vs_int - open_radius - 2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 3, cv2.LINE_AA) cv2.putText(scene_canvas, label, (us_int + open_radius + 2, vs_int - open_radius - 2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 1, cv2.LINE_AA) # --------------------------------------------------------------------------- # Daemon capture loop (bimanual) # --------------------------------------------------------------------------- def _capture_loop_bimanual( scene_cam, wrist_cam_arm1, wrist_cam_arm2, K_s, dist_s, W_s, H_s, K_w1, dist_w1, W_w1, H_w1, K_w2, dist_w2, W_w2, H_w2, follower_arm1, follower_arm2, T_cam_in_arm1_base, T_cam_in_arm2_base, T_ee_cam_arm1, T_ee_cam_arm2, exo_cfg_arm1, exo_cfg_arm2, model, data, arm1_qpos_idx, arm1_grip_qpos_idx, arm2_qpos_idx, arm2_grip_qpos_idx, margin: float, rr, state: _State, viz_width: int, jpeg_quality: int, viz_hz: float, grid_step_px: int, dot_radius_px: int, ): """Per-frame: 1. Grab scene + BOTH wrist frames. 2. Pump live follower joint states into mujoco qpos for BOTH arms. 3. Re-anchor arm 2's body_pos/quat from the LIVE per-arm detection delta (same trick exo_calibrate_bimanual uses). 4. mj_forward + re-attach arm 2's exo/aruco mocap meshes. 5. Detect ArUco markers on the scene image; split per-arm. 6. Render the bimanual mujoco model from the SHARED scene-cam pose (anchored via T_arm1_in_world @ T_cam_in_arm1_base) and from EACH arm's wrist-cam pose (anchored via T_armN_in_world @ T_base_wristN). 7. Build per-arm wrist-pixel grids, back-project to z=0 in that arm's base frame, project into scene pixels. 8. Draw the panels and log to Rerun (throttled to viz_hz). All bimanual-specific logic is in this function; the helpers it calls are factored out for readability. See the heavy comment in exo_calibrate_bimanual._capture_loop_bimanual for the frame-of- reference reminder block -- this loop follows the same conventions. """ 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 # Resolve per-arm board configs 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_link_to_aruco_arm2 = link_to_aruco_transform(link_cfg_arm2) # Bimanual-world frame: 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) # Snapshot the seed scene-cam-in-arm1-world that we'll use for the # bimanual scene render. The same "arm 1 is canonical" choice # exo_calibrate_bimanual makes; documented there. T_cam_in_world_seed = T_arm1_in_world @ T_cam_in_arm1_base T_world_in_scenecam = np.linalg.inv(T_cam_in_world_seed) # MuJoCo's EGL context is thread-bound -- create one in THIS daemon # thread before any renderer (same caveat the single-arm script and # exo_calibrate_bimanual handle). try: _gl_ctx = mujoco.GLContext(max(W_s, W_w1, W_w2), max(H_s, H_w1, H_w2)) _gl_ctx.make_current() print(f"[verify_bimanual] daemon-thread GL context created") except Exception as _exc: print(f"[verify_bimanual] daemon GL context failed: {_exc}") _gl_ctx = None arm2_body_id = model.body("arm2_arm").id aruco_params = cv2.aruco.DetectorParameters() log_interval = 1.0 / max(viz_hz, 1.0) last_log = 0.0 frame_idx = 0 while not state.stop: scene_bgr = _grab_bgr(scene_cam) wrist1_bgr = _grab_bgr(wrist_cam_arm1) wrist2_bgr = _grab_bgr(wrist_cam_arm2) if scene_bgr is None or wrist1_bgr is None or wrist2_bgr is None: time.sleep(0.01) continue # --- Pump live joints into mujoco for BOTH arms. live_q_arm1 = _drive_arm_qpos( follower_arm1, data, arm1_qpos_idx, arm1_grip_qpos_idx) live_q_arm2 = _drive_arm_qpos( follower_arm2, data, arm2_qpos_idx, arm2_grip_qpos_idx) if live_q_arm1 is None or live_q_arm2 is None: time.sleep(0.01) continue # --- Live per-arm scene detection (for re-anchoring arm 2 in # world frame each frame, and for the scene/rgb_aruco panel). try: gray_s = cv2.cvtColor(scene_bgr, cv2.COLOR_BGR2GRAY) corners_all, ids_all, _ = cv2.aruco.detectMarkers( gray_s, ARUCO_DICT, parameters=aruco_params) except Exception: corners_all, ids_all = ([], None) arm1_est_s, arm2_est_s = _partition_corners(corners_all, ids_all) # Per-arm board pose in the scene cam (used for the aruco # overlay AND for the arm-2 re-anchoring trick). T_link_arm1_in_scenecam = None T_link_arm2_in_scenecam = None if arm1_est_s is not None: try: r1 = do_est_aruco_pose( scene_bgr, ARUCO_DICT, aruco_board_arm1, board_length_arm1, cameraMatrix=K_s, distCoeffs=dist_s, corners_est=arm1_est_s) if r1 != -1: T_link_arm1_in_scenecam = ( r1["est_aruco_pose"] @ np.linalg.inv(T_link_to_aruco_arm1)) except Exception: pass if arm2_est_s is not None: try: r2 = do_est_aruco_pose( scene_bgr, ARUCO_DICT, aruco_board_arm2, board_length_arm2, cameraMatrix=K_s, distCoeffs=dist_s, corners_est=arm2_est_s) if r2 != -1: T_link_arm2_in_scenecam = ( r2["est_aruco_pose"] @ np.linalg.inv(T_link_to_aruco_arm2)) except Exception: pass # --- Re-anchor arm 2 in mujoco world frame each frame from the # LIVE per-arm scene detection. Same trick exo_calibrate_bimanual # uses -- the physical rig spacing isn't always exactly --margin, # so we set arm 2's body_pos / body_quat from the actual board # detection delta. Falls through silently if either board isn't # currently visible (the last known body_pos persists). if T_link_arm1_in_scenecam is not None and T_link_arm2_in_scenecam is not None: # T_cam_in_armN_base = inv(T_link_armN_in_cam) T_cam_in_arm1_now = np.linalg.inv(T_link_arm1_in_scenecam) T_cam_in_arm2_now = np.linalg.inv(T_link_arm2_in_scenecam) T_arm2_in_arm1 = T_cam_in_arm1_now @ np.linalg.inv(T_cam_in_arm2_now) 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]] # wxyz mujoco.mj_forward(model, data) # Re-attach arm 2's exo + aruco mocap meshes after its base moved. 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 loop on a transient exo-attach error. pass # --- Per-arm FK + per-arm wrist-cam pose in WORLD frame. T_base_ee_arm1 = _fk_base_to_ee(live_q_arm1) T_base_ee_arm2 = _fk_base_to_ee(live_q_arm2) T_base_wrist_arm1 = T_base_ee_arm1 @ T_ee_cam_arm1 T_base_wrist_arm2 = T_base_ee_arm2 @ T_ee_cam_arm2 # Lift each per-arm wrist into bimanual world frame for the # mujoco render. (The dot-grid math operates in each arm's OWN # base frame; only the renders need the world lift.) T_world_in_wristcam_arm1 = np.linalg.inv( T_arm1_in_world @ T_base_wrist_arm1) T_world_in_wristcam_arm2 = np.linalg.inv( T_arm2_in_world @ T_base_wrist_arm2) # --- scene/rgb_aruco: BOTH boards overlaid with per-arm colours. scene_ann = scene_bgr.copy() if ids_all is not None: cv2.aruco.drawDetectedMarkers(scene_ann, corners_all, ids_all) if T_link_arm1_in_scenecam is not None: T_aruco_in_cam_arm1 = T_link_arm1_in_scenecam @ T_link_to_aruco_arm1 _draw_aruco_plane(scene_ann, T_aruco_in_cam_arm1, board_length_arm1, K_s, color=(0, 255, 255)) # yellow ↔ arm 1 board if T_link_arm2_in_scenecam is not None: T_aruco_in_cam_arm2 = T_link_arm2_in_scenecam @ T_link_to_aruco_arm2 _draw_aruco_plane(scene_ann, T_aruco_in_cam_arm2, board_length_arm2, K_s, color=(255, 0, 255)) # magenta ↔ arm 2 board # --- scene/rgb_overlay: bimanual mujoco render through the # shared scene cam. overlay_s_bgr = scene_bgr.copy() try: rendered_s, seg_s = _render_with_intrinsics( model, data, T_world_in_scenecam, K_s, W_s, H_s) comp_s = _compose_overlay(scene_bgr, rendered_s, seg_mask=seg_s) overlay_s_bgr = cv2.cvtColor(comp_s, cv2.COLOR_RGB2BGR) fg_s = (seg_s[:, :, 0] != -1).astype(np.uint8) * 255 cs, _ = cv2.findContours( fg_s, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cv2.drawContours(overlay_s_bgr, cs, -1, (0, 0, 255), 2, cv2.LINE_AA) except Exception: if not getattr(_capture_loop_bimanual, "_scene_err", False): import traceback print("[verify_bimanual] scene render failed:") traceback.print_exc() _capture_loop_bimanual._scene_err = True # --- wrist_armN/rgb_overlay: per-arm mujoco render through that # arm's wrist cam pose. We render the FULL bimanual model # (both arms + both boards), because at close enough wrist # positions arm 2 could be in arm 1's wrist view (and vice # versa); that "wrong" arm in the rendered overlay is a useful # sanity check, not a bug. overlay_w1_bgr = wrist1_bgr.copy() overlay_w2_bgr = wrist2_bgr.copy() try: rendered_w1, seg_w1 = _render_with_intrinsics( model, data, T_world_in_wristcam_arm1, K_w1, W_w1, H_w1) comp_w1 = _compose_overlay(wrist1_bgr, rendered_w1, seg_mask=seg_w1) overlay_w1_bgr = cv2.cvtColor(comp_w1, cv2.COLOR_RGB2BGR) fg_w1 = (seg_w1[:, :, 0] != -1).astype(np.uint8) * 255 cw1, _ = cv2.findContours( fg_w1, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cv2.drawContours(overlay_w1_bgr, cw1, -1, (0, 0, 255), 2, cv2.LINE_AA) except Exception: if not getattr(_capture_loop_bimanual, "_wrist1_err", False): import traceback print("[verify_bimanual] arm-1 wrist render failed:") traceback.print_exc() _capture_loop_bimanual._wrist1_err = True try: rendered_w2, seg_w2 = _render_with_intrinsics( model, data, T_world_in_wristcam_arm2, K_w2, W_w2, H_w2) comp_w2 = _compose_overlay(wrist2_bgr, rendered_w2, seg_mask=seg_w2) overlay_w2_bgr = cv2.cvtColor(comp_w2, cv2.COLOR_RGB2BGR) fg_w2 = (seg_w2[:, :, 0] != -1).astype(np.uint8) * 255 cw2, _ = cv2.findContours( fg_w2, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cv2.drawContours(overlay_w2_bgr, cw2, -1, (0, 0, 255), 2, cv2.LINE_AA) except Exception: if not getattr(_capture_loop_bimanual, "_wrist2_err", False): import traceback print("[verify_bimanual] arm-2 wrist render failed:") traceback.print_exc() _capture_loop_bimanual._wrist2_err = True # --- Dot grids per arm. valid_uv_1, pts_3d_1 = _build_wrist_dot_grid( W_w1, H_w1, K_w1, T_base_wrist_arm1, grid_step_px) valid_uv_2, pts_3d_2 = _build_wrist_dot_grid( W_w2, H_w2, K_w2, T_base_wrist_arm2, grid_step_px) scene_uv_1, in_bounds_1 = _project_to_scene( pts_3d_1, T_cam_in_arm1_base, K_s, W_s, H_s) scene_uv_2, in_bounds_2 = _project_to_scene( pts_3d_2, T_cam_in_arm2_base, K_s, W_s, H_s) wrist1_dots = wrist1_bgr.copy() wrist2_dots = wrist2_bgr.copy() scene_dots = scene_bgr.copy() # Arm 1 first (red) so arm 2's blue circles draw over the top # if they overlap -- doesn't actually matter for correctness; # if circles overlap that ITSELF is a useful visual cue that # both wrists are pointed at the same physical floor patch. _draw_dot_pair_on_panels( wrist1_dots, scene_dots, valid_uv_1, scene_uv_1, in_bounds_1, color=_ARM1_COLOR, dot_radius_px=dot_radius_px, label_prefix="R", ) _draw_dot_pair_on_panels( wrist2_dots, scene_dots, valid_uv_2, scene_uv_2, in_bounds_2, color=_ARM2_COLOR, dot_radius_px=dot_radius_px, label_prefix="L", ) # --- Log (throttled). now = time.monotonic() if now - last_log >= log_interval: last_log = now import rerun as _rr rr.set_time("step", sequence=frame_idx) rr.log("scene/rgb_aruco", _to_rerun_jpeg(scene_ann, target_w=viz_width, quality=jpeg_quality)) rr.log("scene/rgb_overlay", _to_rerun_jpeg(overlay_s_bgr, target_w=viz_width, quality=jpeg_quality)) rr.log("scene/rgb_dots", _to_rerun_jpeg(scene_dots, target_w=viz_width, quality=jpeg_quality)) rr.log("wrist_arm1/rgb_overlay", _to_rerun_jpeg(overlay_w1_bgr, target_w=viz_width, quality=jpeg_quality)) rr.log("wrist_arm2/rgb_overlay", _to_rerun_jpeg(overlay_w2_bgr, target_w=viz_width, quality=jpeg_quality)) rr.log("wrist_arm1/rgb_dots", _to_rerun_jpeg(wrist1_dots, target_w=viz_width, quality=jpeg_quality)) rr.log("wrist_arm2/rgb_dots", _to_rerun_jpeg(wrist2_dots, target_w=viz_width, quality=jpeg_quality)) n1_in = int(in_bounds_1.sum()) if len(in_bounds_1) else 0 n2_in = int(in_bounds_2.sum()) if len(in_bounds_2) else 0 rr.log("info", _rr.TextDocument( f"verify_bimanual | " f"arm1(red) dots: {len(pts_3d_1)} hit floor, " f"{n1_in} into scene | " f"arm2(blue) dots: {len(pts_3d_2)} hit floor, " f"{n2_in} into scene\n" f" arm1 live_q = {live_q_arm1}\n" f" arm2 live_q = {live_q_arm2}" )) frame_idx += 1 # --------------------------------------------------------------------------- # Main orchestration # --------------------------------------------------------------------------- def run_wrist_verify_bimanual( scene_cam_name: str, right_wrist_cam_name: str, left_wrist_cam_name: str, wrist_calibration_file: Path, scene_calibration_file: Path, camera_config_file: Path, vis_port: int, scene_resolution: str, right_wrist_resolution: str, left_wrist_resolution: str, margin: float, grid_step_px: int, dot_radius_px: int, viz_width: int, jpeg_quality: int, viz_hz: float, ): """End-to-end bimanual wrist + scene extrinsics verification. Heavy raiden imports (RobotController, ExoConfigs, exo_utils, rerun) are deferred until inside this function -- same lazy-import policy the single-arm verify and the bimanual calibration scripts follow -- so module import stays cheap for tools that only want the helpers. """ import rerun as rr from raiden.robot.controller import RobotController from ExoConfigs.yam_exo import YAM_BASE_ONLY_CONFIG, YAM_BASE_ONLY_CONFIG_ARM2 # ─── v2-reverse arm-2 board patch (russet bimanual rig) ──────────────── # See exo_calibrate_bimanual_fidex.py for the rationale. The bimanual # rig at russet flips arm-2's base board across x — both the ArUco # offset and the printed STL mesh are mirrored. This patch is applied # BEFORE the mujoco render model is built so the on-screen overlay # geometry matches the physical board. _ARM2_V2_STL = str(Path(_EXO_DIR) / "so100_blender_testings" / "yam_base_board_v2_reverse_clearance_fixed.stl") print(f" • arm2 v2-reverse patch: STL={Path(_ARM2_V2_STL).name}, " f"aruco_offset_pos x sign flipped") 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]]) # ────────────────────────────────────────────────────────────────────── from exo_utils import ( get_link_poses_from_robot, position_exoskeleton_meshes) from Demos.sim_yam_bimanual import build_bimanual_xml # --- Per-arm wrist extrinsics: read both at startup. # ee-fixed -> safe to cache for the whole session (the wrist cam # is rigidly bolted to the ee mount; nothing moves between sessions). T_ee_cam_arm1 = _load_wrist_extrinsic( wrist_calibration_file, right_wrist_cam_name) T_ee_cam_arm2 = _load_wrist_extrinsic( wrist_calibration_file, left_wrist_cam_name) print(f"T_ee_cam_arm1 (right) translation: {T_ee_cam_arm1[:3, 3]}") print(f"T_ee_cam_arm2 (left) translation: {T_ee_cam_arm2[:3, 3]}") # --- Sanity-check scene calibration. The actual T's we use come # from a live aruco recal below (the scene cam gets bumped between # sessions), but we fail loudly here if the bimanual scene calib # was never run so the user doesn't get a cryptic detection failure # 30 s later. _check_scene_calibration_present(scene_calibration_file, scene_cam_name) # --- Cameras (1 scene + 2 wrists). scene_serial = _scene_serial_from_config( str(camera_config_file), scene_cam_name) right_wrist_serial = _scene_serial_from_config( str(camera_config_file), right_wrist_cam_name) left_wrist_serial = _scene_serial_from_config( str(camera_config_file), left_wrist_cam_name) print(f"Opening scene camera (serial={scene_serial}) at {scene_resolution}...") scene_cam, K_s, dist_s, W_s, H_s = _open_zed_native( scene_serial, scene_resolution) print(f"Opening right-wrist camera (serial={right_wrist_serial}) at {right_wrist_resolution}...") wrist_cam_arm1, K_w1, dist_w1, W_w1, H_w1 = _open_zed_native( right_wrist_serial, right_wrist_resolution) print(f"Opening left-wrist camera (serial={left_wrist_serial}) at {left_wrist_resolution}...") wrist_cam_arm2, K_w2, dist_w2, W_w2, H_w2 = _open_zed_native( left_wrist_serial, left_wrist_resolution) # --- Live scene-cam recal (BOTH arms, single capture pass). # The exo cfg deepcopy mirrors the bimanual scene-cam calibration # script: arm 2's pybullet_name needs to point at the prefixed # body name in the bimanual mujoco model, even though here we only # use it for the link_cfg / aruco_board lookups (not for body # placement). Deepcopy is cheap and keeps shared state clean. 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" T_cam_in_arm1_base, T_cam_in_arm2_base = _live_calibrate_scene_cam_bimanual( scene_cam, K_s, dist_s, exo_cfg_arm1, exo_cfg_arm2) # --- Bimanual mujoco model (same build as exo_calibrate_bimanual). # cwd into exo_redo so the XML's relative texture/mesh paths # resolve (same trick the single-arm scripts use). os.chdir(str(_EXO_DIR)) # ``build_bimanual_xml`` already emits ```` (see sim_yam_bimanual.py, build site). # Do NOT re-inject balanceinertia here — MuJoCo rejects duplicate # attributes with "XML parse error 7" and the model fails to load. # Same goes for the background-include strip: ``include_background # =False`` already suppresses that include in the wrapper XML. 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) 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 " f"({model.nbody} bodies, margin={margin:.3f} m)") # --- Per-arm qpos indices. 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}") # --- Rerun. _init_rerun(vis_port) # Pin ALL 7 panels via an explicit blueprint — rerun's auto-layout # reliably buries some entities (wrist_arm2/rgb_dots went invisible, # observed 2026-07-08). Grid of: scene aruco/overlay/dots + per-arm # wrist overlay/dots. try: import rerun as _rr_bp import rerun.blueprint as _rrb _views = [ _rrb.Spatial2DView(origin="scene/rgb_aruco", name="scene aruco"), _rrb.Spatial2DView(origin="scene/rgb_overlay", name="scene overlay"), _rrb.Spatial2DView(origin="scene/rgb_dots", name="scene dots"), _rrb.Spatial2DView(origin="wrist_arm1/rgb_overlay", name="R wrist overlay"), _rrb.Spatial2DView(origin="wrist_arm1/rgb_dots", name="R wrist dots"), _rrb.Spatial2DView(origin="wrist_arm2/rgb_overlay", name="L wrist overlay"), _rrb.Spatial2DView(origin="wrist_arm2/rgb_dots", name="L wrist dots"), _rrb.TextDocumentView(origin="info", name="info"), ] _rr_bp.send_blueprint(_rrb.Blueprint(_rrb.Grid(*_views), collapse_panels=True)) print(" [verify] blueprint sent (8 views pinned)") except Exception as _bpe: print(f" [verify] blueprint failed ({_bpe}); viewer auto-layout") # --- Robot (BOTH leaders + BOTH followers; same as bimanual calibrate). rc = RobotController( use_right_leader=True, use_left_leader=True, use_right_follower=True, use_left_follower=True, ) rc.setup_for_teleop_recording() rc.enable_estop() rc.start_teleoperation() follower_arm1 = rc.follower_r # right -> arm 1 (dataset recorder convention) follower_arm2 = rc.follower_l # left -> arm 2 state = _State() th = threading.Thread( target=_capture_loop_bimanual, daemon=True, args=(scene_cam, wrist_cam_arm1, wrist_cam_arm2, K_s, dist_s, W_s, H_s, K_w1, dist_w1, W_w1, H_w1, K_w2, dist_w2, W_w2, H_w2, follower_arm1, follower_arm2, T_cam_in_arm1_base, T_cam_in_arm2_base, T_ee_cam_arm1, T_ee_cam_arm2, exo_cfg_arm1, exo_cfg_arm2, model, data, arm1_qpos_idx, arm1_grip_qpos_idx, arm2_qpos_idx, arm2_grip_qpos_idx, margin, rr, state, viz_width, jpeg_quality, viz_hz, grid_step_px, dot_radius_px, ), ) th.start() print() print("=" * 72) print(" BIMANUAL verify -- teleop BOTH leaders. Watch the rerun panels:") print(" scene/rgb_aruco scene cam + BOTH boards annotated") print(" scene/rgb_overlay scene cam + bimanual mesh overlay") print(" wrist_arm1/rgb_overlay right wrist + arm 1 mesh") print(" wrist_arm2/rgb_overlay left wrist + arm 2 mesh") print(" wrist_arm1/rgb_dots right wrist + RED filled dots") print(" wrist_arm2/rgb_dots left wrist + BLUE filled dots") print(" scene/rgb_dots scene cam + RED/BLUE open circles") print(" Filled-dot-i (red, R-prefix) on arm1 wrist <->") print(" open-circle-Ri (red) on scene = same physical floor point.") print(" Filled-dot-i (blue, L-prefix) on arm2 wrist <->") print(" open-circle-Li (blue) on scene = same physical floor point.") print(" Press YELLOW leader button (either) to END + return BOTH home.") print("=" * 72) print() try: while True: if rc.check_button_press() is not None: print("\n[yellow] END -- returning both arms home") rc.stop_teleoperation() break time.sleep(0.05) state.stop = True time.sleep(0.2) # Sequenced (arm 1 then arm 2). Concurrent smooth-moves would # save ~5 s but two YAMs at close range swinging through space # at the same time is a collision hazard; the bimanual wrist # calibration script makes the same call for the same reason. print("[cleanup] arm1 (right): current -> INTERMEDIATE") _smooth_move(follower_arm1, INTERMEDIATE_POSE_RIGHT) print("[cleanup] arm1 (right): intermediate -> HOME") _smooth_move(follower_arm1, HOME_POSE) print("\n[cleanup] arm2 (left): current -> INTERMEDIATE") _smooth_move(follower_arm2, INTERMEDIATE_POSE_LEFT) print("[cleanup] arm2 (left): intermediate -> HOME") _smooth_move(follower_arm2, HOME_POSE) finally: state.stop = True time.sleep(0.2) for cam in (scene_cam, wrist_cam_arm1, wrist_cam_arm2): try: cam.close() except Exception: pass try: rc.cleanup() except SystemExit: pass def main(): # CALIBRATION_FILE / CAMERA_CONFIG are read lazily to keep the CLI # parser fast when --help is invoked. from raiden._config import CALIBRATION_FILE, CAMERA_CONFIG p = argparse.ArgumentParser( description="Bimanual wrist + scene extrinsics verification. " "Streams 7 Rerun panels: scene aruco/overlay/dots and " "per-arm wrist overlay/dots. Arm 1 (right) dots = RED, " "arm 2 (left) dots = BLUE.") p.add_argument("--scene_cam", default="scene_camera") p.add_argument("--right_wrist_cam", default="right_wrist_camera") p.add_argument("--left_wrist_cam", default="left_wrist_camera") p.add_argument( "--wrist_calibration_file", default=str(_DEFAULT_WRIST_CAL_BIMANUAL), help="Path to bimanual wrist calibration JSON (produced by " "wrist_calibrate_bimanual.py).", ) p.add_argument( "--scene_calibration_file", default=str(CALIBRATION_FILE), help="Path to raiden CALIBRATION_FILE that holds the " "{scene_cam}__arm1 / __arm2 records (produced by " "exo_calibrate_bimanual.py).", ) p.add_argument( "--camera_config_file", default=str(CAMERA_CONFIG), help="Path to raiden camera.json mapping cam name -> ZED serial.", ) p.add_argument("--vis_port", type=int, default=9092) p.add_argument("--scene_resolution", default="HD1080", choices=("HD720", "HD1080", "HD2K")) p.add_argument("--right_wrist_resolution", default="HD1080", choices=("HD720", "HD1080", "HD2K")) p.add_argument("--left_wrist_resolution", default="HD1080", choices=("HD720", "HD1080", "HD2K")) p.add_argument("--margin", type=float, default=0.7, help="Lateral distance between the two YAM arm bases " "(m). MUST match the physical rig + the value " "passed to exo_calibrate_bimanual. Used to lift " "per-arm camera poses into the bimanual mujoco " "world frame for the renders.") p.add_argument("--grid_step_px", type=int, default=80, help="Pixel spacing of the dot grid on each wrist " "image. Smaller = denser dots. The grid auto-" "sizes to fit the image with a 15% padding.") p.add_argument("--dot_radius_px", type=int, default=6, help="Radius (pixels) of the filled dots on the " "wrist panels. Open circles on the scene panel " "are drawn 4 px larger for visual contrast.") p.add_argument("--viz_width", type=int, default=1280, help="Rerun-side resize width before JPEG-encoding.") p.add_argument("--jpeg_quality", type=int, default=75) p.add_argument("--viz_hz", type=float, default=10.0, help="Rerun image-log rate cap (Hz).") args = p.parse_args() run_wrist_verify_bimanual( scene_cam_name=args.scene_cam, right_wrist_cam_name=args.right_wrist_cam, left_wrist_cam_name=args.left_wrist_cam, wrist_calibration_file=Path(args.wrist_calibration_file).expanduser(), scene_calibration_file=Path(args.scene_calibration_file).expanduser(), camera_config_file=Path(args.camera_config_file).expanduser(), vis_port=args.vis_port, scene_resolution=args.scene_resolution, right_wrist_resolution=args.right_wrist_resolution, left_wrist_resolution=args.left_wrist_resolution, margin=args.margin, grid_step_px=args.grid_step_px, dot_radius_px=args.dot_radius_px, viz_width=args.viz_width, jpeg_quality=args.jpeg_quality, viz_hz=args.viz_hz, ) if __name__ == "__main__": main()