File size: 8,304 Bytes
b1b6f13
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
"""The gel frame: the sensor frame re-expressed so +z is the real gel normal.

The OptiTrack rigid-body frame attached to each sensor is arbitrary — its axes come
from how the markers were placed, not from the gel. The dual-ball calibration in
``data/<task>/calibration/T_gel_to_rigid_<side>.json`` records where the gel actually
is relative to that rigid body:

    gel_axis_in_rigid       unit normal of the gel surface, in RIGID coordinates
    gel_center_in_rigid_mm  gel surface centre, in RIGID coordinates, millimetres

Both are far from the rigid axes: the normal is roughly -y (left
``(-0.174, -0.932, -0.316)``, right ``(+0.350, -0.925, +0.149)``) and the surface sits
66 mm (left) / 72 mm (right) from the rigid origin. So a "sensor-frame" action has a
``dp_z`` that is not press depth, a ``dp_x, dp_y`` that are not shear, and a rotation
whose lever arm is taken about the wrong point.

This module builds the rigid <- gel transform

    T_rigid_gel = [R_g | c_gel],   R_g = [e1 e2 gel_axis],   c_gel = gel_center / 1000

so that composing it onto the world poses BEFORE ``common.actions.sensor_action_9d``
yields a 9D action already in the gel frame:

    T_world_gel = T_world_rigid @ T_rigid_gel
    action_gel  = sensor_action_9d(pose7(T_world_gel))

Doing it at the pose level rather than rotating downstream rasters means every
consumer becomes correct for free — in particular ``build_inverse_action_flow.py``
puts its grid on the gel frame's ``z = 0`` plane, which is now the actual gel surface
in its actual location.

The in-plane axes e1, e2 are NOT pinned by the calibration (only the normal is). We
fix them deterministically from a reference vector so every rebuild agrees; the choice
is arbitrary and a learned encoder absorbs it, but it must be recorded so Stage 2 can
reproduce the identical basis.
"""
from __future__ import annotations

import json
from pathlib import Path

import numpy as np
import torch

_REPO = Path(__file__).resolve().parents[1]

# Both tasks were calibrated with the same rig and the files are byte-identical, but
# resolve per task anyway so a future re-calibration of one does not silently apply
# to the other.
GEL_CALIB_DIRS = {
    "motherboard": _REPO / "data" / "motherboard" / "calibration",
    "pusht": _REPO / "data" / "pushT" / "calibration",
}


def calib_dir_for(episode: str) -> Path:
    """``motherboard_0510_episode_000`` -> that task's calibration directory."""
    task = episode.split("_")[0]
    try:
        return GEL_CALIB_DIRS[task]
    except KeyError:
        raise KeyError(
            f"no gel calibration registered for task {task!r} (episode {episode!r}); "
            f"known tasks: {sorted(GEL_CALIB_DIRS)}"
        ) from None


def gel_frame(side: str, calib_dir: Path) -> tuple[np.ndarray, np.ndarray, dict]:
    """-> (R_g (3,3), c_gel (3,) metres, info) for ``rigid <- gel``.

    ``R_g[:, 2]`` is ``gel_axis_in_rigid``: the press / normal axis. The returned
    matrix is orthonormal with det +1.
    """
    if side not in ("left", "right"):
        raise ValueError(f"side must be 'left' or 'right', got {side!r}")
    path = Path(calib_dir) / f"T_gel_to_rigid_{side}.json"
    if not path.is_file():
        raise FileNotFoundError(f"gel calibration missing: {path}")
    c = json.loads(path.read_text())

    n = np.asarray(c["gel_axis_in_rigid"], dtype=np.float64)
    n = n / np.linalg.norm(n)
    c_gel = np.asarray(c["gel_center_in_rigid_mm"], dtype=np.float64) / 1000.0

    # Deterministic in-plane basis: project a fixed reference onto the tangent plane.
    # Swap the reference when it is near-parallel to n, or the cross product degenerates.
    ref = np.array([0.0, 0.0, 1.0]) if abs(n[2]) < 0.9 else np.array([1.0, 0.0, 0.0])
    e1 = np.cross(ref, n)
    e1 /= np.linalg.norm(e1)
    e2 = np.cross(n, e1)
    R_g = np.stack([e1, e2, n], axis=1)          # columns

    if np.linalg.det(R_g) < 0:                   # keep it a rotation, never a reflection
        R_g[:, 1] *= -1.0

    info = {
        "gel_axis_in_rigid": n.tolist(),
        "gel_center_in_rigid_m": c_gel.tolist(),
        "R_rigid_from_gel": R_g.tolist(),
        "in_plane_reference": ref.tolist(),
        "in_plane_rule": "e1 = normalize(ref x n); e2 = n x e1; ref = +z unless |n_z| >= 0.9, then +x",
        "gel_distance_mm": float(c.get("gel_distance_mm", float("nan"))),
        "depth_offset_mm": float(c.get("depth_offset_mm", float("nan"))),
        "calib_file": str(path),
        "calib_method": c.get("method"),
    }
    return R_g, c_gel, info


def rotmat_to_quat(R: np.ndarray) -> np.ndarray:
    """(..., 3, 3) rotation -> (..., 4) quaternion, scipy xyzw order.

    The inverse of ``common.actions.quat_to_rotmat``. Written out rather than taken
    from scipy because scipy is not importable under ``PYTHONNOUSERSITE=1``, which is
    how the training scripts launch. Uses Shepperd's method: pick the branch whose
    denominator is largest, so the division never approaches 0.
    """
    R = np.asarray(R, dtype=np.float64)
    shape = R.shape[:-2]
    m = R.reshape(-1, 3, 3)
    n = m.shape[0]

    d = np.stack([
        1.0 + m[:, 0, 0] + m[:, 1, 1] + m[:, 2, 2],
        1.0 + m[:, 0, 0] - m[:, 1, 1] - m[:, 2, 2],
        1.0 - m[:, 0, 0] + m[:, 1, 1] - m[:, 2, 2],
        1.0 - m[:, 0, 0] - m[:, 1, 1] + m[:, 2, 2],
    ], axis=1)
    k = d.argmax(axis=1)
    s = 2.0 * np.sqrt(np.maximum(d[np.arange(n), k], 1e-20))

    q = np.zeros((n, 4), dtype=np.float64)           # x, y, z, w
    for case in range(4):
        i = k == case
        if not i.any():
            continue
        si, mi = s[i], m[i]
        if case == 0:
            q[i, 3] = si / 4.0
            q[i, 0] = (mi[:, 2, 1] - mi[:, 1, 2]) / si
            q[i, 1] = (mi[:, 0, 2] - mi[:, 2, 0]) / si
            q[i, 2] = (mi[:, 1, 0] - mi[:, 0, 1]) / si
        elif case == 1:
            q[i, 0] = si / 4.0
            q[i, 3] = (mi[:, 2, 1] - mi[:, 1, 2]) / si
            q[i, 1] = (mi[:, 0, 1] + mi[:, 1, 0]) / si
            q[i, 2] = (mi[:, 0, 2] + mi[:, 2, 0]) / si
        elif case == 2:
            q[i, 1] = si / 4.0
            q[i, 3] = (mi[:, 0, 2] - mi[:, 2, 0]) / si
            q[i, 0] = (mi[:, 0, 1] + mi[:, 1, 0]) / si
            q[i, 2] = (mi[:, 1, 2] + mi[:, 2, 1]) / si
        else:
            q[i, 2] = si / 4.0
            q[i, 3] = (mi[:, 1, 0] - mi[:, 0, 1]) / si
            q[i, 0] = (mi[:, 0, 2] + mi[:, 2, 0]) / si
            q[i, 1] = (mi[:, 1, 2] + mi[:, 2, 1]) / si

    q /= np.linalg.norm(q, axis=1, keepdims=True)
    return q.reshape(*shape, 4)


def poses_to_gel(pose7: torch.Tensor, R_g: np.ndarray, c_gel: np.ndarray) -> torch.Tensor:
    """(T, 7) world <- rigid poses -> (T, 7) world <- gel poses, same [xyz, qxyzw] layout.

    Quaternion sign is not made continuous: ``sensor_action_9d`` goes through
    ``quat_to_rotmat``, which is invariant to q -> -q.
    """
    from common.actions import quat_to_rotmat

    if pose7.ndim != 2 or pose7.shape[-1] != 7:
        raise ValueError(f"expected (T,7) poses, got {tuple(pose7.shape)}")

    p = pose7[:, :3].double().numpy()
    R = quat_to_rotmat(pose7[:, 3:].double()).numpy()  # (T,3,3) world <- rigid

    R_wg = R @ R_g                                     # world <- gel
    p_wg = p + np.einsum("tij,j->ti", R, c_gel)        # gel origin in world

    return torch.from_numpy(np.concatenate([p_wg, rotmat_to_quat(R_wg)], axis=1))


def gel_action_closed_form(a9_rigid: torch.Tensor, R_g: np.ndarray,
                           c_gel: np.ndarray) -> torch.Tensor:
    """Independent reference for the pose-composition path, used by the verify gate.

    ``A_gel = T_rg^-1 A_rigid T_rg`` expands to a conjugation of the rotation plus a
    lever-arm term on the translation:

        dR_gel = R_g^T dR R_g
        dp_gel = R_g^T ((dR - I) c_gel + dp)
    """
    from common.actions import matrix_to_rot6d, rot6d_to_matrix

    dp = a9_rigid[:, :3].double().numpy()
    dR = rot6d_to_matrix(a9_rigid[:, 3:].double()).numpy()

    dp_gel = np.einsum("ij,tj->ti", R_g.T,
                       np.einsum("tij,j->ti", dR - np.eye(3), c_gel) + dp)
    dR_gel = np.einsum("ij,tjk,kl->til", R_g.T, dR, R_g)

    r6 = matrix_to_rot6d(torch.from_numpy(dR_gel))
    return torch.cat([torch.from_numpy(dp_gel), r6], dim=1)