"""One-shot rerun panels for the record_testing.py auto-calibration. After auto-cal + downshift, this module logs ONE calibration-overlay panel per scene camera to a rerun viewer. Each panel is composited on top of the live camera frame + rendered against the LIVE robot joint state (i.e. wherever the arms happen to be at the moment of logging). Not called during normal recording — this is a diagnostic that fires once, right before the first episode's READY banner, so the operator can eyeball calibration alignment before pressing go. """ from __future__ import annotations import os # mujoco needs EGL on russet (headless). Set BEFORE the mujoco import # below or ``mujoco.Renderer(...)`` raises ``GLFW not supported``. os.environ.setdefault("MUJOCO_GL", "egl") os.environ.setdefault("PYOPENGL_PLATFORM", "egl") import copy import json import sys from pathlib import Path from typing import Optional import cv2 import numpy as np # exo_redo carries ExoConfigs + exo_utils + Demos.sim_yam_bimanual. _EXO_REDO = Path("/home/robot-lab/raiden_internal/third_party/exo_redo") if str(_EXO_REDO) not in sys.path: sys.path.insert(0, str(_EXO_REDO)) # One shared MuJoCo renderer per (W,H); avoids the EGL init cost per cam. # NOTE: renderers are EGL-context-thread-bound. The initial one-shot log # creates its renderer in the main thread; the live daemon thread creates # its own (see _live_overlay_loop). Do NOT share renderers across threads. _CACHED_RENDERER = None _CACHED_RENDER_SHAPE: tuple[int, int] | None = None # Flag so the initial one-shot panel only fires once per record session. _PANELS_LOGGED = [False] # Live-overlay daemon thread singleton. _LIVE_THREAD = [None] def _to_rerun_jpeg(img_bgr: np.ndarray, quality: int = 75): """JPEG-compress a BGR image for rerun logging. Raw RGB at 720p is ~2.7 MB/frame; at 2 Hz x 2 cams that saturates rerun's grpc channel when no viewer is draining it, and the SDK's exit-flush then blocks the whole process shutdown (observed hang, 2026-07-05). JPEG at q75 is ~100-200 KB — two orders of magnitude less. Same approach as exo_calibrate_fidex._to_rerun_jpeg. """ import rerun as rr ok, enc = cv2.imencode(".jpg", img_bgr, [cv2.IMWRITE_JPEG_QUALITY, quality]) if not ok: return rr.Image(cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB)) return rr.EncodedImage(contents=enc.tobytes(), media_type="image/jpeg") def _compose_overlay(rgb_bgr: np.ndarray, rendered_rgb: np.ndarray, seg_mask: Optional[np.ndarray] = None, alpha: float = 0.7) -> np.ndarray: """Alpha-blend the mujoco render over the camera frame. Copied verbatim from raiden_internal/raiden/calibration/exo_calibrate_fidex.py:187. If seg_mask is given (mujoco segmentation render, geom_id channel = -1 for background), use it for the foreground mask so BLACK pixels inside foreground geometry (aruco marker squares) stay opaque. """ rgb_frame_rgb = cv2.cvtColor(rgb_bgr, cv2.COLOR_BGR2RGB).astype(np.float32) rendered_f = rendered_rgb.astype(np.float32) if seg_mask is not None: fg = (seg_mask[:, :, 0] != -1).astype(np.float32)[..., None] else: fg = (rendered_f.sum(axis=-1) >= 30).astype(np.float32)[..., None] composite = (fg * (alpha * rendered_f + (1 - alpha) * rgb_frame_rgb) + (1 - fg) * rgb_frame_rgb) return composite.clip(0, 255).astype(np.uint8) def _render_with_intrinsics(model, data, T_link_in_cam, K, W, H, renderer=None): """Render the mujoco scene at the detected camera pose for overlay on a live camera frame. Handles off-center principal points via a post-render pixel shift (mujoco's cam_fovy renderer assumes cx=W/2, cy=H/2). Copied verbatim from exo_calibrate_fidex.py:208. Returns (rgb, seg) — both shifted into the live-frame pixel grid. """ import mujoco # lazy — MUJOCO_GL must be set before first import cam_id = model.cam('estimated_camera').id data.cam_xpos[cam_id] = np.linalg.inv(T_link_in_cam)[:3, 3] flip = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) data.cam_xmat[cam_id] = (flip @ T_link_in_cam[:3, :3]).T.reshape(-1) model.cam_fovy[cam_id] = float(np.degrees(2 * np.arctan(H / (2 * K[1, 1])))) global _CACHED_RENDERER, _CACHED_RENDER_SHAPE if renderer is not None: # Caller-owned renderer (used by the live daemon thread, which has # its own EGL context). r = renderer else: if _CACHED_RENDER_SHAPE != (W, H) or _CACHED_RENDERER is None: _CACHED_RENDERER = mujoco.Renderer(model, height=H, width=W) _CACHED_RENDER_SHAPE = (W, H) r = _CACHED_RENDERER r.disable_segmentation_rendering() r.update_scene(data, camera=cam_id) rgb = r.render() r.enable_segmentation_rendering() r.update_scene(data, camera=cam_id) seg = r.render() r.disable_segmentation_rendering() dx = int(round(K[0, 2] - W / 2)) dy = int(round(K[1, 2] - H / 2)) if dx or dy: M = np.float32([[1, 0, dx], [0, 1, dy]]) rgb = cv2.warpAffine(rgb, M, (W, H), flags=cv2.INTER_NEAREST, borderValue=(0, 0, 0)) seg_geom = seg[:, :, 0].astype(np.int32) seg_geom_shifted = cv2.warpAffine( seg_geom, M, (W, H), flags=cv2.INTER_NEAREST, borderValue=-1) seg = np.stack([seg_geom_shifted, seg[:, :, 1]], axis=-1) return rgb, seg def _build_bimanual_scene(margin: float, patch_russet_v2_reverse: bool = True): """Build a bimanual mujoco scene ready for `_render_with_intrinsics`. Returns (model, data, exo_cfg_arm1, exo_cfg_arm2, arm2_body_id). """ import mujoco from ExoConfigs.yam_exo import ( YAM_BASE_ONLY_CONFIG, YAM_BASE_ONLY_CONFIG_ARM2) from exo_utils import ( position_exoskeleton_meshes, get_link_poses_from_robot) from Demos.sim_yam_bimanual import build_bimanual_xml from raiden.calibration.exo_auto import apply_russet_arm2_v2_reverse_patch exo_arm1 = YAM_BASE_ONLY_CONFIG exo_arm2 = copy.deepcopy(YAM_BASE_ONLY_CONFIG_ARM2) if patch_russet_v2_reverse: apply_russet_arm2_v2_reverse_patch(exo_arm2, verbose=False) for link_cfg in exo_arm2.links.values(): link_cfg.pybullet_name = "arm2_arm" # exo_redo's XML uses relative texture/mesh paths; chdir so they resolve. prev_cwd = os.getcwd() os.chdir(str(_EXO_REDO)) try: xml = build_bimanual_xml(margin=margin, include_background=False) finally: os.chdir(prev_cwd) model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) mujoco.mj_forward(model, data) position_exoskeleton_meshes( exo_arm1, model, data, get_link_poses_from_robot(exo_arm1, model, data)) position_exoskeleton_meshes( exo_arm2, model, data, get_link_poses_from_robot(exo_arm2, model, data)) arm2_body_id = model.body("arm2_arm").id return model, data, exo_arm1, exo_arm2, arm2_body_id def _discover_arm_qpos(model, prefix: str = ""): """Locate (arm_qpos_idx, grip_qpos_idx) for one arm. Copied from exo_calibrate_bimanual_fidex._discover_arm_qpos:827.""" import mujoco 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): """Push live follower joints into mujoco qpos. Copied from exo_calibrate_bimanual_fidex._drive_arm_qpos:801.""" 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: pass _KIN_CACHE = None def _fk_base_to_ee(joint_state_7: np.ndarray) -> np.ndarray: """T_base_ee from a YAM follower's 7-vec joint state (same helper as the wrist calibrate/verify scripts — grasp_site FK on the 4310 XML).""" global _KIN_CACHE if _KIN_CACHE is None: from i2rt.robots.kinematics import Kinematics from raiden._xml_paths import get_yam_4310_linear_xml_path _KIN_CACHE = Kinematics(get_yam_4310_linear_xml_path(), site_name="grasp_site") q = np.zeros(8, dtype=np.float64) q[:6] = np.asarray(joint_state_7, dtype=np.float64)[:6] return np.asarray(_KIN_CACHE.fk(q), dtype=np.float64) def _load_wrist_hand_eye(path: Path) -> dict: """Read the bimanual wrist calibration JSON → {cam_name: (arm_label, T_ee_cam 4x4)}. Missing file / bad records return {} (wrist panels simply don't come up).""" out = {} try: w = json.loads(Path(path).read_text()) for name, rec in w.get("cameras", {}).items(): ext = rec.get("extrinsics", {}) R = np.asarray(ext.get("rotation_matrix"), dtype=np.float64) t = np.asarray(ext.get("translation_vector"), dtype=np.float64).reshape(3) if R.shape != (3, 3): continue T = np.eye(4) T[:3, :3] = R T[:3, 3] = t out[name] = (rec.get("arm", "right"), T) except Exception: pass return out def _init_rerun(port: int = 9877, public_host: Optional[str] = None) -> Optional[object]: """Init rerun; return rr or None on failure. Never raises. ``public_host`` should be the IP or hostname the OPERATOR reaches the box on (russet's tailscale IP, typically). It's embedded in the web viewer's ``connect_to`` URL — otherwise the browser tries ``127.0.0.1:`` which is the operator's own Mac, not russet. Auto-detects from the primary NIC if not passed. """ try: import rerun as rr import socket rr.init("record_testing_calibration") grpc_port = port + 1 # Auto-detect a reachable IP if not provided. host = public_host if not host: try: # UDP connect trick: doesn't actually send; picks the IP # that would be used to reach an external host. s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) s.connect(("8.8.8.8", 80)) host = s.getsockname()[0] s.close() except Exception: host = "127.0.0.1" try: rr.serve_grpc(grpc_port=grpc_port) except Exception: pass try: rr.serve_web_viewer( web_port=port, connect_to=f"rerun+http://{host}:{grpc_port}/proxy") print(f" [record_testing] rerun panels at " f"http://{host}:{port}") except Exception: pass return rr except Exception as e: print(f" [record_testing] rerun init failed ({e}); panels skipped") return None def _spawn_live_overlay_thread( per_cam_render_state: dict, model, data, arm2_body_id: int, arm1_qpos_idx, arm1_grip_qpos_idx, arm2_qpos_idx, arm2_grip_qpos_idx, robot_controller, rr, hz: float = 5.0, exo_cfg_arm2=None, ) -> None: """Spawn a daemon thread that keeps the rerun overlay panels live. Uses `retrieve_image` (NOT `grab()`) so the record loop's grab cycle stays unchallenged. Each iteration: - retrieve latest buffered frame per cam (no camera lock) - drive current follower joint state into mujoco qpos - render mujoco scene (stored extrinsic + fresh qpos) - composite over the live retrieved frame - log to rerun with an incrementing time step Runs at ``hz`` Hz until the process exits (thread is daemon). """ if _LIVE_THREAD[0] is not None: return import threading import time import mujoco import pyzed.sl as sl # Own sl.Mat per cam so we never contend with the record loop's Mat. mats = {name: sl.Mat() for name in per_cam_render_state} def _loop(): try: import cv2 as _cv2 import numpy as _np # Per-thread renderer (EGL context created in THIS thread). renderers = {} for name, s in per_cam_render_state.items(): renderers[name] = mujoco.Renderer( model, height=int(s["H"]), width=int(s["W"])) period = 1.0 / max(hz, 0.5) frame_idx = 0 print(f" [record_testing] live overlay daemon started " f"({hz:.1f} Hz over {list(per_cam_render_state)})") while True: t0 = time.monotonic() # 1) drive latest follower joints into mujoco qpos if robot_controller is not None: _drive_arm_qpos(getattr(robot_controller, "follower_r", None), data, arm1_qpos_idx, arm1_grip_qpos_idx) _drive_arm_qpos(getattr(robot_controller, "follower_l", None), data, arm2_qpos_idx, arm2_grip_qpos_idx) frame_idx += 1 # 2) per-cam: re-anchor arm2 from THIS cam's own solve, then # grab + retrieve + render + composite + log. The per-cam # anchor keeps every view self-consistent — rendering cam X # with cam Y's anchor would show X↔Y calibration disagreement # as a phantom arm-2 offset. # We call grab() ourselves so the ZED buffer is refreshed # while no episode is recording; during recording the SDK # serialises grab requests with the record loop. for name, s in per_cam_render_state.items(): # Wrist cams: recompute the extrinsic EVERY frame — # the camera rides the arm. if s.get("wrist"): f = s.get("follower") if f is None: continue try: q = np.asarray(f.get_joint_pos(), dtype=np.float64) T_base_ee = _fk_base_to_ee(q) T_cam_in_world = (s["T_arm_in_world"] @ T_base_ee @ s["T_ee_cam"]) s["T_world_in_cam"] = np.linalg.inv(T_cam_in_world) except Exception: continue if s.get("T_world_in_cam") is None: continue if s.get("arm2_anchor_pos") is not None: model.body_pos[arm2_body_id] = s["arm2_anchor_pos"] model.body_quat[arm2_body_id] = s["arm2_anchor_quat_wxyz"] mujoco.mj_forward(model, data) if (s.get("arm2_anchor_pos") is not None and exo_cfg_arm2 is not None): try: from exo_utils import ( position_exoskeleton_meshes, get_link_poses_from_robot) position_exoskeleton_meshes( exo_cfg_arm2, model, data, get_link_poses_from_robot( exo_cfg_arm2, model, data)) except Exception: pass cam = s["cam"] try: # Only grab when the camera is NOT recording an # episode. During recording the record loop grabs at # 30 fps (each grab also writes an SVO2 frame), so a # second grabber would perturb SVO frame timing vs # the default recorder. When idle (READY prompt), # nobody else grabs, so we must, or the buffer goes # stale and panels freeze. try: is_rec = cam._camera.get_recording_status().is_recording except Exception: is_rec = False if not is_rec: cam._camera.grab() # refresh buffer; ignore result cam._camera.retrieve_image(mats[name], sl.VIEW.LEFT) bgr = mats[name].get_data()[:, :, :3].copy() except Exception: continue W = bgr.shape[1]; H = bgr.shape[0] try: rendered_rgb, seg = _render_with_intrinsics( model, data, s["T_world_in_cam"], s["K"], W, H, renderer=renderers[name]) overlay = _compose_overlay(bgr, rendered_rgb, seg_mask=seg) overlay_bgr = _cv2.cvtColor(overlay, _cv2.COLOR_RGB2BGR) # Only the overlay is logged live to stay within # rerun's batcher throughput. Raw is available in # the initial one-shot log; toggle in viewer if you # want to see it side-by-side. rr.log(f"calibration/{name}/overlay", _to_rerun_jpeg(overlay_bgr)) except Exception as _err: # Log the error EVERY 20 frames so it's visible but # not spammy (5 Hz × 20 = 4 sec between error prints). if frame_idx % 20 == 0: print(f" [live overlay] render err '{name}': " f"{type(_err).__name__}: {_err}") dt = time.monotonic() - t0 if dt < period: time.sleep(period - dt) except Exception as exc: print(f" [record_testing] live overlay daemon died: " f"{type(exc).__name__}: {exc}") t = threading.Thread(target=_loop, daemon=True, name="cal-overlay-live") t.start() _LIVE_THREAD[0] = t def log_calibration_overlay_panels( cameras_list, camera_config_file: str, calibration_file: str, robot_controller=None, rerun_port: int = 9877, frame_pool_size: int = 5, ) -> bool: """Log one overlay panel per scene camera to rerun. For each scene cam: 1. Grab N live frames + pool per-arm ArUco detections (cheap re-verify — the extrinsic in calibration_file was already computed from a 15-frame pool during auto-cal, but pooling a few fresh frames here gives a live consistency check). 2. Read current follower joints, push into mujoco qpos for both arms. 3. Render mujoco scene with T_cam_in_arm1_base + current qpos. 4. Composite over the LATEST live frame from that cam. 5. Log to `calibration/{cam_name}/overlay`. Returns True if at least one panel was logged; False otherwise. Never raises. """ if _PANELS_LOGGED[0]: return True # already logged this session try: rr = _init_rerun(rerun_port) if rr is None: return False # Load the cal results we just wrote. cal = json.loads(Path(calibration_file).read_text()) cams = cal.get("cameras", {}) # Map cam objects to names via camera.json. with open(camera_config_file) as f: cfg = json.load(f) names = list(cfg.keys()) cams_by_name = dict(zip(names, cameras_list)) scene_names = [n for n, e in cfg.items() if isinstance(e, dict) and e.get("role") == "scene"] # Recover margin from bimanual_transform if available, else default. margin = 0.591 bt = cal.get("bimanual_transform", {}) t = bt.get("translation_m_right_base_in_left_base") if isinstance(t, list) and len(t) == 3: margin = float(abs(t[1])) # arm 2 sits at world y = +margin/2 # Build the mujoco scene once. Note: after downshift the auto-cal # extrinsics remain valid (extrinsic doesn't depend on K); we just # use each cam's CURRENT K/dist (HD720) for the render. from raiden.calibration.exo_auto import ( apply_russet_arm2_v2_reverse_patch) import mujoco model, data, exo_a1, exo_a2, arm2_body_id = _build_bimanual_scene( margin=margin, patch_russet_v2_reverse=True) # Discover per-arm qpos indices for driving current joints in. 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_") # Push current joint state into mujoco qpos (if follower alive). if robot_controller is not None: _drive_arm_qpos(getattr(robot_controller, "follower_r", None), data, arm1_qpos_idx, arm1_grip_qpos_idx) _drive_arm_qpos(getattr(robot_controller, "follower_l", None), data, arm2_qpos_idx, arm2_grip_qpos_idx) # World-frame transforms (arm 1 at y=-margin/2, arm 2 at y=+margin/2). T_arm1_in_world = np.eye(4); T_arm1_in_world[1, 3] = -margin / 2.0 T_arm2_in_world = np.eye(4); T_arm2_in_world[1, 3] = +margin / 2.0 # Cache per-cam render state so the live daemon thread can reuse # T_world_in_cam / K / cam handle without re-loading calibration # each iteration. per_cam_render_state: dict = {} # For arm-2 re-anchor: use both calibrated poses to fix arm2's # world position (matches exo_calibrate_bimanual_fidex.py). If # only one arm calibrated, skip re-anchoring. n_logged = 0 for scene_name in scene_names: if scene_name not in cams_by_name: continue arm1_key, arm2_key = f"{scene_name}__arm1", f"{scene_name}__arm2" T_cam_in_arm1 = None T_cam_in_arm2 = None if arm1_key in cams: T_cam_in_arm1 = np.asarray( cams[arm1_key]["extrinsics"]["T_cam_in_base"]) if arm2_key in cams: T_cam_in_arm2 = np.asarray( cams[arm2_key]["extrinsics"]["T_cam_in_base"]) if T_cam_in_arm1 is None and T_cam_in_arm2 is None: # Still log the raw frame so operator can see what this # scene cam is currently looking at (helps diagnose the # "why aren't the boards being detected" case). cam = cams_by_name[scene_name] bgr_raw = None for _ in range(frame_pool_size): if cam.grab(): fr = cam.get_frame() if fr is not None and fr.color is not None: bgr_raw = fr.color if bgr_raw is not None: rr.log(f"calibration/{scene_name}/raw", _to_rerun_jpeg(bgr_raw)) _dump_path = Path(f"/tmp/cal_raw_{scene_name}.png") cv2.imwrite(str(_dump_path), bgr_raw) rr.log(f"calibration/{scene_name}/info", rr.TextDocument( f"scene cam '{scene_name}' — NO CALIBRATION " f"(0 markers detected during auto-cal pool). " f"Raw frame dumped to {_dump_path} for " f"diagnostic (physical aim / occlusion " f"check).")) print(f" [record_testing] no cal for scene cam " f"'{scene_name}'; raw frame dumped to {_dump_path}") else: print(f" [record_testing] no cal for scene cam " f"'{scene_name}' — skipping panel") continue # Re-anchor arm2 body in world from THIS cam's per-arm delta. # IMPORTANT: the anchor is per-camera. Each cam's PnP pair # implies its own T_arm2_in_arm1; rendering cam X's overlay # with cam Y's anchor bakes the X↔Y disagreement into the # overlay as a visible arm-2 offset (bug observed 2026-07-05: # ego's anchor, applied last, shifted arm 2 in the scene # view). We save the anchor into per_cam_render_state below # and the live daemon re-applies it before rendering each cam. arm2_anchor_pos = None arm2_anchor_quat_wxyz = None if T_cam_in_arm1 is not None and T_cam_in_arm2 is not None: from scipy.spatial.transform import Rotation as _R T_arm2_in_arm1 = T_cam_in_arm1 @ np.linalg.inv(T_cam_in_arm2) T_arm2_now = T_arm1_in_world @ T_arm2_in_arm1 arm2_anchor_pos = T_arm2_now[:3, 3].copy() q_xyzw = _R.from_matrix(T_arm2_now[:3, :3]).as_quat() arm2_anchor_quat_wxyz = q_xyzw[[3, 0, 1, 2]].copy() model.body_pos[arm2_body_id] = arm2_anchor_pos model.body_quat[arm2_body_id] = arm2_anchor_quat_wxyz mujoco.mj_forward(model, data) # Re-position arm-2 exo meshes now that its base moved. if T_cam_in_arm1 is not None and T_cam_in_arm2 is not None: try: from exo_utils import ( position_exoskeleton_meshes, get_link_poses_from_robot) position_exoskeleton_meshes( exo_a2, model, data, get_link_poses_from_robot(exo_a2, model, data)) except Exception: pass cam = cams_by_name[scene_name] K, dist, _ = cam.get_intrinsics() # Grab the freshest live frame — drain a few for warmup. bgr = None for _ in range(frame_pool_size): if cam.grab(): fr = cam.get_frame() if fr is not None and fr.color is not None: bgr = fr.color if bgr is None: print(f" [record_testing] no live frame from " f"'{scene_name}' — panel skipped") continue H, W = bgr.shape[:2] # Anchor render on arm 1's cal (documented choice in fidex). if T_cam_in_arm1 is not None: T_cam_in_world = T_arm1_in_world @ T_cam_in_arm1 else: T_cam_in_world = T_arm2_in_world @ T_cam_in_arm2 T_world_in_cam = np.linalg.inv(T_cam_in_world) try: rendered_rgb, seg = _render_with_intrinsics( model, data, T_world_in_cam, K, W, H) overlay = _compose_overlay(bgr, rendered_rgb, seg_mask=seg) overlay_bgr = cv2.cvtColor(overlay, cv2.COLOR_RGB2BGR) fg = (seg[:, :, 0] != -1).astype(np.uint8) * 255 contours, _ = cv2.findContours( fg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cv2.drawContours(overlay_bgr, contours, -1, (0, 0, 255), 2, cv2.LINE_AA) # rerun expects RGB rr.log(f"calibration/{scene_name}/overlay", _to_rerun_jpeg(overlay_bgr)) rr.log(f"calibration/{scene_name}/raw", _to_rerun_jpeg(bgr)) # Also dump the composite to /tmp so it can be eyeballed # after the record session (rerun servers die with the # process; PNG survives). _dump_path = Path(f"/tmp/cal_overlay_{scene_name}.png") cv2.imwrite(str(_dump_path), overlay_bgr) print(f" (also dumped composite to {_dump_path})") # Also log a MARKERS panel — reads the best-of-pool # annotated frame saved by `calibrate_bimanual_scene` at # CAL RESOLUTION (HD1080 for scene cam). If we detected # on the current post-downshift frame instead, scene cam # would report 0/18 markers (720p is below its detection # threshold at this distance). try: _ann_path = Path(f"/tmp/cal_bestmarkers_{scene_name}.png") if _ann_path.exists(): _mk_bgr = cv2.imread(str(_ann_path)) if _mk_bgr is not None: rr.log(f"calibration/{scene_name}/markers", _to_rerun_jpeg(_mk_bgr)) print(f" (markers panel from {_ann_path})") else: print(f" [record_testing] couldn't load " f"{_ann_path} for markers panel") else: print(f" [record_testing] no best-of-pool frame " f"at {_ann_path} — markers panel skipped") except Exception as _me: print(f" [record_testing] markers panel skipped for " f"'{scene_name}' ({type(_me).__name__}: {_me})") # Cache for live daemon thread — including this cam's OWN # arm-2 anchor so each view renders self-consistently. per_cam_render_state[scene_name] = { "cam": cam, "K": K, "T_world_in_cam": T_world_in_cam, "W": W, "H": H, "arm2_anchor_pos": arm2_anchor_pos, "arm2_anchor_quat_wxyz": arm2_anchor_quat_wxyz, } arm1_ok = "yes" if T_cam_in_arm1 is not None else "no" arm2_ok = "yes" if T_cam_in_arm2 is not None else "no" rr.log(f"calibration/{scene_name}/info", rr.TextDocument( f"scene cam '{scene_name}' overlay\n" f" arm1 calibrated: {arm1_ok}\n" f" arm2 calibrated: {arm2_ok}\n" f" overlay = mujoco arm+board render composited " f"onto live frame; red contour = rendered " f"silhouette; arms rendered at CURRENT joint " f"state at time of logging")) print(f" ✓ [record_testing] rerun panel logged for " f"'{scene_name}' (arm1={arm1_ok} arm2={arm2_ok})") n_logged += 1 except Exception as e: print(f" [record_testing] render failed for " f"'{scene_name}' ({e}); panel skipped") _PANELS_LOGGED[0] = n_logged > 0 # Wrist cameras: add MOVING-camera overlay entries. Unlike the # scene cams (static extrinsic solved at startup), a wrist cam's # world pose changes every frame — the daemon recomputes it as # T_cam_in_world = T_arm_in_world @ FK(live_q) @ T_ee_cam # using the hand-eye from the latest bimanual wrist calibration. try: _wrist_cal = _load_wrist_hand_eye( Path.home() / ".config" / "raiden" / "wrist_calibration_result_bimanual.json") for _wname, (_arm_label, _T_ee_cam) in _wrist_cal.items(): if _wname not in cams_by_name: continue _wcam = cams_by_name[_wname] _Kw, _distw, _wh = _wcam.get_intrinsics() _follower = None if robot_controller is not None: _follower = getattr( robot_controller, "follower_r" if _arm_label == "right" else "follower_l", None) per_cam_render_state[_wname] = { "cam": _wcam, "K": _Kw, "W": int(_wh[0]), "H": int(_wh[1]), "wrist": True, "T_ee_cam": _T_ee_cam, "T_arm_in_world": (T_arm1_in_world if _arm_label == "right" else T_arm2_in_world), "follower": _follower, "T_world_in_cam": None, # filled per-frame by daemon "arm2_anchor_pos": None, "arm2_anchor_quat_wxyz": None, } print(f" [record_testing] wrist overlay armed for " f"'{_wname}' ({_arm_label}, hand-eye loaded)") except Exception as _we: print(f" [record_testing] wrist panels skipped " f"({type(_we).__name__}: {_we})") # Send an explicit blueprint pinning every cam's overlay (+ markers # for scene cams) into a grid. Without it rerun's auto-layout can # bury or drop entities that were logged before the viewer # connected (markers panels went invisible, observed 2026-07-05). if n_logged > 0: try: import rerun.blueprint as rrb views = [] for _nm, _s in per_cam_render_state.items(): views.append(rrb.Spatial2DView( origin=f"calibration/{_nm}/overlay", name=f"{_nm} overlay")) if not _s.get("wrist"): views.append(rrb.Spatial2DView( origin=f"calibration/{_nm}/markers", name=f"{_nm} markers")) rr.send_blueprint(rrb.Blueprint( rrb.Grid(*views), collapse_panels=True)) print(f" [record_testing] blueprint sent " f"({len(views)} views pinned)") except Exception as _bpe: print(f" [record_testing] blueprint failed ({_bpe}); " f"using viewer auto-layout") # Spawn the live-overlay daemon. It keeps re-rendering + logging # panels at ~5 Hz for the rest of the process lifetime, using the # STORED extrinsic (from the 10-frame pool) + CURRENT joint state # + latest retrieved frame. No re-calibration during the loop. if n_logged > 0 and per_cam_render_state: try: _spawn_live_overlay_thread( per_cam_render_state=per_cam_render_state, model=model, data=data, arm2_body_id=arm2_body_id, 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, robot_controller=robot_controller, rr=rr, hz=2.0, exo_cfg_arm2=exo_a2, ) except Exception as e: print(f" [record_testing] live overlay daemon spawn " f"failed ({e}); one-shot panels only") return n_logged > 0 except Exception as e: print(f" [record_testing] overlay panel logging failed " f"({type(e).__name__}: {e}); continuing without panels") return False