"""Bimanual wrist-camera calibration via live teleop + 5-press freeze cycle. Sibling of :file:`wrist_calibrate.py` — that script handles ONE arm at a time (``--arm right`` or ``--arm left``). This one solves the bimanual problem: with ONE shared scene camera and TWO wrist cameras (one per arm), recover BOTH ``T_ee_cam`` extrinsics in a SINGLE calibration session, sharing the same teleop / button-press cadence the operator is already used to. Why a separate script (do NOT modify wrist_calibrate.py) -------------------------------------------------------- The single-arm version pins several decisions to one arm: * opens ``YAM_BASE_ONLY_CONFIG`` (arm 1, board IDs 217–225) only * spins up ``RobotController`` with one leader + one follower * opens one wrist camera * solves a single ``T_ee_cam`` and writes ``cal["cameras"][wrist_cam]`` The bimanual rig has to generalise all four. Crucially we want to preserve the EXACT same 5-press yellow-button cadence as the single-arm script — the operator already has muscle memory for it. So each press here affects BOTH arms together (one toggle per press, applied to both followers and both pose histories simultaneously). v2 could split the press per arm; leave as a TODO. Interactive flow (identical cadence to the single-arm script, but every press affects BOTH arms together): yellow#1 FREEZE 1: stop teleop on BOTH arms; clear BOTH histories; start accumulating aruco detections for BOTH arms' calibration solves. Save per-arm wrist+scene snapshots to ``/freeze1/arm{1,2}/``. yellow#2 UNFREEZE: resume teleop on BOTH arms. (Both arms' calibration data is locked in from freeze 1; no more accumulation past this point.) yellow#3 FREEZE 2: stop teleop on BOTH arms. Save SECOND per-arm snapshots to ``/freeze2/arm{1,2}/``. No new aruco accumulation — for cross-validation only. yellow#4 UNFREEZE: resume teleop on BOTH arms. yellow#5 END: solve EACH arm's T_ee_cam independently from its freeze-1 accumulation, save BOTH records to ONE combined JSON, route BOTH followers to INTERMEDIATE -> HOME. Per-arm math (run twice independently, once per arm): T_ee_cam_arm{i} = inv(FK(freeze1_q_arm{i})) @ link_to_aruco_transform(arm{i}_link_cfg) @ inv(T_cam_aruco_arm{i}) Each arm's solve uses ONLY its OWN wrist's accumulated correspondences and ONLY its OWN follower's freeze-1 joint state. The two arms never share calibration data — a single press just gives them a synchronised trigger to freeze + accumulate. Per-arm histories vs shared --------------------------- The pose accumulation is PER ARM (two independent ``_PoseHistory`` objects). Each wrist camera only sees its OWN arm's base ArUco board in the normal rig geometry — the cameras face inward toward their own arm's grasp area, and the boards sit at the back of each arm's base. Even if a wrist accidentally catches a glimpse of the OTHER arm's board, we ignore those correspondences: a wrist's only useful calibration target is its OWN arm's board, because the math (FK from this arm's follower joint state + this arm's link_to_aruco transform) only closes when both halves of the chain belong to the same arm. Cross-board sighting fall-through --------------------------------- We do ONE ``cv2.aruco.detectMarkers`` call per camera per frame and ``_partition_corners`` on the result. The right wrist's solve consumes ONLY the arm-1 partition (IDs 217–225); the left wrist's solve consumes ONLY the arm-2 partition (IDs 226–234). If a wrist sees the WRONG arm's board, those markers are silently dropped (the partition matching that wrist's target is empty -> no detection that frame). This is the right semantic — calibrating a wrist against the opposing arm's board would solve for ``T_otherArmBase_cam`` which is geometrically meaningful but USELESS to raiden's downstream code that expects ``T_ee_cam`` in the wrist's OWN arm's grasp_site frame. Combined JSON output -------------------- Single file (default ``~/.config/raiden/wrist_calibration_result_bimanual.json``) with the schema: { "version": "1.0", "timestamp": "...", "cameras": { "right_wrist_camera": { ...per-arm record... }, "left_wrist_camera": { ...per-arm record... } } } Each per-camera record matches the single-arm schema produced by ``_save_calibration_result`` in wrist_calibrate.py (type, method, arm, resolution, intrinsics, num_frames_used, mean_reproj_residual_px, capture_joint_state, extrinsics). If one arm fails to accumulate any frames, only the successful arm's record is written. (Compare with exo_calibrate_bimanual.py which uses ``__arm1`` / ``__arm2`` suffixes on a SHARED camera key — there the same physical camera produces two records; here the two records belong to two DIFFERENT physical cameras, so we just key by the canonical wrist camera name.) Per-arm INTERMEDIATE -> HOME ---------------------------- Both arms move home at end-of-session. We reuse the same intermediate pose for both arms as the single-arm script does (it's a symmetric "arm bent inward toward chest" pose that works for either side). The two smooth-moves are sequenced (arm 1 first, then arm 2) to keep the movement readable — running them concurrently would save ~5 s but risks collision if the operator left the arms in an awkward intermediate state. Usage: python ~/cameron/raiden_fork/raiden/calibration/wrist_calibrate_bimanual.py python -m raiden.calibration.wrist_calibrate_bimanual --vis_port 9093 """ import os os.environ.setdefault("MUJOCO_GL", "egl") os.environ.setdefault("PYOPENGL_PLATFORM", "egl") import json import sys import threading import time from dataclasses import dataclass, field from datetime import datetime from pathlib import Path from typing import List, Optional, Tuple import cv2 import numpy as np # Self-locate exo_redo the same way the single-arm wrist calibrate does. # This MUST run before the raiden.calibration.exo_calibrate import below # because that module pulls in third_party/exo_redo helpers (ExoConfigs, # exo_utils) that aren't on sys.path otherwise. _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 via import — DO NOT duplicate. Anything we'd otherwise # re-implement (ZED open, JPEG-encode-for-rerun, multi-frame pose # pooling, _PoseHistory) comes straight from the single-arm scene # calibrate module. from raiden.calibration.exo_calibrate_fidex import ( _scene_serial_from_config, _open_zed_native, _grab_bgr, _init_rerun, _draw_aruco_plane, _to_rerun_jpeg, _PoseHistory, _solve_multiframe_pose, ) _DEFAULT_OUT_DIR = Path.home() / ".config" / "raiden" _VALIDATION_OUT_BASE = Path("/home/robot-lab/cameron/wrist_validation_bimanual") DOF = 7 # 6 arm joints + 1 gripper # Same intermediate / home pose for both arms (symmetric "fold inward" # waypoint). Lifted verbatim from wrist_calibrate.py — keep these in # sync if the single-arm 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) # ArUco ID partitioning. These ranges are baked into the printed boards # (see ExoConfigs/yam_exo.py); duplicated verbatim from # exo_calibrate_bimanual.py rather than imported because that module # imports mujoco at import time and we don't want to drag mujoco into # the wrist calibrate process unnecessarily. _ARM1_IDS = set(range(217, 217 + 9)) # larger_coarse_board (arm 1, right) _ARM2_IDS = set(range(226, 226 + 9)) # larger_coarse_board_arm2 (arm 2, left) def _partition_corners(corners_all, ids_all): """Split a single ``cv2.aruco.detectMarkers`` result into per-arm ``(corners, ids, rejected)`` tuples consumable by ``do_est_aruco_pose`` via its ``corners_est=`` kwarg. 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 in this frame. Lifted verbatim (modulo this docstring) from exo_calibrate_bimanual.py — see the long rationale comment there. """ 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 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) @dataclass class _ArmSlot: """Per-arm slot inside the bimanual loop state. Each wrist calibration is genuinely independent (own camera, own history, own freeze joint state, own solve), so we keep ALL per-arm fields under this nested struct rather than naming them ``..._arm1`` / ``..._arm2`` at the top level. The shared 5-press control flags (frozen / accumulate / status_text) live on the parent ``_LoopState`` because every press affects both slots together. """ arm_label: str # "right" or "left" history: _PoseHistory # PnP correspondences pool last_detection: Optional[dict] = None # last successful per-frame detect # Populated by the daemon on every successful detect; the main thread # reads only at freeze time, so a plain assign is race-safe enough # (numpy 4x4 array assignment is atomic at the python-level). last_T_cam_aruco: Optional[np.ndarray] = None last_resid_px: float = float("nan") @dataclass class _LoopState: """Shared 5-press control + per-arm slots. The control flags (``stop``, ``frozen``, ``accumulate``, ``status_text``) are shared across both arms because one yellow-button press triggers a synchronised transition for both arms — that's the v1 lock policy. The per-arm data (history + last_detection) lives in the two ``_ArmSlot`` instances. """ stop: bool = False frozen: bool = False # both arms held (informational) accumulate: bool = False # both arms pooling aruco detections status_text: str = "TELEOP" arm1: Optional[_ArmSlot] = None arm2: Optional[_ArmSlot] = None # --------------------------------------------------------------------------- # FK + smooth move (per-arm wrappers around the single-arm helpers) # --------------------------------------------------------------------------- def _fk_base_to_ee(joint_state_7: np.ndarray) -> np.ndarray: """Compute ``T_base_ee`` from a YAM follower's 7-vector joint state. Lifted from wrist_calibrate.py — the kinematics object is cached at module level to amortise the mujoco model load. We share the SAME kinematics model for both arms because both YAM arms have identical morphology (the bimanual variant differs only in world placement, which is irrelevant to per-arm FK in the arm's OWN base frame). """ 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: np.ndarray, max_vel_rad_s: float, min_move_s: float = 0.5, steps: int = 100) -> float: """Smooth-move one follower to a 7-vector joint target. Identical to the single-arm version — kept here so callers don't have to import from wrist_calibrate.py (we'd rather not have a cross-module helper dependency given the hard "don't modify wrist_calibrate.py" rule). """ 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_move_s, delta / max(max_vel_rad_s, 1e-6)) print(f" smooth_move dmax={delta:.3f} rad -> {t_s:.2f}s") smooth_move_joints(robot, target, time_interval_s=t_s, steps=steps) return t_s # --------------------------------------------------------------------------- # Daemon capture loop (bimanual) # --------------------------------------------------------------------------- def _process_one_wrist( wrist_bgr, K_w, dist_w, arm_corners_est, aruco_board, board_length, state_accumulate: bool, slot: _ArmSlot, ): """Per-wrist per-frame: PnP solve + (optionally) pool the correspondences into the slot's history. Mirrors the single-arm capture loop's wrist block, factored into a helper so we don't duplicate it twice in the bimanual capture loop body. Returns the annotated BGR frame and a boolean ``wrist_seen``. """ from exo_utils import do_est_aruco_pose, ARUCO_DICT wrist_ann = wrist_bgr.copy() wrist_seen = False if arm_corners_est is None: return wrist_ann, wrist_seen try: rw = do_est_aruco_pose( wrist_bgr, ARUCO_DICT, aruco_board, board_length, cameraMatrix=K_w, distCoeffs=dist_w, corners_est=arm_corners_est, ) except Exception: return wrist_ann, wrist_seen if rw == -1: return wrist_ann, wrist_seen wrist_ann = rw["pose_vis"] _draw_aruco_plane(wrist_ann, rw["est_aruco_pose"], board_length, K_w) wrist_seen = True # SAME center-shift undo as the single-arm version. ``do_est_aruco_pose`` # returns a tvec that's been shifted to the board CENTER for rendering # convenience; the multi-frame pooled solver wants corner-origin # correspondences, so we shift back. See wrist_calibrate.py lines # 149-161 for the rationale (it's the same math). obj_cam, img_pts = rw["obj_img_pts"] rvec, tvec_pre = rw["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_pre - R_mat.dot(center_offset_board) obj_board = (R_mat.T @ (obj_cam.T - tvec_corner.reshape(3, 1))).T proj, _ = cv2.projectPoints( obj_cam.astype(np.float32), rvec, tvec_pre, K_w, dist_w ) resid_px = float(np.linalg.norm( proj.reshape(-1, 2) - img_pts.reshape(-1, 2), axis=1 ).mean()) T_cam_aruco = rw["est_aruco_pose"] # Only accumulate during freeze #1 (parent state.accumulate). Same # rationale as the single-arm script: keeps the multi-frame pool # consistent with a single FK at solve time. Freeze #2's detections # are NOT pooled (it's a separate cross-validation snapshot, not # part of the calibration data). if state_accumulate: slot.history.add( obj_pts=obj_board.astype(np.float32), img_pts=img_pts.astype(np.float32), residual_px=resid_px, single_frame_t=T_cam_aruco[:3, 3], ) slot.last_detection = { "T_cam_aruco": T_cam_aruco, "resid_px": resid_px, "n_markers": int(len(obj_cam) // 4), } slot.last_T_cam_aruco = T_cam_aruco slot.last_resid_px = resid_px return wrist_ann, wrist_seen def _capture_loop( scene_cam, wrist_cam_arm1, wrist_cam_arm2, K_s, dist_s, K_w_arm1, dist_w_arm1, K_w_arm2, dist_w_arm2, exo_cfg_arm1, exo_cfg_arm2, rr, state: _LoopState, viz_width: int, jpeg_quality: int, viz_hz: int, ) -> None: """Daemon: each frame, grab scene + BOTH wrist frames, detect aruco on each, run per-arm PnP, accumulate to per-arm history ONLY during FREEZE 1. Log annotated frames + per-arm status to rerun. Three independent ``detectMarkers`` calls per frame (one per camera). We do NOT share detection across cameras — they have different intrinsics, different vantage points, and different lighting; running a single detect across the union would be wrong. The shared machinery is the ID partitioner and the per-frame PnP helper. """ from exo_utils import ARUCO_DICT # noqa: F401 — exo_utils side-effects # Resolve per-arm config once. arm 1 uses ``larger_coarse_board`` (IDs # 217-225); arm 2 uses ``larger_coarse_board_arm2`` (IDs 226-234). 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 log_interval = 1.0 / max(viz_hz, 1) last_log = 0.0 frame_idx = 0 aruco_params = cv2.aruco.DetectorParameters() 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 # ── Scene camera: detect ALL markers, draw BOTH per-arm overlays ── # The scene camera sees both boards; we annotate it with both # for visual confirmation. Per-arm PnP on the scene image is not # needed for wrist calibration (we only solve wrist↔base), so # we only draw the board planes for diagnostic purposes. scene_ann = scene_bgr.copy() try: gray_s = cv2.cvtColor(scene_bgr, cv2.COLOR_BGR2GRAY) corners_s, ids_s, _ = cv2.aruco.detectMarkers( gray_s, ARUCO_DICT, parameters=aruco_params) arm1_est_s, arm2_est_s = _partition_corners(corners_s, ids_s) if ids_s is not None: cv2.aruco.drawDetectedMarkers(scene_ann, corners_s, ids_s) # Per-arm PnP on the scene image just for the overlay outline. # Failure is fine; we keep the markers drawn either way. from exo_utils import do_est_aruco_pose if arm1_est_s is not None: try: rs1 = 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 rs1 != -1: _draw_aruco_plane( scene_ann, rs1["est_aruco_pose"], board_length_arm1, K_s, color=(0, 255, 255)) # yellow except Exception: pass if arm2_est_s is not None: try: rs2 = 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 rs2 != -1: _draw_aruco_plane( scene_ann, rs2["est_aruco_pose"], board_length_arm2, K_s, color=(255, 0, 255)) # magenta except Exception: pass except Exception: pass # ── Arm 1 wrist (right) ───────────────────────────────────────── # Detect on THIS camera's frame, take ONLY the arm-1 partition, # solve & (optionally) accumulate. Any arm-2 marker accidentally # caught is silently dropped — see module docstring on # cross-board sighting fall-through. try: gray_w1 = cv2.cvtColor(wrist1_bgr, cv2.COLOR_BGR2GRAY) corners_w1, ids_w1, _ = cv2.aruco.detectMarkers( gray_w1, ARUCO_DICT, parameters=aruco_params) arm1_est_w1, _arm2_est_w1 = _partition_corners(corners_w1, ids_w1) except Exception: arm1_est_w1 = None wrist1_ann, wrist1_seen = _process_one_wrist( wrist1_bgr, K_w_arm1, dist_w_arm1, arm1_est_w1, aruco_board_arm1, board_length_arm1, state_accumulate=state.accumulate, slot=state.arm1, ) # ── Arm 2 wrist (left) ────────────────────────────────────────── try: gray_w2 = cv2.cvtColor(wrist2_bgr, cv2.COLOR_BGR2GRAY) corners_w2, ids_w2, _ = cv2.aruco.detectMarkers( gray_w2, ARUCO_DICT, parameters=aruco_params) _arm1_est_w2, arm2_est_w2 = _partition_corners(corners_w2, ids_w2) except Exception: arm2_est_w2 = None wrist2_ann, wrist2_seen = _process_one_wrist( wrist2_bgr, K_w_arm2, dist_w_arm2, arm2_est_w2, aruco_board_arm2, board_length_arm2, state_accumulate=state.accumulate, slot=state.arm2, ) 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("wrist_arm1/rgb_aruco", _to_rerun_jpeg(wrist1_ann, target_w=viz_width, quality=jpeg_quality)) rr.log("wrist_arm2/rgb_aruco", _to_rerun_jpeg(wrist2_ann, target_w=viz_width, quality=jpeg_quality)) seen1 = "SEEN" if wrist1_seen else "not seen" seen2 = "SEEN" if wrist2_seen else "not seen" hist1 = state.arm1.history.stats() if state.arm1 is not None else "" hist2 = state.arm2.history.stats() if state.arm2 is not None else "" rr.log("info", _rr.TextDocument( f"[{state.status_text}]\n" f" arm1 (right) wrist board: {seen1} | {hist1}\n" f" arm2 (left) wrist board: {seen2} | {hist2}" )) frame_idx += 1 # --------------------------------------------------------------------------- # Per-arm result save + combined JSON writer # --------------------------------------------------------------------------- def _build_camera_record( arm: str, K: np.ndarray, dist: np.ndarray, T_ee_cam: np.ndarray, resolution: str, n_used: int, mean_resid_px: float, joint_state: List[float], ) -> dict: """Build ONE per-camera record matching the single-arm JSON schema (``_save_calibration_result`` in wrist_calibrate.py, lines 212–230). Kept in lockstep with that helper — if you add fields there, mirror them here. """ return { "type": "wrist_one_shot", "method": "exo_board_pnp_at_known_joint_state", "arm": arm, "resolution": resolution, "intrinsics": { "camera_matrix": K.tolist(), "distortion_coeffs": [float(v) for v in np.asarray(dist).reshape(-1)[:5]], # image_size lets LiveVisualizer's point-cloud unprojection # scale K to the streamed resolution. Without it the default # (1280, 720) is used and points project to wrong locations. "image_size": ({"HD720": [1280, 720], "HD1080": [1920, 1080], "HD2K": [2208, 1242]}.get(resolution, [1920, 1080])), }, "num_frames_used": int(n_used), "mean_reproj_residual_px": float(mean_resid_px), "capture_joint_state": [float(x) for x in joint_state], "extrinsics": { "success": True, "rotation_matrix": T_ee_cam[:3, :3].tolist(), "translation_vector": T_ee_cam[:3, 3].tolist(), "reference_frame": f"{arm}_grasp_site", }, } def _save_bimanual_calibration_result( output_file: Path, records: List[Tuple[str, dict]], ) -> None: """Merge per-arm camera records into ONE combined JSON. ``records`` is a list of ``(wrist_cam_name, record_dict)`` tuples, one per arm that successfully solved. We DON'T modify any single-arm helper here; instead we build the camera records via ``_build_camera_record`` (which mirrors the single-arm schema) and merge them all under a single ``cameras`` dict. If the output file already exists we preserve any pre-existing ``cameras`` keys that we aren't overwriting (same idempotent behavior the single-arm helper has). The two wrist camera records just become two new entries. """ cal: dict = {} if output_file.exists(): try: with open(output_file) as f: cal = json.load(f) except Exception: # Corrupt previous run — start fresh. cal = {} cal.setdefault("version", "1.0") cal["timestamp"] = datetime.now().isoformat(timespec="seconds") cal.setdefault("cameras", {}) for cam_name, rec in records: cal["cameras"][cam_name] = rec output_file.parent.mkdir(parents=True, exist_ok=True) with open(output_file, "w") as f: json.dump(cal, f, indent=2) print(f"\n Saved BIMANUAL wrist calibration -> {output_file}") # Also merge the same records into the canonical raiden calibration # file (~/.config/raiden/calibration_results.json) so `rd teleop # --visualize` (LiveVisualizer in raiden/visualizer.py) picks up the # bimanual wrist extrinsics automatically. The visualizer reads the # field name "hand_eye_calibration" (not our "extrinsics" key), so # we rename the inner block during the copy. All other fields are # carried over unchanged — same shape, different key. try: from raiden._config import CALIBRATION_FILE as _CAL_FILE _cal_path = Path(_CAL_FILE) _cal = {} if _cal_path.exists(): try: with open(_cal_path) as f: _cal = json.load(f) except Exception: _cal = {} _cal.setdefault("version", "1.0") _cal["timestamp"] = datetime.now().isoformat(timespec="seconds") _cal.setdefault("cameras", {}) for cam_name, rec in records: _rec_viz = {k: v for k, v in rec.items() if k != "extrinsics"} _rec_viz["hand_eye_calibration"] = rec["extrinsics"] _cal["cameras"][cam_name] = _rec_viz _cal_path.parent.mkdir(parents=True, exist_ok=True) with open(_cal_path, "w") as f: json.dump(_cal, f, indent=2) print(f" Also merged into raiden CALIBRATION_FILE -> {_cal_path}") print(f" (renamed 'extrinsics' -> 'hand_eye_calibration' for " f"LiveVisualizer compatibility)") except Exception as _exc: print(f" ⚠ could not merge into CALIBRATION_FILE: {_exc}") for cam_name, rec in records: t = rec["extrinsics"]["translation_vector"] print(f" [{cam_name}] arm={rec['arm']} cam_in_ee xyz = {t}") print(f" reference_frame = {rec['extrinsics']['reference_frame']}") print(f" n_frames_used = {rec['num_frames_used']}, " f"mean_resid = {rec['mean_reproj_residual_px']:.2f} px") # --------------------------------------------------------------------------- # Per-arm freeze snapshot writer # --------------------------------------------------------------------------- def _save_freeze_snapshot_arm( out_dir: Path, scene_bgr: np.ndarray, wrist_bgr: np.ndarray, achieved_q: List[float], arm: str, scene_cam_name: str, scene_serial: int, K_s: np.ndarray, dist_s: np.ndarray, scene_res: str, wrist_cam_name: str, wrist_serial: int, K_w: np.ndarray, dist_w: np.ndarray, wrist_res: str, ): """Write one arm's freeze snapshot dir: ``/{wrist.png, scene.png, state.json}``. Same format as the single-arm ``_save_freeze_snapshot`` in wrist_calibrate.py — caller arranges the per-arm subdirectory layout (``/freeze1/arm1/``, ``/freeze1/arm2/``, ...). The scene PNG is identical across both arms (same physical camera), but we save it twice (once under each arm's freeze dir) so each arm has a self-contained snapshot directory for offline analysis. The duplicate is small (~1 MB at HD1080) and saves the analyst from having to cross-reference dirs. """ out_dir.mkdir(parents=True, exist_ok=True) cv2.imwrite(str(out_dir / "scene.png"), scene_bgr) cv2.imwrite(str(out_dir / "wrist.png"), wrist_bgr) state_doc = { "arm": arm, "joint_state_achieved": achieved_q, "scene_camera": { "name": scene_cam_name, "serial": scene_serial, "K": K_s.tolist(), "dist": dist_s.tolist(), "resolution": scene_res, }, "wrist_camera": { "name": wrist_cam_name, "serial": wrist_serial, "K": K_w.tolist(), "dist": dist_w.tolist(), "resolution": wrist_res, }, "timestamp_utc": datetime.utcnow().isoformat() + "Z", } with open(out_dir / "state.json", "w") as f: json.dump(state_doc, f, indent=2) print(f" Saved freeze snapshot ({arm}) -> {out_dir}") # --------------------------------------------------------------------------- # Main orchestration # --------------------------------------------------------------------------- def run_wrist_calibrate_bimanual( scene_cam_name: str, right_wrist_cam_name: str, left_wrist_cam_name: str, camera_config_file: str, output_file: str, vis_port: int, scene_resolution: str, wrist_resolution: str, viz_width: int, jpeg_quality: int, viz_hz: int, max_joint_vel_rad_s: float, history_size: int, top_k: int, teleop: bool = True, ): """End-to-end bimanual wrist calibration. Heavy raiden imports (RobotController, i2rt.robots.kinematics) are deferred until inside this function, matching the single-arm version's lazy-import policy — keeps the module import cheap for tools that just want the helpers (e.g. the freeze-snapshot writer from an offline analysis notebook). """ 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) ──────────────── # The bimanual rig at russet uses the new mirror-flipped base board for # arm 2 (yam_base_board_v2_reverse_clearance_fixed.stl). The board's # ArUco offset along x is the NEGATIVE of arm-1's, and the mesh path # points at the reversed STL. These two edits keep `link_to_aruco_transform` # and any mujoco overlay rendering consistent with 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 ExoConfigs.exoskeleton import link_to_aruco_transform # --- Cameras (1 scene + 2 wrists) scene_serial = _scene_serial_from_config(camera_config_file, scene_cam_name) wrist1_serial = _scene_serial_from_config(camera_config_file, right_wrist_cam_name) wrist2_serial = _scene_serial_from_config(camera_config_file, left_wrist_cam_name) print(f"Opening scene camera ({scene_cam_name}, 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 ({right_wrist_cam_name}, serial={wrist1_serial}) at {wrist_resolution}...") wrist_cam_arm1, K_w_arm1, dist_w_arm1, W_w1, H_w1 = _open_zed_native( wrist1_serial, wrist_resolution) print(f"Opening left-wrist camera ({left_wrist_cam_name}, serial={wrist2_serial}) at {wrist_resolution}...") wrist_cam_arm2, K_w_arm2, dist_w_arm2, W_w2, H_w2 = _open_zed_native( wrist2_serial, wrist_resolution) _init_rerun(vis_port) # --- Robot (both leaders + both followers). Optional on russet: # the rig may not have leader CANs wired during a port-verification run, # so we wrap init in try/except and fall through to a viewer-only demo # when teleop isn't available. Same pattern as the scene-cam script. rc = None follower_arm1 = None follower_arm2 = None if teleop: try: print("\nInitialising BIMANUAL robot (both leaders + both followers)...") 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 follower drives arm 1 follower_arm2 = rc.follower_l # left follower drives arm 2 except Exception as _e: print(f" ⚠ Teleop init failed ({_e}); continuing without it. " "Use --no-teleop to suppress this message.") rc = None follower_arm1 = None follower_arm2 = None else: print("\n[--no-teleop] skipping RobotController init — viewer-only demo") # --- Per-arm state slots state = _LoopState( arm1=_ArmSlot(arm_label="right", history=_PoseHistory(max_size=history_size)), arm2=_ArmSlot(arm_label="left", history=_PoseHistory(max_size=history_size)), status_text="TELEOP — drive both leaders until each wrist sees its board", ) th = threading.Thread( target=_capture_loop, daemon=True, args=(scene_cam, wrist_cam_arm1, wrist_cam_arm2, K_s, dist_s, K_w_arm1, dist_w_arm1, K_w_arm2, dist_w_arm2, YAM_BASE_ONLY_CONFIG, YAM_BASE_ONLY_CONFIG_ARM2, rr, state, viz_width, jpeg_quality, viz_hz), ) th.start() if rc is None: print() print("=" * 72) print(" [no-teleop] viewer demo: cameras + ArUco overlay live for 30 s") print(" Verify in rerun that scene + both wrists detect their boards,") print(" then this script will exit cleanly. No calibration is solved.") print("=" * 72) try: import time as _time for _i in range(30): _time.sleep(1) except KeyboardInterrupt: print("\n (ctrl-c)") state.stop = True _time.sleep(0.3) for _cam in (scene_cam, wrist_cam_arm1, wrist_cam_arm2): try: _cam.close() except Exception: pass return print() print("=" * 72) print(" BIMANUAL wrist calibration — every yellow press affects BOTH arms.") print(" Teleop both leaders until BOTH wrists clearly see their boards.") print(" Yellow #1 -> FREEZE 1 (calibrate). Accumulate for both arms.") print(" Yellow #2 -> UNFREEZE. Drive to a SECOND aruco-visible pose.") print(" Yellow #3 -> FREEZE 2 (verify). Per-arm snapshots only.") print(" Yellow #4 -> UNFREEZE. Drive somewhere safe to end.") print(" Yellow #5 -> END (solve each arm, save combined JSON, home).") print("=" * 72) print() press_count = 0 freeze1_q_arm1: Optional[np.ndarray] = None freeze1_q_arm2: Optional[np.ndarray] = None freeze2_q_arm1: Optional[np.ndarray] = None freeze2_q_arm2: Optional[np.ndarray] = None session_ts = datetime.now().strftime("%Y%m%d_%H%M%S") session_dir = _VALIDATION_OUT_BASE / f"wrist_calib_bimanual_{session_ts}" def _capture_freeze_bimanual(label: str) -> Tuple[ Optional[np.ndarray], Optional[np.ndarray]]: """Grab synchronized scene + BOTH wrist frames + BOTH follower joint states into ``/