# Rendering the UMI gripper / scene board in MuJoCo over real images

How to take a real camera image that sees an ArUco board, localize the camera, render a
MuJoCo model (the UMI gripper, the scene calib board, or the arm) from that viewpoint, and
blend the render over the image. This is the engine behind `arview.py` and `record_dataset.py`.

## The idea

1. **Detect** the ArUco board in the image (`cv2.aruco`, `DICT_6X6_250`).
2. **PnP** the board with the camera intrinsics `K` → board→camera transform.
3. **Camera pose in world** = compose with the board's frame in the MuJoCo world.
4. **Set a MuJoCo camera** to that pose (OpenCV→MuJoCo convention) with `fovy` from `K`.
5. **Render** (`mujoco.Renderer`) and **blend** the sim RGB over the real image.

The world frame is the board's ArUco `GridBoard` frame. The board's pose *in the MuJoCo model*
is recovered **once** by **render loop-closure** (render the sim board from a known camera,
detect it, back out its world frame) — because round-tripped GLB→MJCF bakes transforms to
identity and texture UV conventions must be measured, not guessed.

## Conventions (important)

- **Dictionary:** `DICT_6X6_250` for every board.
- **Camera frames:** OpenCV = +Z forward, X right, Y down. MuJoCo = −Z forward. Convert with
  `RX180 = diag(1,-1,-1)`: `R_mujoco = R_opencv @ RX180` (see `calib_utils.cam_mj_to_cv` / `cam_cv_to_mj`).
- **FOV is resolution-independent:** `fovy = 2*atan(H/(2*fy))`. So you can **detect/PnP at full
  resolution** (accurate) and **render at any size** (fast) with the same `fovy`.
- **Pinhole, zero distortion.** `fx=fy`, principal point at image center. Undistort first only if
  your `K` has distortion (ours doesn't).
- **Poses are `local→world` 4×4** (`X_world = T @ X_local`).

## Files you need (copy these)

Reusable, no hardware deps:
- `render_core.py` — the `Layer` class + helpers (`inject_extcam`, `set_cam`, `look_at_mj`,
  `board_geom_pose`, `measure_board_world`, `blend_cell`, `blank`, `label`).
- `calib_utils.py` — `detect_boards`, `solve_pose`, `estimate_focal`, `cam_mj_to_cv`, `RX180`, `K_from_fovy`.
- `aruco_boards.py` — the board registry (`ARM_BASE`, `UMI_V2`, `board(spec)`, `DICT`).
- `calib_board.py` — `CALIB_BOARD` (the large scene board, ids 109–132).

Python deps: `mujoco`, `opencv-python` (with `aruco`), `numpy`. On headless Linux set
`MUJOCO_GL=egl`; on macOS the default context works.

## Models (and where they live)

| what | model file | board spec | board geom material |
|---|---|---|---|
| UMI gripper | `mj_umi_v2/umi_gripper_v2.xml` | `UMI_V2` (150–161) | `ar_back` |
| scene calib board | `calib_board_model/calib_board.xml` | `CALIB_BOARD` (109–132) | `calib` |
| arm + arm board | `mj_arm_v2/arm_umi_v2.xml` | `ARM_BASE` (100–108) | `armboard` |

Each model dir must contain its meshes + ArUco texture PNG + `aruco_plane.obj` (paths are
relative to the `.xml`). Locations:
- **Mac:** `~/Projects/robotics_testing/stl_transfers/mj_umi_v2`, `mj_arm_v2`; and `ar_view/calib_board_model/`.
- **Mirrored on the VPS:** `/data/cameron/cad_recovery/mj_umi_v2`, `mj_arm_v2` — **tracked in `cad.git`**
  (bare repo `phe108:/data/cameron/repos/cad.git`). `calib_board_model/` ships inside `ar_view/`.
- Regenerate the calib board + its XML anytime with `gen_calib_board.py`.

`configs.py` points the umi/arm models at **Mac absolute paths** — repoint them if you run elsewhere.

## The `Layer` API

A `Layer` = one MuJoCo model + one ArUco board. On construction it measures the board's world
frame (loop-closure). Then per image you localize + render + blend with `blend_cell`.

```python
import cv2, numpy as np
from render_core import Layer, blend_cell
from calib_utils import detect_boards, estimate_focal
from calib_board import CALIB_BOARD          # or: from aruco_boards import UMI_V2

# 1) build the layer once (measures the board frame in the sim). map_joint maps a calibrated
#    servo's joint name -> this model's joint (use `lambda jn: None` if you don't pose joints).
layer = Layer("scene", "calib_board_model/calib_board.xml", CALIB_BOARD, "calib", lambda jn: None)

# 2) per image:
img  = cv2.imread("frame.jpg")               # BGR
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
dets = detect_boards(gray, [CALIB_BOARD])    # {name: (objpts Nx3, imgpts Nx2)}

# intrinsics: a saved K, or a quick single-view estimate (pinhole):
K, _, _ = estimate_focal([dets[CALIB_BOARD["name"]]], (img.shape[1], img.shape[0]),
                         f_guess=0.9*max(img.shape[:2]))
fovy = 2*np.degrees(np.arctan(img.shape[0] / 2.0 / K[1,1]))

overlay = blend_cell(layer, dets, K, img, fovy)   # real | sim 50% blend, or "not observed" panel
cv2.imwrite("overlay.jpg", overlay)
```

Detect and render can be **different resolutions**: pass full-res `dets`/`K`, a smaller `img`,
and the (resolution-independent) `fovy` — `blend_cell` renders at `img`'s size.

### Posing joints from servo state (optional)

To move the gripper jaw (or arm joints) to a live state before rendering:

```python
layer = Layer("umi", "mj_umi_v2/umi_gripper_v2.xml", UMI_V2, "ar_back",
              lambda jn: "jaw" if jn in ("jaw", "gripper_jaw") else None)
layer.build_posing(calib_dict)     # calib_dict = {servo_id(str): {"joint":..., "sign":...}}
layer.pose({0: 2100})              # {servo_id: raw_tick}; sets qpos via the recorded sign
# ...then blend_cell as above
```

### Lower level (arbitrary camera pose, no board)

```python
from render_core import inject_extcam, set_cam
import mujoco
tmp = inject_extcam("calib_board_model/calib_board.xml")   # adds a repose-able "extcam"
model = mujoco.MjModel.from_xml_path(tmp); data = mujoco.MjData(model)
extcam = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, "extcam")
set_cam(model, extcam, T_camCV_world, fovy_deg)            # T is OpenCV camera pose in world
mujoco.mj_forward(model, data)
with mujoco.Renderer(model, H, W) as r:
    r.update_scene(data, camera=extcam)
    sim_rgb = r.render()                                   # RGB uint8
```

`T_camCV_world` for a board = `board_world_frame @ inv(solve_pose(objpts, imgpts, K))`, where
`board_world_frame` is `layer.T_board_w` (from the loop-closure measurement).

## Gotchas

- **GLB round-trips bake transforms to identity** — never read a body/camera pose straight from a
  round-tripped model; use `measure_board_world` (loop-closure) and geometry-derived camera poses.
- **The board must render detectably** in the sim (texture must load) for loop-closure to work.
- **Mirror transforms on chiral parts print as mirror images** — compose rigid transforms, don't reflect.
- **`board_geom_pose(model, data, material_name)`** finds the board plane by its material — pass the
  right material name (`calib` / `ar_back` / `armboard`).
- The optical/board-normal sign for cameras derived from geometry can flip; `measure_board_world`
  tries both board-normal directions.