preprocessing/vm_diffusion/model/pose7d_action.py
| 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 | |