preprocessing/common/gel_frame.py
8.1 KB · 204 lines · python Raw
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