preprocessing/common/actions.py
| 1 | """Canonical sensor-frame action encoding. |
| 2 | |
| 3 | One definition, shared by every consumer. This used to be duplicated in |
| 4 | ``cfm/data/encode_episodes.py``, ``jepa/model/anchors.py`` and |
| 5 | ``vm_diffusion/data/dataset.py``; all three computed the rotation in the sensor's |
| 6 | local frame but the translation in the *world* frame, and two of them disagreed |
| 7 | about the 6D rotation layout. |
| 8 | |
| 9 | The action from frame t to t+1 is expressed entirely in the sensor frame at time t: |
| 10 | |
| 11 | dp = R_t^T (p_{t+1} - p_t) (3,) metres |
| 12 | dR = R_t^T R_{t+1} (3,3) |
| 13 | r6d = Rot6D(dR) (6,) first two columns, [c1; c2] |
| 14 | a_t = [dp, r6d] (9,) |
| 15 | |
| 16 | Being body-frame matters because the tactile image is rigidly attached to the |
| 17 | sensor: a given slide across the gel must map to the same action vector no matter |
| 18 | how the hand is oriented in the world. Equivalently, the encoding is invariant to |
| 19 | any global transform of the whole pose trajectory — which is exactly what |
| 20 | ``tests`` check, and what the old world-frame translation failed. |
| 21 | |
| 22 | The 6D rotation follows Zhou et al. 2019 (the first two columns of the matrix, |
| 23 | concatenated), which is continuous in SO(3) and so friendlier to regression than |
| 24 | quaternions or Euler angles. |
| 25 | """ |
| 26 | from __future__ import annotations |
| 27 | |
| 28 | import torch |
| 29 | |
| 30 | # Rot6D of the identity rotation, in [c1; c2] layout. Subtracting this makes a |
| 31 | # near-stationary action sit near zero instead of on a large constant offset; |
| 32 | # consumers that standardize their conditioning want it, the stored artifact does not. |
| 33 | ROT6D_IDENTITY: tuple[float, ...] = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0) |
| 34 | |
| 35 | |
| 36 | def identity_action(action_dim: int = 9) -> torch.Tensor: |
| 37 | """The action of standing still, as an (action_dim,) vector. |
| 38 | |
| 39 | Zero translation plus the identity rotation, repeated per sensor. Useful as the |
| 40 | default conditioning offset: near-stationary steps then sit near zero instead of |
| 41 | on the constant 1.0s that Rot6D(I) puts in two of every nine dims. |
| 42 | """ |
| 43 | if action_dim % 9: |
| 44 | raise ValueError(f"action_dim must be a multiple of 9, got {action_dim}") |
| 45 | one = torch.tensor((0.0, 0.0, 0.0) + ROT6D_IDENTITY) |
| 46 | return one.repeat(action_dim // 9) |
| 47 | |
| 48 | |
| 49 | def quat_to_rotmat(q: torch.Tensor) -> torch.Tensor: |
| 50 | """(..., 4) quaternion in scipy xyzw order -> (..., 3, 3) rotation matrix. |
| 51 | |
| 52 | The quaternion is renormalized first: the stored poses are only unit to |
| 53 | float32 precision, and the drift compounds through the relative products. |
| 54 | """ |
| 55 | if q.shape[-1] != 4: |
| 56 | raise ValueError(f"expected (...,4) xyzw quaternion, got {tuple(q.shape)}") |
| 57 | |
| 58 | q = q / q.norm(dim=-1, keepdim=True).clamp_min(1e-8) |
| 59 | qx, qy, qz, qw = q.unbind(-1) |
| 60 | |
| 61 | r00 = 1 - 2 * (qy * qy + qz * qz) |
| 62 | r01 = 2 * (qx * qy - qz * qw) |
| 63 | r02 = 2 * (qx * qz + qy * qw) |
| 64 | r10 = 2 * (qx * qy + qz * qw) |
| 65 | r11 = 1 - 2 * (qx * qx + qz * qz) |
| 66 | r12 = 2 * (qy * qz - qx * qw) |
| 67 | r20 = 2 * (qx * qz - qy * qw) |
| 68 | r21 = 2 * (qy * qz + qx * qw) |
| 69 | r22 = 1 - 2 * (qx * qx + qy * qy) |
| 70 | |
| 71 | return torch.stack([ |
| 72 | torch.stack([r00, r01, r02], dim=-1), |
| 73 | torch.stack([r10, r11, r12], dim=-1), |
| 74 | torch.stack([r20, r21, r22], dim=-1), |
| 75 | ], dim=-2) |
| 76 | |
| 77 | |
| 78 | def matrix_to_rot6d(R: torch.Tensor) -> torch.Tensor: |
| 79 | """(..., 3, 3) -> (..., 6), the first two columns as [c1x,c1y,c1z, c2x,c2y,c2z].""" |
| 80 | cols = R[..., :, :2] # (..., 3, 2) |
| 81 | return cols.transpose(-1, -2).reshape(*R.shape[:-2], 6) |
| 82 | |
| 83 | |
| 84 | def rot6d_to_matrix(r6: torch.Tensor) -> torch.Tensor: |
| 85 | """(..., 6) -> (..., 3, 3), inverting matrix_to_rot6d by Gram-Schmidt. |
| 86 | |
| 87 | The third column is recovered as c1 x c2, so the result is orthonormal even if |
| 88 | the input drifted (e.g. came out of a network rather than a real rotation). |
| 89 | """ |
| 90 | if r6.shape[-1] != 6: |
| 91 | raise ValueError(f"expected (...,6) rot6d, got {tuple(r6.shape)}") |
| 92 | |
| 93 | a1, a2 = r6[..., :3], r6[..., 3:] |
| 94 | c1 = a1 / a1.norm(dim=-1, keepdim=True).clamp_min(1e-8) |
| 95 | a2 = a2 - (c1 * a2).sum(-1, keepdim=True) * c1 |
| 96 | c2 = a2 / a2.norm(dim=-1, keepdim=True).clamp_min(1e-8) |
| 97 | c3 = torch.cross(c1, c2, dim=-1) |
| 98 | |
| 99 | return torch.stack([c1, c2, c3], dim=-1) # columns |
| 100 | |
| 101 | |
| 102 | def sensor_action_9d(pose7: torch.Tensor) -> torch.Tensor: |
| 103 | """(..., T, 7) poses [x,y,z, qx,qy,qz,qw] -> (..., T-1, 9) sensor-frame actions. |
| 104 | |
| 105 | Poses are world <- sensor. Output row t is the motion from frame t to t+1 as |
| 106 | seen from the sensor frame at t: ``[dp(3), Rot6D(dR)(6)]``. |
| 107 | """ |
| 108 | if pose7.shape[-1] != 7: |
| 109 | raise ValueError(f"expected (...,T,7) poses, got {tuple(pose7.shape)}") |
| 110 | if pose7.shape[-2] < 2: |
| 111 | raise ValueError(f"need at least 2 frames to form an action, got {pose7.shape[-2]}") |
| 112 | |
| 113 | pos = pose7[..., :3] |
| 114 | R = quat_to_rotmat(pose7[..., 3:]) # (..., T, 3, 3) |
| 115 | |
| 116 | R_t = R[..., :-1, :, :] |
| 117 | R_t1 = R[..., 1:, :, :] |
| 118 | |
| 119 | # Contract the *first* index of R_t, i.e. apply R_t^T to the world-frame delta. |
| 120 | dp = torch.einsum("...ji,...j->...i", R_t, pos[..., 1:, :] - pos[..., :-1, :]) |
| 121 | dR = R_t.transpose(-1, -2) @ R_t1 |
| 122 | |
| 123 | return torch.cat([dp, matrix_to_rot6d(dR)], dim=-1) |
| 124 | |
| 125 | |
| 126 | def pose_action_delta(left: torch.Tensor, right: torch.Tensor) -> torch.Tensor: |
| 127 | """Left+right pose streams -> (..., T-1, 18), the two 9D actions concatenated.""" |
| 128 | if left.shape != right.shape: |
| 129 | raise ValueError(f"pose shape mismatch: left={tuple(left.shape)} right={tuple(right.shape)}") |
| 130 | return torch.cat([sensor_action_9d(left), sensor_action_9d(right)], dim=-1) |
| 131 | |
| 132 | |
| 133 | # ── SE(3) composition ───────────────────────────────────────────────────────── |
| 134 | # Actions are stored per consecutive frame pair. Consumers that sample frames at a |
| 135 | # stride (fps-subsampled latents) need the action *across* the stride, which is the |
| 136 | # ordered product of the steps it spans, not the first step of the run. |
| 137 | |
| 138 | def action9_to_matrix(a9: torch.Tensor) -> torch.Tensor: |
| 139 | """(..., 9) -> (..., 4, 4) homogeneous, with T_{t+1} = T_t @ action.""" |
| 140 | if a9.shape[-1] != 9: |
| 141 | raise ValueError(f"expected (...,9) action, got {tuple(a9.shape)}") |
| 142 | |
| 143 | T = torch.eye(4, dtype=a9.dtype, device=a9.device).expand(*a9.shape[:-1], 4, 4).clone() |
| 144 | T[..., :3, :3] = rot6d_to_matrix(a9[..., 3:]) |
| 145 | T[..., :3, 3] = a9[..., :3] |
| 146 | return T |
| 147 | |
| 148 | |
| 149 | def matrix_to_action9(T: torch.Tensor) -> torch.Tensor: |
| 150 | """(..., 4, 4) -> (..., 9), inverse of action9_to_matrix.""" |
| 151 | return torch.cat([T[..., :3, 3], matrix_to_rot6d(T[..., :3, :3])], dim=-1) |
| 152 | |
| 153 | |
| 154 | def compose_action9(a9: torch.Tensor) -> torch.Tensor: |
| 155 | """(M, 9) consecutive actions -> (9,) their ordered composition. |
| 156 | |
| 157 | An empty input yields the identity action (no motion), which is what a clamped |
| 158 | boundary window should see. |
| 159 | """ |
| 160 | if a9.ndim != 2 or a9.shape[-1] != 9: |
| 161 | raise ValueError(f"expected (M,9) actions, got {tuple(a9.shape)}") |
| 162 | |
| 163 | if a9.shape[0] == 0: |
| 164 | eye = torch.eye(4, dtype=a9.dtype, device=a9.device) |
| 165 | return matrix_to_action9(eye) |
| 166 | if a9.shape[0] == 1: |
| 167 | return a9[0] |
| 168 | |
| 169 | T = action9_to_matrix(a9) |
| 170 | out = T[0] |
| 171 | for i in range(1, T.shape[0]): |
| 172 | out = out @ T[i] |
| 173 | return matrix_to_action9(out) |
| 174 | |
| 175 | |
| 176 | def window_action9(a9: torch.Tensor, frame_idx: torch.Tensor) -> torch.Tensor: |
| 177 | """(N, 9) episode actions + (W,) frame indices -> (W-1, 9) per-window actions. |
| 178 | |
| 179 | ``frame_idx`` are indices into the *pose/frame* stream (so valid actions are |
| 180 | ``a9[i]`` for i in ``[frame_idx[k], frame_idx[k+1])``). Repeated indices — which |
| 181 | is how a clamped boundary window ends — give the identity action. |
| 182 | """ |
| 183 | steps = [] |
| 184 | for k in range(len(frame_idx) - 1): |
| 185 | i0, i1 = int(frame_idx[k]), int(frame_idx[k + 1]) |
| 186 | steps.append(compose_action9(a9[i0:max(i0, i1)])) |
| 187 | return torch.stack(steps, dim=0) |
| 188 | |
| 189 | |
| 190 | __all__ = [ |
| 191 | "ROT6D_IDENTITY", |
| 192 | "quat_to_rotmat", |
| 193 | "matrix_to_rot6d", |
| 194 | "rot6d_to_matrix", |
| 195 | "identity_action", |
| 196 | "sensor_action_9d", |
| 197 | "pose_action_delta", |
| 198 | "action9_to_matrix", |
| 199 | "matrix_to_action9", |
| 200 | "compose_action9", |
| 201 | "window_action9", |
| 202 | ] |
| 203 | |