preprocessing/common/gel_frame.py
| 1 | """The gel frame: the sensor frame re-expressed so +z is the real gel normal. |
| 2 | |
| 3 | The OptiTrack rigid-body frame attached to each sensor is arbitrary — its axes come |
| 4 | from how the markers were placed, not from the gel. The dual-ball calibration in |
| 5 | ``data/<task>/calibration/T_gel_to_rigid_<side>.json`` records where the gel actually |
| 6 | is relative to that rigid body: |
| 7 | |
| 8 | gel_axis_in_rigid unit normal of the gel surface, in RIGID coordinates |
| 9 | gel_center_in_rigid_mm gel surface centre, in RIGID coordinates, millimetres |
| 10 | |
| 11 | Both are far from the rigid axes: the normal is roughly -y (left |
| 12 | ``(-0.174, -0.932, -0.316)``, right ``(+0.350, -0.925, +0.149)``) and the surface sits |
| 13 | 66 mm (left) / 72 mm (right) from the rigid origin. So a "sensor-frame" action has a |
| 14 | ``dp_z`` that is not press depth, a ``dp_x, dp_y`` that are not shear, and a rotation |
| 15 | whose lever arm is taken about the wrong point. |
| 16 | |
| 17 | This module builds the rigid <- gel transform |
| 18 | |
| 19 | T_rigid_gel = [R_g | c_gel], R_g = [e1 e2 gel_axis], c_gel = gel_center / 1000 |
| 20 | |
| 21 | so that composing it onto the world poses BEFORE ``common.actions.sensor_action_9d`` |
| 22 | yields a 9D action already in the gel frame: |
| 23 | |
| 24 | T_world_gel = T_world_rigid @ T_rigid_gel |
| 25 | action_gel = sensor_action_9d(pose7(T_world_gel)) |
| 26 | |
| 27 | Doing it at the pose level rather than rotating downstream rasters means every |
| 28 | consumer becomes correct for free — in particular ``build_inverse_action_flow.py`` |
| 29 | puts its grid on the gel frame's ``z = 0`` plane, which is now the actual gel surface |
| 30 | in its actual location. |
| 31 | |
| 32 | The in-plane axes e1, e2 are NOT pinned by the calibration (only the normal is). We |
| 33 | fix them deterministically from a reference vector so every rebuild agrees; the choice |
| 34 | is arbitrary and a learned encoder absorbs it, but it must be recorded so Stage 2 can |
| 35 | reproduce the identical basis. |
| 36 | """ |
| 37 | from __future__ import annotations |
| 38 | |
| 39 | import json |
| 40 | from pathlib import Path |
| 41 | |
| 42 | import numpy as np |
| 43 | import torch |
| 44 | |
| 45 | _REPO = Path(__file__).resolve().parents[1] |
| 46 | |
| 47 | # Both tasks were calibrated with the same rig and the files are byte-identical, but |
| 48 | # resolve per task anyway so a future re-calibration of one does not silently apply |
| 49 | # to the other. |
| 50 | GEL_CALIB_DIRS = { |
| 51 | "motherboard": _REPO / "data" / "motherboard" / "calibration", |
| 52 | "pusht": _REPO / "data" / "pushT" / "calibration", |
| 53 | } |
| 54 | |
| 55 | |
| 56 | def calib_dir_for(episode: str) -> Path: |
| 57 | """``motherboard_0510_episode_000`` -> that task's calibration directory.""" |
| 58 | task = episode.split("_")[0] |
| 59 | try: |
| 60 | return GEL_CALIB_DIRS[task] |
| 61 | except KeyError: |
| 62 | raise KeyError( |
| 63 | f"no gel calibration registered for task {task!r} (episode {episode!r}); " |
| 64 | f"known tasks: {sorted(GEL_CALIB_DIRS)}" |
| 65 | ) from None |
| 66 | |
| 67 | |
| 68 | def gel_frame(side: str, calib_dir: Path) -> tuple[np.ndarray, np.ndarray, dict]: |
| 69 | """-> (R_g (3,3), c_gel (3,) metres, info) for ``rigid <- gel``. |
| 70 | |
| 71 | ``R_g[:, 2]`` is ``gel_axis_in_rigid``: the press / normal axis. The returned |
| 72 | matrix is orthonormal with det +1. |
| 73 | """ |
| 74 | if side not in ("left", "right"): |
| 75 | raise ValueError(f"side must be 'left' or 'right', got {side!r}") |
| 76 | path = Path(calib_dir) / f"T_gel_to_rigid_{side}.json" |
| 77 | if not path.is_file(): |
| 78 | raise FileNotFoundError(f"gel calibration missing: {path}") |
| 79 | c = json.loads(path.read_text()) |
| 80 | |
| 81 | n = np.asarray(c["gel_axis_in_rigid"], dtype=np.float64) |
| 82 | n = n / np.linalg.norm(n) |
| 83 | c_gel = np.asarray(c["gel_center_in_rigid_mm"], dtype=np.float64) / 1000.0 |
| 84 | |
| 85 | # Deterministic in-plane basis: project a fixed reference onto the tangent plane. |
| 86 | # Swap the reference when it is near-parallel to n, or the cross product degenerates. |
| 87 | ref = np.array([0.0, 0.0, 1.0]) if abs(n[2]) < 0.9 else np.array([1.0, 0.0, 0.0]) |
| 88 | e1 = np.cross(ref, n) |
| 89 | e1 /= np.linalg.norm(e1) |
| 90 | e2 = np.cross(n, e1) |
| 91 | R_g = np.stack([e1, e2, n], axis=1) # columns |
| 92 | |
| 93 | if np.linalg.det(R_g) < 0: # keep it a rotation, never a reflection |
| 94 | R_g[:, 1] *= -1.0 |
| 95 | |
| 96 | info = { |
| 97 | "gel_axis_in_rigid": n.tolist(), |
| 98 | "gel_center_in_rigid_m": c_gel.tolist(), |
| 99 | "R_rigid_from_gel": R_g.tolist(), |
| 100 | "in_plane_reference": ref.tolist(), |
| 101 | "in_plane_rule": "e1 = normalize(ref x n); e2 = n x e1; ref = +z unless |n_z| >= 0.9, then +x", |
| 102 | "gel_distance_mm": float(c.get("gel_distance_mm", float("nan"))), |
| 103 | "depth_offset_mm": float(c.get("depth_offset_mm", float("nan"))), |
| 104 | "calib_file": str(path), |
| 105 | "calib_method": c.get("method"), |
| 106 | } |
| 107 | return R_g, c_gel, info |
| 108 | |
| 109 | |
| 110 | def rotmat_to_quat(R: np.ndarray) -> np.ndarray: |
| 111 | """(..., 3, 3) rotation -> (..., 4) quaternion, scipy xyzw order. |
| 112 | |
| 113 | The inverse of ``common.actions.quat_to_rotmat``. Written out rather than taken |
| 114 | from scipy because scipy is not importable under ``PYTHONNOUSERSITE=1``, which is |
| 115 | how the training scripts launch. Uses Shepperd's method: pick the branch whose |
| 116 | denominator is largest, so the division never approaches 0. |
| 117 | """ |
| 118 | R = np.asarray(R, dtype=np.float64) |
| 119 | shape = R.shape[:-2] |
| 120 | m = R.reshape(-1, 3, 3) |
| 121 | n = m.shape[0] |
| 122 | |
| 123 | d = np.stack([ |
| 124 | 1.0 + m[:, 0, 0] + m[:, 1, 1] + m[:, 2, 2], |
| 125 | 1.0 + m[:, 0, 0] - m[:, 1, 1] - m[:, 2, 2], |
| 126 | 1.0 - m[:, 0, 0] + m[:, 1, 1] - m[:, 2, 2], |
| 127 | 1.0 - m[:, 0, 0] - m[:, 1, 1] + m[:, 2, 2], |
| 128 | ], axis=1) |
| 129 | k = d.argmax(axis=1) |
| 130 | s = 2.0 * np.sqrt(np.maximum(d[np.arange(n), k], 1e-20)) |
| 131 | |
| 132 | q = np.zeros((n, 4), dtype=np.float64) # x, y, z, w |
| 133 | for case in range(4): |
| 134 | i = k == case |
| 135 | if not i.any(): |
| 136 | continue |
| 137 | si, mi = s[i], m[i] |
| 138 | if case == 0: |
| 139 | q[i, 3] = si / 4.0 |
| 140 | q[i, 0] = (mi[:, 2, 1] - mi[:, 1, 2]) / si |
| 141 | q[i, 1] = (mi[:, 0, 2] - mi[:, 2, 0]) / si |
| 142 | q[i, 2] = (mi[:, 1, 0] - mi[:, 0, 1]) / si |
| 143 | elif case == 1: |
| 144 | q[i, 0] = si / 4.0 |
| 145 | q[i, 3] = (mi[:, 2, 1] - mi[:, 1, 2]) / si |
| 146 | q[i, 1] = (mi[:, 0, 1] + mi[:, 1, 0]) / si |
| 147 | q[i, 2] = (mi[:, 0, 2] + mi[:, 2, 0]) / si |
| 148 | elif case == 2: |
| 149 | q[i, 1] = si / 4.0 |
| 150 | q[i, 3] = (mi[:, 0, 2] - mi[:, 2, 0]) / si |
| 151 | q[i, 0] = (mi[:, 0, 1] + mi[:, 1, 0]) / si |
| 152 | q[i, 2] = (mi[:, 1, 2] + mi[:, 2, 1]) / si |
| 153 | else: |
| 154 | q[i, 2] = si / 4.0 |
| 155 | q[i, 3] = (mi[:, 1, 0] - mi[:, 0, 1]) / si |
| 156 | q[i, 0] = (mi[:, 0, 2] + mi[:, 2, 0]) / si |
| 157 | q[i, 1] = (mi[:, 1, 2] + mi[:, 2, 1]) / si |
| 158 | |
| 159 | q /= np.linalg.norm(q, axis=1, keepdims=True) |
| 160 | return q.reshape(*shape, 4) |
| 161 | |
| 162 | |
| 163 | def poses_to_gel(pose7: torch.Tensor, R_g: np.ndarray, c_gel: np.ndarray) -> torch.Tensor: |
| 164 | """(T, 7) world <- rigid poses -> (T, 7) world <- gel poses, same [xyz, qxyzw] layout. |
| 165 | |
| 166 | Quaternion sign is not made continuous: ``sensor_action_9d`` goes through |
| 167 | ``quat_to_rotmat``, which is invariant to q -> -q. |
| 168 | """ |
| 169 | from common.actions import quat_to_rotmat |
| 170 | |
| 171 | if pose7.ndim != 2 or pose7.shape[-1] != 7: |
| 172 | raise ValueError(f"expected (T,7) poses, got {tuple(pose7.shape)}") |
| 173 | |
| 174 | p = pose7[:, :3].double().numpy() |
| 175 | R = quat_to_rotmat(pose7[:, 3:].double()).numpy() # (T,3,3) world <- rigid |
| 176 | |
| 177 | R_wg = R @ R_g # world <- gel |
| 178 | p_wg = p + np.einsum("tij,j->ti", R, c_gel) # gel origin in world |
| 179 | |
| 180 | return torch.from_numpy(np.concatenate([p_wg, rotmat_to_quat(R_wg)], axis=1)) |
| 181 | |
| 182 | |
| 183 | def gel_action_closed_form(a9_rigid: torch.Tensor, R_g: np.ndarray, |
| 184 | c_gel: np.ndarray) -> torch.Tensor: |
| 185 | """Independent reference for the pose-composition path, used by the verify gate. |
| 186 | |
| 187 | ``A_gel = T_rg^-1 A_rigid T_rg`` expands to a conjugation of the rotation plus a |
| 188 | lever-arm term on the translation: |
| 189 | |
| 190 | dR_gel = R_g^T dR R_g |
| 191 | dp_gel = R_g^T ((dR - I) c_gel + dp) |
| 192 | """ |
| 193 | from common.actions import matrix_to_rot6d, rot6d_to_matrix |
| 194 | |
| 195 | dp = a9_rigid[:, :3].double().numpy() |
| 196 | dR = rot6d_to_matrix(a9_rigid[:, 3:].double()).numpy() |
| 197 | |
| 198 | dp_gel = np.einsum("ij,tj->ti", R_g.T, |
| 199 | np.einsum("tij,j->ti", dR - np.eye(3), c_gel) + dp) |
| 200 | dR_gel = np.einsum("ij,tjk,kl->til", R_g.T, dR, R_g) |
| 201 | |
| 202 | r6 = matrix_to_rot6d(torch.from_numpy(dR_gel)) |
| 203 | return torch.cat([torch.from_numpy(dp_gel), r6], dim=1) |
| 204 | |