preprocessing/vm_diffusion/model/pose7d_action.py
36.1 KB · 802 lines · python Raw
1 """Convert raw 7D world-frame poses (+ optional normal force) into 9D relative actions.
2
3 Input pose format (world frame):
4 [x, y, z, qx, qy, qz, qw] (quaternion order is xyzw)
5
6 Output action (9D):
7 [dx, dy, dz, r6_1, ..., r6_6]
8
9 first 3 dims: relative translation expressed in the previous sensor frame; when force
10 is enabled, force is encoded as extra virtual displacement along the
11 GelSight normal (local -y by default)
12 last 6 dims: relative rotation R_t^T @ R_{t+1} as Rotation6D
13 (first-two-COLUMNS convention: [col0; col1], NOT identity-subtracted)
14
15 Force is never a 10th dimension; it only shifts translation along the normal:
16 a_t = [ R_t^T (p_{t+1} - p_t) + k_f * f_n * n_gel_S ,
17 Rot6D(R_t^T R_{t+1}) ]
18
19 Two action representations share this force operation and differ only in WHICH pose
20 pair is converted (ACTION_REPRS):
21
22 delta_action (default) consecutive frames: pair (t, t+1), force f_t
23 a_t = [ R_t^T (p_{t+1} - p_t) + k_f f_t n , Rot6D(R_t^T R_{t+1}) ]
24 -> raw_pose_pair_to_force_informed_9d / convert_pose_sequence
25 action_chunking anchor-relative: pair (0, t) with anchor = first frame of the chunk
26 (the training window), force f_t OF THE CURRENT FRAME
27 a_t = [ R_0^T (p_t - p_0) + k_f f_t n , Rot6D(R_0^T R_t) ]
28 -> raw_pose_chunk_to_force_informed_9d, materialised per window from
29 the subsampled pose/force streams (subsample_pose_sequence)
30 The normal is NOT rotated: the force operation is identical to
31 delta_action, only the pose pair changes. Chunk rows are not
32 SE(3)-composable (never feed them to compose_action9).
33
34 Two force ENCODINGS (FORCE_TERMS) decide which scalar enters that force operation:
35
36 force_term delta_action row k (pair idx[k]->idx[k+1]) action_chunking row k (pair idx[0]->idx[k+1])
37 'absolute' f[idx[k]] (start frame; force_agg) f[idx[k+1]] (arrival frame)
38 'delta' f[idx[k+1]] - f[idx[k]] f[idx[k+1]] - f[idx[0]]
39
40 'absolute' encodes the indentation depth as a per-frame STATE: it does not depend
41 on the frame rate (the same 5 N is the same 2.5 mm whether a step is 33 or 167 ms).
42 'delta' encodes the CHANGE of indentation over the step -- a rate x duration quantity
43 like dp itself, so it grows with the stride. Under 'delta' the two representations
44 agree exactly at W == 2 (both f[1]-f[0]), and the delta rows of a window telescope
45 to the chunk's last row: sum_k (f[k+1]-f[k]) = f[end]-f[start]. A steady press has
46 delta ~ 0: the action then says "pressed deeper / released", not "is pressed".
47
48 Config-driven conversion (Pose7DActionConfig) additionally handles fps downsampling:
49 `target_fps < src_fps` subsamples the POSES by stride and converts between the kept
50 endpoints. For the pure-pose part this is bit-exact with the ordered SE(3) composition
51 of the native per-step actions (build_fps6_actions.py convention) -- relative transforms
52 compose to the endpoint transform. Under 'absolute' the force term is aggregated over the
53 spanned native steps (`force_agg`: 'mean' or 'start') and applied once per coarse step;
54 under 'delta' it is the endpoint difference and `force_agg` is ignored.
55
56 Run self tests:
57 python pose7d_action.py
58 """
59 from dataclasses import dataclass
60
61 import numpy as np
62
63 # GelSight normal / pressing direction in the sensor frame (local -y).
64 DEFAULT_NORMAL_AXIS = (0.0, -1.0, 0.0)
65
66 # Supported action representations (see module docstring).
67 ACTION_REPRS = ("delta_action", "action_chunking")
68
69 # Gel normal stiffness k: f_n = k * indentation. Measured value 2 N/mm (user-confirmed
70 # calibration; source reference TBD). The force-to-displacement gain used everywhere
71 # below is its inverse, k_f = 1/k. NOTE: the parquet column force_<side>_penetration_mm is
72 # exactly f / 2 (corr 1.000000, |0.5 f - pen| max 0.0000 mm over 43k rows) -- it is this
73 # same assumption restated upstream, NOT an independent measurement, so it cannot be used
74 # to validate k.
75 DEFAULT_STIFFNESS_N_PER_MM = 2.0
76
77 # Force encodings (see module docstring): 'absolute' = per-frame indentation state,
78 # 'delta' = change of indentation over the pair's span.
79 FORCE_TERMS = ("absolute", "delta")
80
81
82 def stiffness_to_k_f(stiffness_n_per_mm=DEFAULT_STIFFNESS_N_PER_MM):
83 """Gel stiffness k [N/mm] -> force-to-displacement gain k_f [m/N].
84
85 displacement = f_n / k, so k_f = 1 / (k * 1000). k = 2 N/mm -> k_f = 0.0005 m/N.
86 """
87 k = float(stiffness_n_per_mm)
88 if k <= 0:
89 raise ValueError(f"stiffness must be positive, got {k} N/mm")
90 return 1.0 / (k * 1000.0)
91
92
93 def normalize(v, eps=1e-12):
94 v = np.asarray(v, dtype=np.float64)
95 n = np.linalg.norm(v)
96 if n < eps:
97 raise ValueError(f"cannot normalize near-zero vector: {v}")
98 return v / n
99
100
101 def quaternion_xyzw_to_matrix(q):
102 """[qx, qy, qz, qw] (world<-sensor, i.e. R_WS) -> 3x3 rotation matrix."""
103 qx, qy, qz, qw = normalize(q)
104 return np.array(
105 [
106 [1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)],
107 [2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)],
108 [2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)],
109 ],
110 dtype=np.float64,
111 )
112
113
114 def matrix_to_rotation_6d(R):
115 """3x3 -> 6D, first-two-columns convention: [R[:,0], R[:,1]]."""
116 R = np.asarray(R, dtype=np.float64)
117 return np.concatenate([R[:, 0], R[:, 1]])
118
119
120 def rotation_6d_to_matrix(r6):
121 """6D -> 3x3 via Gram-Schmidt on the two columns (inverse of matrix_to_rotation_6d)."""
122 r6 = np.asarray(r6, dtype=np.float64)
123 a1, a2 = r6[:3], r6[3:6]
124 b1 = normalize(a1)
125 b2 = normalize(a2 - np.dot(b1, a2) * b1)
126 b3 = np.cross(b1, b2)
127 return np.stack([b1, b2, b3], axis=1)
128
129
130 def compose_action9(actions):
131 """Ordered SE(3) composition of (N, 9) pure-pose actions -> one (9,) action.
132
133 Equals the direct action between the endpoint poses. Only valid on actions WITHOUT
134 a force term (force displacement would be wrongly rotated by later steps).
135 """
136 actions = np.asarray(actions, dtype=np.float64)
137 p, R = np.zeros(3), np.eye(3)
138 for a in actions:
139 p = p + R @ a[:3]
140 R = R @ rotation_6d_to_matrix(a[3:])
141 return np.concatenate([p, matrix_to_rotation_6d(R)])
142
143
144 def raw_pose_pair_to_force_informed_9d(
145 pose_t,
146 pose_tp1,
147 normal_force,
148 k_f,
149 normal_axis=DEFAULT_NORMAL_AXIS,
150 ):
151 """
152 pose_t / pose_tp1:
153 [x, y, z, qx, qy, qz, qw] in world coordinates
154 normal_force:
155 scalar normal force in Newtons (force at step t)
156 k_f:
157 force-to-displacement gain in meters/Newton
158 normal_axis:
159 GelSight normal in sensor coordinates (default local -y)
160
161 returns:
162 (9,) float64 force-informed relative action
163 """
164 pose_t = np.asarray(pose_t, dtype=np.float64)
165 pose_tp1 = np.asarray(pose_tp1, dtype=np.float64)
166 if pose_t.shape != (7,) or pose_tp1.shape != (7,):
167 raise ValueError(f"poses must be 7D [x,y,z,qx,qy,qz,qw], got {pose_t.shape} / {pose_tp1.shape}")
168
169 p_t_W, q_t = pose_t[:3], pose_t[3:7]
170 p_tp1_W, q_tp1 = pose_tp1[:3], pose_tp1[3:7]
171
172 R_t = quaternion_xyzw_to_matrix(q_t) # R_WS at t
173 R_tp1 = quaternion_xyzw_to_matrix(q_tp1) # R_WS at t+1
174
175 # world displacement -> previous sensor frame
176 delta_p_S = R_t.T @ (p_tp1_W - p_t_W)
177
178 # force as virtual displacement along the gel normal (translation only)
179 n_gel_S = normalize(normal_axis)
180 delta_p_force_informed_S = delta_p_S + k_f * float(normal_force) * n_gel_S
181
182 # relative rotation in the previous sensor frame
183 delta_R = R_t.T @ R_tp1
184 rot6d = matrix_to_rotation_6d(delta_R)
185
186 return np.concatenate([delta_p_force_informed_S, rot6d])
187
188
189 def raw_pose_sequence_to_force_informed_9d(
190 poses,
191 normal_forces,
192 k_f,
193 normal_axis=DEFAULT_NORMAL_AXIS,
194 ):
195 """
196 poses: (T, 7) world-frame raw poses
197 normal_forces: (T,) or (T-1,) scalar normal forces; force at step t is
198 applied to the action t -> t+1, so only the first T-1 are used
199 returns: (T-1, 9) force-informed actions
200 """
201 poses = np.asarray(poses, dtype=np.float64)
202 normal_forces = np.asarray(normal_forces, dtype=np.float64).reshape(-1)
203 T = poses.shape[0]
204 if poses.ndim != 2 or poses.shape[1] != 7 or T < 2:
205 raise ValueError(f"poses must be (T>=2, 7), got {poses.shape}")
206 if normal_forces.shape[0] not in (T, T - 1):
207 raise ValueError(f"need {T} or {T - 1} forces for {T} poses, got {normal_forces.shape[0]}")
208
209 return np.stack(
210 [
211 raw_pose_pair_to_force_informed_9d(poses[t], poses[t + 1], normal_forces[t], k_f, normal_axis)
212 for t in range(T - 1)
213 ]
214 )
215
216
217 def raw_pose_chunk_to_force_informed_9d(
218 poses,
219 normal_forces,
220 k_f,
221 normal_axis=DEFAULT_NORMAL_AXIS,
222 force_term="absolute",
223 ):
224 """`action_chunking`: (W, 7) chunk poses + (W,) per-frame forces -> (W-1, 9) rows.
225
226 Row k (k = 0..W-2) is the force-informed action from the ANCHOR (frame 0) to frame
227 k+1, i.e. ``raw_pose_pair_to_force_informed_9d(poses[0], poses[k+1], f_k, ...)`` with
228
229 force_term='absolute' f_k = normal_forces[k+1] (arrival frame)
230 force_term='delta' f_k = normal_forces[k+1] - normal_forces[0] (change since the anchor)
231
232 Why the arrival frame and not "the start frame" like delta_action: every chunk row
233 starts at the same anchor, so a start-frame rule would put f[0] on every row and
234 carry no information. Under 'absolute' this makes W == 2 differ from the delta_action
235 row only by k_f * (f[1] - f[0]) along the normal; under 'delta' the two coincide.
236
237 Rows are relative to one common anchor and are NOT SE(3)-composable.
238 """
239 if force_term not in FORCE_TERMS:
240 raise ValueError(f"force_term must be one of {FORCE_TERMS}, got {force_term!r}")
241 poses = np.asarray(poses, dtype=np.float64)
242 normal_forces = np.asarray(normal_forces, dtype=np.float64).reshape(-1)
243 W = poses.shape[0]
244 if poses.ndim != 2 or poses.shape[1] != 7 or W < 2:
245 raise ValueError(f"poses must be (W>=2, 7), got {poses.shape}")
246 if normal_forces.shape[0] != W:
247 raise ValueError(f"need {W} per-frame forces for {W} poses, got {normal_forces.shape[0]}")
248 anchor = poses[0]
249 f_ref = normal_forces[0] if force_term == "delta" else 0.0
250 return np.stack(
251 [
252 raw_pose_pair_to_force_informed_9d(
253 anchor, poses[k + 1], normal_forces[k + 1] - f_ref, k_f, normal_axis)
254 for k in range(W - 1)
255 ]
256 )
257
258
259 # ------------------------------------------------------------------- config-driven pipeline
260
261 @dataclass
262 class Pose7DActionConfig:
263 """End-to-end conversion settings: raw pose stream -> training-rate 9D actions.
264
265 use_force False -> pure pose actions (sensor_actions_fps6.pt convention);
266 True -> force-informed (needs `normal_forces` at call time)
267 stiffness_n_per_mm gel normal stiffness k in N/mm; the force gain is its inverse,
268 k_f = 1/(k*1000) m/N (only read when use_force)
269 k_f explicit force-to-displacement gain in m/N; None (default) ->
270 derived from stiffness_n_per_mm, a value overrides it
271 normal_axis GelSight normal in the sensor frame
272 src_fps rate of the raw pose stream (e.g. 30.0)
273 target_fps None / 0 / >= src_fps -> no downsampling; else poses are subsampled
274 by stride = round(src_fps / target_fps) (the AllTracker fps6 stride)
275 force_agg ('absolute' only) force per coarse step: 'mean' over the spanned
276 native steps, or 'start' (the force at the step's first native
277 frame). NOTE the dataclass default is 'mean' while
278 build_force_actions.py forces 'start'; ignored under 'delta'.
279 force_term 'absolute' (per-frame indentation state, default) or 'delta'
280 (change of indentation over the pair's span); see module docstring
281 action_repr 'delta_action' (consecutive pairs, convert_pose_sequence) or
282 'action_chunking' (anchor-relative rows built per window by
283 raw_pose_chunk_to_force_informed_9d from subsample_pose_sequence)
284
285 target_fps defaults to 15 (the stride-2 Wan2.2 tree is the default since 2026-09-07);
286 the builders always pass it explicitly.
287 """
288 use_force: bool = True
289 stiffness_n_per_mm: float = DEFAULT_STIFFNESS_N_PER_MM
290 k_f: float | None = None
291 normal_axis: tuple = DEFAULT_NORMAL_AXIS
292 src_fps: float = 30.0
293 target_fps: float | None = 15.0
294 force_agg: str = "mean"
295 force_term: str = "absolute"
296 action_repr: str = "delta_action"
297
298 def __post_init__(self):
299 if self.action_repr not in ACTION_REPRS:
300 raise ValueError(f"action_repr must be one of {ACTION_REPRS}, got {self.action_repr!r}")
301 if self.force_term not in FORCE_TERMS:
302 raise ValueError(f"force_term must be one of {FORCE_TERMS}, got {self.force_term!r}")
303
304 def resolved_k_f(self) -> float:
305 return self.k_f if self.k_f is not None else stiffness_to_k_f(self.stiffness_n_per_mm)
306
307 def stride(self) -> int:
308 if not self.target_fps or self.target_fps >= self.src_fps:
309 return 1
310 return max(1, int(round(self.src_fps / self.target_fps)))
311
312
313 def convert_pose_sequence(poses, normal_forces=None, config: Pose7DActionConfig = None):
314 """(T, 7) raw poses [+ (T,) or (T-1,) native-rate forces] -> downsampled 9D actions.
315
316 Downsampling subsamples the poses (frame_idx = arange(0, T, stride)) and converts
317 between kept endpoints -- for the pose part identical to composing the native
318 per-step actions, without compounding error. Force for coarse step k: under
319 force_term='absolute' it aggregates the native forces spanned by
320 [frame_idx[k], frame_idx[k+1]) (`force_agg`); under 'delta' it is the endpoint
321 difference f[frame_idx[k+1]] - f[frame_idx[k]] (needs all T per-frame forces).
322
323 This is the `delta_action` representation; for `action_chunking` store the streams
324 from ``subsample_pose_sequence`` and build rows per window with
325 ``raw_pose_chunk_to_force_informed_9d``.
326
327 returns:
328 actions (F-1, 9) float64
329 frame_idx (F,) int64 indices into the native pose stream; action[k]
330 covers native frames [frame_idx[k], frame_idx[k+1])
331 """
332 if config is None:
333 config = Pose7DActionConfig()
334 if config.action_repr != "delta_action":
335 raise ValueError(
336 f"convert_pose_sequence builds delta_action rows only; config.action_repr="
337 f"{config.action_repr!r} -> use subsample_pose_sequence + raw_pose_chunk_to_force_informed_9d"
338 )
339 poses = np.asarray(poses, dtype=np.float64)
340 T = poses.shape[0]
341 if poses.ndim != 2 or poses.shape[1] != 7 or T < 2:
342 raise ValueError(f"poses must be (T>=2, 7), got {poses.shape}")
343
344 if config.use_force:
345 if normal_forces is None:
346 raise ValueError("config.use_force=True but normal_forces is None")
347 normal_forces = np.asarray(normal_forces, dtype=np.float64).reshape(-1)
348 if normal_forces.shape[0] not in (T, T - 1):
349 raise ValueError(f"need {T} or {T - 1} forces for {T} poses, got {normal_forces.shape[0]}")
350 if config.force_term == "delta" and normal_forces.shape[0] != T:
351 raise ValueError(f"force_term='delta' needs one force per frame ({T}), "
352 f"got {normal_forces.shape[0]}")
353 step_forces = normal_forces[: T - 1] # force of native step t -> t+1
354
355 frame_idx = np.arange(0, T, config.stride(), dtype=np.int64)
356 if frame_idx.shape[0] < 2:
357 raise ValueError(f"stride {config.stride()} leaves <2 frames of {T}")
358
359 k_f = config.resolved_k_f() if config.use_force else 0.0
360 actions = []
361 for k in range(frame_idx.shape[0] - 1):
362 s, e = int(frame_idx[k]), int(frame_idx[k + 1])
363 if not config.use_force:
364 f = 0.0
365 elif config.force_term == "delta":
366 f = float(normal_forces[e] - normal_forces[s])
367 else:
368 span = step_forces[s:e]
369 f = float(span.mean()) if config.force_agg == "mean" else float(span[0])
370 actions.append(
371 raw_pose_pair_to_force_informed_9d(poses[s], poses[e], f, k_f, config.normal_axis)
372 )
373 return np.stack(actions), frame_idx
374
375
376 def subsample_pose_sequence(poses, normal_forces, config: Pose7DActionConfig = None):
377 """(T, 7) raw poses + (T,) native-rate forces -> per-frame streams at the target rate.
378
379 The `action_chunking` counterpart of ``convert_pose_sequence``: the same frame grid
380 ``frame_idx = arange(0, T, stride)`` is kept, but poses and forces are stored PER
381 FRAME (not converted), because the anchor-relative rows depend on the window they
382 are built for. Force is point-sampled at the native frame ``frame_idx[k]``
383 (``force_sample='frame'``) -- the same value as force_fps6.pt[k] and as the
384 delta_action file's 'start' force at stride 5.
385
386 returns:
387 poses_sub (F, 7) float64
388 forces_sub (F,) float64
389 frame_idx (F,) int64
390 """
391 if config is None:
392 config = Pose7DActionConfig(action_repr="action_chunking")
393 poses = np.asarray(poses, dtype=np.float64)
394 normal_forces = np.asarray(normal_forces, dtype=np.float64).reshape(-1)
395 T = poses.shape[0]
396 if poses.ndim != 2 or poses.shape[1] != 7 or T < 2:
397 raise ValueError(f"poses must be (T>=2, 7), got {poses.shape}")
398 if normal_forces.shape[0] != T:
399 raise ValueError(f"need {T} per-frame forces for {T} poses, got {normal_forces.shape[0]}")
400 frame_idx = np.arange(0, T, config.stride(), dtype=np.int64)
401 if frame_idx.shape[0] < 2:
402 raise ValueError(f"stride {config.stride()} leaves <2 frames of {T}")
403 return poses[frame_idx].copy(), normal_forces[frame_idx].copy(), frame_idx
404
405
406 # ----------------------------------------------------------------------------- tests
407
408 def _quat_z(deg):
409 h = np.deg2rad(deg) / 2.0
410 return [0.0, 0.0, np.sin(h), np.cos(h)]
411
412
413 def _random_poses(rng, T):
414 return np.stack(
415 [np.concatenate([rng.standard_normal(3) * 0.1, normalize(rng.standard_normal(4))]) for _ in range(T)]
416 )
417
418
419 def _test_zero_motion_zero_force():
420 pose = np.array([0.1, 0.2, 0.3] + _quat_z(30.0))
421 a = raw_pose_pair_to_force_informed_9d(pose, pose, normal_force=0.0, k_f=0.002)
422 assert np.allclose(a[:3], 0.0, atol=1e-12), a[:3]
423 assert np.allclose(rotation_6d_to_matrix(a[3:]), np.eye(3), atol=1e-12)
424
425
426 def _test_zero_motion_positive_force():
427 pose = np.array([0.1, 0.2, 0.3] + _quat_z(30.0))
428 f_n, k_f = 4.0, 0.002
429 a = raw_pose_pair_to_force_informed_9d(pose, pose, f_n, k_f)
430 assert np.allclose(a[:3], [0.0, -k_f * f_n, 0.0], atol=1e-12), a[:3]
431 # more force -> more displacement along the pressing direction (-y)
432 a2 = raw_pose_pair_to_force_informed_9d(pose, pose, 2 * f_n, k_f)
433 assert a2[1] < a[1] < 0.0
434
435
436 def _test_spec_numerical_example():
437 pose_t = np.array([0.30, 0.20, 0.10] + _quat_z(45.0))
438 pose_tp1 = np.array([0.31, 0.20, 0.10] + _quat_z(90.0))
439 a = raw_pose_pair_to_force_informed_9d(pose_t, pose_tp1, normal_force=4.0, k_f=0.002)
440 s = np.sqrt(0.5)
441 expected = np.array([0.01 * s, -0.01 * s - 0.008, 0.0, s, s, 0.0, -s, s, 0.0])
442 assert np.allclose(a, expected, atol=1e-4), f"\ngot {a}\nexpected {expected}"
443
444
445 def _test_translation_in_previous_sensor_frame():
446 # sensor rotated 90deg about world z; world +x displacement must appear as
447 # sensor-frame [0, -d, 0] (world x is the sensor's -y axis)
448 pose_t = np.array([0.0, 0.0, 0.0] + _quat_z(90.0))
449 pose_tp1 = np.array([0.05, 0.0, 0.0] + _quat_z(90.0))
450 a = raw_pose_pair_to_force_informed_9d(pose_t, pose_tp1, 0.0, k_f=0.002)
451 assert np.allclose(a[:3], [0.0, -0.05, 0.0], atol=1e-12), a[:3]
452 assert np.allclose(rotation_6d_to_matrix(a[3:]), np.eye(3), atol=1e-12)
453
454
455 def _test_rot6d_round_trip():
456 rng = np.random.default_rng(0)
457 for _ in range(100):
458 q = normalize(rng.standard_normal(4))
459 R = quaternion_xyzw_to_matrix(q)
460 R_rec = rotation_6d_to_matrix(matrix_to_rotation_6d(R))
461 assert np.allclose(R_rec, R, atol=1e-10)
462
463
464 def _test_random_consistency():
465 # reconstruct pose_tp1 from pose_t + action (with force removed) and compare
466 rng = np.random.default_rng(1)
467 f_n, k_f = 3.0, 0.002
468 n_gel_S = np.array(DEFAULT_NORMAL_AXIS)
469 for _ in range(50):
470 p_t, p_tp1 = rng.standard_normal(3) * 0.1, rng.standard_normal(3) * 0.1
471 q_t, q_tp1 = normalize(rng.standard_normal(4)), normalize(rng.standard_normal(4))
472 a = raw_pose_pair_to_force_informed_9d(
473 np.concatenate([p_t, q_t]), np.concatenate([p_tp1, q_tp1]), f_n, k_f
474 )
475 R_t = quaternion_xyzw_to_matrix(q_t)
476 delta_p_S = a[:3] - k_f * f_n * n_gel_S
477 assert np.allclose(p_t + R_t @ delta_p_S, p_tp1, atol=1e-10)
478 assert np.allclose(R_t @ rotation_6d_to_matrix(a[3:]), quaternion_xyzw_to_matrix(q_tp1), atol=1e-10)
479
480
481 def _test_unnormalized_quaternion():
482 pose_t = np.array([0.0, 0.0, 0.0] + list(2.0 * np.array(_quat_z(45.0))))
483 pose_ref = np.array([0.0, 0.0, 0.0] + _quat_z(45.0))
484 pose_tp1 = np.array([0.01, 0.0, 0.0] + _quat_z(90.0))
485 a = raw_pose_pair_to_force_informed_9d(pose_t, pose_tp1, 1.0, k_f=0.001)
486 a_ref = raw_pose_pair_to_force_informed_9d(pose_ref, pose_tp1, 1.0, k_f=0.001)
487 assert np.allclose(a, a_ref, atol=1e-12)
488
489
490 def _test_sequence():
491 poses = np.stack(
492 [
493 np.array([0.30, 0.20, 0.10] + _quat_z(45.0)),
494 np.array([0.31, 0.20, 0.10] + _quat_z(90.0)),
495 np.array([0.31, 0.20, 0.10] + _quat_z(90.0)),
496 ]
497 )
498 forces = np.array([4.0, 0.0, 5.0]) # last force unused (T forces for T poses)
499 actions = raw_pose_sequence_to_force_informed_9d(poses, forces, k_f=0.002)
500 assert actions.shape == (2, 9)
501 a0 = raw_pose_pair_to_force_informed_9d(poses[0], poses[1], 4.0, 0.002)
502 assert np.allclose(actions[0], a0)
503 assert np.allclose(actions[1], np.concatenate([[0, 0, 0], matrix_to_rotation_6d(np.eye(3))]))
504 # (T-1,) forces also accepted
505 actions2 = raw_pose_sequence_to_force_informed_9d(poses, forces[:2], k_f=0.002)
506 assert np.allclose(actions, actions2)
507
508
509 def _test_config_no_force():
510 # use_force=False ignores forces entirely and matches f=0 pairwise conversion
511 rng = np.random.default_rng(2)
512 poses = _random_poses(rng, 4)
513 cfg = Pose7DActionConfig(use_force=False, src_fps=30.0, target_fps=None)
514 actions, frame_idx = convert_pose_sequence(poses, None, cfg)
515 assert actions.shape == (3, 9) and np.array_equal(frame_idx, np.arange(4))
516 ref = raw_pose_sequence_to_force_informed_9d(poses, np.zeros(4), k_f=0.0)
517 assert np.allclose(actions, ref)
518
519
520 def _test_config_no_downsample_when_target_ge_src():
521 rng = np.random.default_rng(3)
522 poses = _random_poses(rng, 5)
523 forces = rng.uniform(0, 5, 5)
524 for tfps in (None, 0, 30.0, 60.0):
525 cfg = Pose7DActionConfig(use_force=True, k_f=0.002, src_fps=30.0, target_fps=tfps)
526 actions, frame_idx = convert_pose_sequence(poses, forces, cfg)
527 assert cfg.stride() == 1 and actions.shape == (4, 9)
528 assert np.allclose(actions, raw_pose_sequence_to_force_informed_9d(poses, forces, 0.002))
529
530
531 def _test_config_downsample_matches_composition():
532 # pure-pose downsampled action == ordered composition of the native per-step actions
533 # (the build_fps6_actions.py invariant), and frame_idx matches the fps6 stride
534 rng = np.random.default_rng(4)
535 poses = _random_poses(rng, 16)
536 cfg = Pose7DActionConfig(use_force=False, src_fps=30.0, target_fps=6.0)
537 assert cfg.stride() == 5
538 actions, frame_idx = convert_pose_sequence(poses, None, cfg)
539 assert np.array_equal(frame_idx, [0, 5, 10, 15]) and actions.shape == (3, 9)
540 native = raw_pose_sequence_to_force_informed_9d(poses, np.zeros(16), k_f=0.0)
541 for k in range(3):
542 composed = compose_action9(native[5 * k: 5 * (k + 1)])
543 assert np.allclose(actions[k], composed, atol=1e-10)
544
545
546 def _test_config_downsample_force_agg():
547 rng = np.random.default_rng(5)
548 poses = _random_poses(rng, 11)
549 forces = rng.uniform(0, 6, 10) # (T-1,) native step forces
550 k_f = 0.002
551 for agg in ("mean", "start"):
552 cfg = Pose7DActionConfig(use_force=True, k_f=k_f, src_fps=30.0, target_fps=6.0, force_agg=agg)
553 actions, frame_idx = convert_pose_sequence(poses, forces, cfg)
554 assert np.array_equal(frame_idx, [0, 5, 10]) and actions.shape == (2, 9)
555 no_force, _ = convert_pose_sequence(
556 poses, None, Pose7DActionConfig(use_force=False, src_fps=30.0, target_fps=6.0))
557 for k, (s, e) in enumerate(((0, 5), (5, 10))):
558 f = forces[s:e].mean() if agg == "mean" else forces[s]
559 expected_dy = no_force[k, 1] - k_f * f
560 assert np.isclose(actions[k, 1], expected_dy, atol=1e-12)
561 assert np.allclose(actions[k, [0, 2]], no_force[k, [0, 2]], atol=1e-12) # only -y shifts
562 assert np.allclose(actions[k, 3:], no_force[k, 3:], atol=1e-12) # rotation untouched
563
564
565 def _test_stiffness_parameter():
566 # k = 2 N/mm -> k_f = 0.0005 m/N; 4 N with zero motion -> 2 mm along -y
567 assert np.isclose(stiffness_to_k_f(2.0), 0.0005)
568 pose = np.array([0.1, 0.2, 0.3] + _quat_z(30.0))
569 poses = np.stack([pose, pose])
570 cfg = Pose7DActionConfig(use_force=True, src_fps=30.0, target_fps=None) # defaults: k=2 N/mm
571 assert np.isclose(cfg.resolved_k_f(), 0.0005)
572 actions, _ = convert_pose_sequence(poses, np.array([4.0]), cfg)
573 assert np.allclose(actions[0, :3], [0.0, -0.002, 0.0], atol=1e-12), actions[0, :3]
574 # explicit k_f overrides the stiffness-derived gain
575 cfg_kf = Pose7DActionConfig(use_force=True, k_f=0.002, src_fps=30.0, target_fps=None)
576 assert np.isclose(cfg_kf.resolved_k_f(), 0.002)
577 actions_kf, _ = convert_pose_sequence(poses, np.array([4.0]), cfg_kf)
578 assert np.allclose(actions_kf[0, :3], [0.0, -0.008, 0.0], atol=1e-12)
579 # a softer gel (smaller k) presses further for the same force
580 cfg_soft = Pose7DActionConfig(use_force=True, stiffness_n_per_mm=1.0, src_fps=30.0, target_fps=None)
581 actions_soft, _ = convert_pose_sequence(poses, np.array([4.0]), cfg_soft)
582 assert actions_soft[0, 1] < actions[0, 1] < 0.0
583
584
585 def _test_config_force_required():
586 poses = _random_poses(np.random.default_rng(6), 3)
587 try:
588 convert_pose_sequence(poses, None, Pose7DActionConfig(use_force=True))
589 except ValueError:
590 return
591 raise AssertionError("use_force=True with no forces should raise")
592
593
594 def _test_chunk_w2_equals_delta():
595 # at W=2 the chunk row is the pair action with the END frame's force
596 rng = np.random.default_rng(7)
597 poses = _random_poses(rng, 2)
598 forces = np.array([1.5, 4.0])
599 k_f = 0.002
600 chunk = raw_pose_chunk_to_force_informed_9d(poses, forces, k_f)
601 assert chunk.shape == (1, 9)
602 assert np.allclose(chunk[0], raw_pose_pair_to_force_informed_9d(poses[0], poses[1], forces[1], k_f), atol=1e-12)
603 # with zero force the two representations coincide exactly
604 chunk0 = raw_pose_chunk_to_force_informed_9d(poses, np.zeros(2), k_f)
605 delta0 = raw_pose_sequence_to_force_informed_9d(poses, np.zeros(2), k_f)
606 assert np.allclose(chunk0, delta0, atol=1e-12)
607
608
609 def _test_chunk_zero_motion_force_only():
610 pose = np.array([0.1, 0.2, 0.3] + _quat_z(30.0))
611 poses = np.stack([pose, pose, pose])
612 forces = np.array([0.0, 4.0, 2.0])
613 k_f = 0.002
614 rows = raw_pose_chunk_to_force_informed_9d(poses, forces, k_f)
615 assert rows.shape == (2, 9)
616 for k in range(2):
617 assert np.allclose(rows[k, :3], [0.0, -k_f * forces[k + 1], 0.0], atol=1e-12), rows[k, :3]
618 assert np.allclose(rotation_6d_to_matrix(rows[k, 3:]), np.eye(3), atol=1e-12)
619 # the anchor's own force (index 0) is never used
620 rows2 = raw_pose_chunk_to_force_informed_9d(poses, np.array([9.0, 4.0, 2.0]), k_f)
621 assert np.allclose(rows, rows2, atol=1e-12)
622
623
624 def _test_chunk_reconstruction():
625 # every row reconstructs frame k+1 from the anchor once its force term is removed
626 rng = np.random.default_rng(8)
627 W, k_f = 16, 0.002
628 poses = _random_poses(rng, W)
629 forces = rng.uniform(0, 6, W)
630 rows = raw_pose_chunk_to_force_informed_9d(poses, forces, k_f)
631 assert rows.shape == (W - 1, 9)
632 n = np.array(DEFAULT_NORMAL_AXIS)
633 R_0 = quaternion_xyzw_to_matrix(poses[0, 3:])
634 for k in range(W - 1):
635 dp = rows[k, :3] - k_f * forces[k + 1] * n
636 assert np.allclose(poses[0, :3] + R_0 @ dp, poses[k + 1, :3], atol=1e-10)
637 assert np.allclose(R_0 @ rotation_6d_to_matrix(rows[k, 3:]),
638 quaternion_xyzw_to_matrix(poses[k + 1, 3:]), atol=1e-10)
639
640
641 def _test_chunk_not_composable():
642 # anchor-relative rows are not per-step actions: composing them is meaningless
643 rng = np.random.default_rng(9)
644 poses = _random_poses(rng, 3)
645 rows = raw_pose_chunk_to_force_informed_9d(poses, np.array([0.0, 3.0, 3.0]), 0.002)
646 assert not np.allclose(compose_action9(rows[:2]), rows[1], atol=1e-6)
647 # wrong force length is rejected
648 try:
649 raw_pose_chunk_to_force_informed_9d(poses, np.zeros(2), 0.002)
650 except ValueError:
651 pass
652 else:
653 raise AssertionError("(W-1,) forces should be rejected for chunk rows")
654
655
656 def _test_subsample_pose_sequence():
657 rng = np.random.default_rng(10)
658 poses = _random_poses(rng, 16)
659 forces = rng.uniform(0, 6, 16)
660 cfg = Pose7DActionConfig(use_force=True, k_f=0.002, src_fps=30.0, target_fps=6.0,
661 action_repr="action_chunking")
662 poses_sub, forces_sub, frame_idx = subsample_pose_sequence(poses, forces, cfg)
663 assert np.array_equal(frame_idx, [0, 5, 10, 15])
664 assert np.allclose(poses_sub, poses[[0, 5, 10, 15]]) and np.allclose(forces_sub, forces[[0, 5, 10, 15]])
665 # pose part of a W=2 chunk row over the subsampled grid == the delta_action row at stride
666 delta, _ = convert_pose_sequence(
667 poses, forces, Pose7DActionConfig(use_force=True, k_f=0.002, src_fps=30.0, target_fps=6.0, force_agg="start"))
668 chunk = raw_pose_chunk_to_force_informed_9d(poses_sub[:2], forces_sub[:2], 0.002)
669 n = np.array(DEFAULT_NORMAL_AXIS)
670 assert np.allclose(chunk[0, 3:], delta[0, 3:], atol=1e-12)
671 assert np.allclose(chunk[0, :3] - 0.002 * forces_sub[1] * n, delta[0, :3] - 0.002 * forces_sub[0] * n, atol=1e-12)
672 # convert_pose_sequence refuses a chunking config
673 try:
674 convert_pose_sequence(poses, forces, cfg)
675 except ValueError:
676 pass
677 else:
678 raise AssertionError("convert_pose_sequence should reject action_repr=action_chunking")
679 try:
680 Pose7DActionConfig(action_repr="bogus")
681 except ValueError:
682 pass
683 else:
684 raise AssertionError("unknown action_repr should be rejected")
685
686
687 def _test_force_term_delta_pair_rows():
688 # delta_action under force_term='delta': force scalar is f[e] - f[s]; pose part unchanged
689 rng = np.random.default_rng(11)
690 poses = _random_poses(rng, 11)
691 forces = rng.uniform(0, 6, 11)
692 k_f = 0.002
693 n = np.array(DEFAULT_NORMAL_AXIS)
694 for tfps, grid in ((None, np.arange(11)), (6.0, np.array([0, 5, 10]))):
695 cfg_abs = Pose7DActionConfig(use_force=True, k_f=k_f, src_fps=30.0, target_fps=tfps,
696 force_agg="start", force_term="absolute")
697 cfg_dlt = Pose7DActionConfig(use_force=True, k_f=k_f, src_fps=30.0, target_fps=tfps,
698 force_term="delta")
699 a_abs, fi = convert_pose_sequence(poses, forces, cfg_abs)
700 a_dlt, fi2 = convert_pose_sequence(poses, forces, cfg_dlt)
701 assert np.array_equal(fi, grid) and np.array_equal(fi2, grid)
702 for k in range(len(grid) - 1):
703 s, e = grid[k], grid[k + 1]
704 assert np.allclose(a_dlt[k, 3:], a_abs[k, 3:], atol=1e-12)
705 assert np.allclose(a_dlt[k, :3] - k_f * (forces[e] - forces[s]) * n,
706 a_abs[k, :3] - k_f * forces[s] * n, atol=1e-12)
707 # 'delta' needs one force per frame
708 try:
709 convert_pose_sequence(poses, forces[:-1], Pose7DActionConfig(use_force=True, k_f=k_f,
710 src_fps=30.0, target_fps=None,
711 force_term="delta"))
712 except ValueError:
713 pass
714 else:
715 raise AssertionError("force_term='delta' with T-1 forces should raise")
716 try:
717 Pose7DActionConfig(force_term="bogus")
718 except ValueError:
719 pass
720 else:
721 raise AssertionError("unknown force_term should be rejected")
722
723
724 def _test_force_term_delta_w2_and_telescoping():
725 # under 'delta' the two representations coincide at W == 2, and the window's delta
726 # rows telescope to the chunk's last row's force
727 rng = np.random.default_rng(12)
728 W, k_f = 16, 0.002
729 poses = _random_poses(rng, W)
730 forces = rng.uniform(0, 6, W)
731 n = np.array(DEFAULT_NORMAL_AXIS)
732 delta = convert_pose_sequence(poses, forces, Pose7DActionConfig(
733 use_force=True, k_f=k_f, src_fps=30.0, target_fps=None, force_term="delta"))[0]
734 chunk = raw_pose_chunk_to_force_informed_9d(poses, forces, k_f, force_term="delta")
735 assert np.allclose(chunk[0], delta[0], atol=1e-12)
736 assert np.allclose(raw_pose_chunk_to_force_informed_9d(poses[:2], forces[:2], k_f, force_term="delta")[0],
737 delta[0], atol=1e-12)
738 # chunk row k == compose(pure delta rows 0..k) + k_f * (f[k+1] - f[0]) * n
739 pure = delta.copy()
740 pure[:, :3] -= k_f * (forces[1:] - forces[:-1])[:, None] * n
741 for k in range(W - 1):
742 c = compose_action9(pure[: k + 1])
743 assert np.allclose(chunk[k, 3:], c[3:], atol=1e-9)
744 assert np.allclose(chunk[k, :3], c[:3] + k_f * (forces[k + 1] - forces[0]) * n, atol=1e-9)
745 # telescoping of the force scalars themselves
746 assert np.isclose(np.sum(forces[1:] - forces[:-1]), forces[-1] - forces[0])
747 # 'absolute' chunk rows are unchanged by the new argument's default
748 assert np.allclose(raw_pose_chunk_to_force_informed_9d(poses, forces, k_f),
749 raw_pose_chunk_to_force_informed_9d(poses, forces, k_f, force_term="absolute"))
750
751
752 def _test_force_term_absolute_regression():
753 # the default encoding is bit-identical to the pre-force_term implementation
754 rng = np.random.default_rng(13)
755 poses = _random_poses(rng, 21)
756 forces = rng.uniform(0, 6, 21)
757 k_f = 0.0005
758 n = np.array(DEFAULT_NORMAL_AXIS)
759 for agg in ("start", "mean"):
760 cfg = Pose7DActionConfig(use_force=True, k_f=k_f, src_fps=30.0, target_fps=6.0, force_agg=agg)
761 assert cfg.force_term == "absolute"
762 a, fi = convert_pose_sequence(poses, forces, cfg)
763 for k in range(len(fi) - 1):
764 s, e = fi[k], fi[k + 1]
765 f = forces[s:e].mean() if agg == "mean" else forces[s]
766 ref = raw_pose_pair_to_force_informed_9d(poses[s], poses[e], f, k_f)
767 assert np.array_equal(a[k], ref)
768 ch = raw_pose_chunk_to_force_informed_9d(poses[:5], forces[:5], k_f)
769 for k in range(4):
770 assert np.array_equal(ch[k], raw_pose_pair_to_force_informed_9d(poses[0], poses[k + 1], forces[k + 1], k_f))
771
772
773 if __name__ == "__main__":
774 tests = [
775 _test_zero_motion_zero_force,
776 _test_zero_motion_positive_force,
777 _test_spec_numerical_example,
778 _test_translation_in_previous_sensor_frame,
779 _test_rot6d_round_trip,
780 _test_random_consistency,
781 _test_unnormalized_quaternion,
782 _test_sequence,
783 _test_config_no_force,
784 _test_config_no_downsample_when_target_ge_src,
785 _test_config_downsample_matches_composition,
786 _test_config_downsample_force_agg,
787 _test_stiffness_parameter,
788 _test_config_force_required,
789 _test_chunk_w2_equals_delta,
790 _test_chunk_zero_motion_force_only,
791 _test_chunk_reconstruction,
792 _test_chunk_not_composable,
793 _test_subsample_pose_sequence,
794 _test_force_term_delta_pair_rows,
795 _test_force_term_delta_w2_and_telescoping,
796 _test_force_term_absolute_regression,
797 ]
798 for t in tests:
799 t()
800 print(f"[pass] {t.__name__}")
801 print(f"all {len(tests)} tests passed")
802