preprocessing/common/actions.py
8.0 KB · 203 lines · python Raw
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