diff --git a/docs/ARC_CLOCK_CONTRACT.md b/docs/ARC_CLOCK_CONTRACT.md index 6c90fece3..ab04958aa 100644 --- a/docs/ARC_CLOCK_CONTRACT.md +++ b/docs/ARC_CLOCK_CONTRACT.md @@ -13,7 +13,7 @@ for models, frozen data pools, budgets, retained variants and migration. ```yaml _target_: egomimic.rldb.zarr.arc_length_tokenizer.TokenizeBimanualArcLengthCartesian clock_mode: velocity # velocity | duration -streams: full_separation # full_separation | translation_rotation +streams: full_separation # full_separation | translation_rotation | translation resampled_vector_length: 100 min_distance_unit: 0.42 # meters, per arm rotation_distance_unit: 1.7453292519943295 # radians, per arm (100 degrees) @@ -32,6 +32,7 @@ Geometry always occupies the first 14 columns: | --- | --- | --- | | full_separation | M × 20 | T-left, T-right, R-left, R-right, G-left, G-right | | translation_rotation | M × 18 | T-left, T-right, R-left, R-right | +| translation | M × 16 | T-left, T-right (rotation and gripper ride the translation supports) | Each arm and stream spends its own spatial budget. Supports from different streams in the same row are not necessarily simultaneous. @@ -51,9 +52,13 @@ There is no repeated final padding clock. - velocity: row zero is seconds; later rows are the distance between emitted supports divided by the source interval's seconds. Units are m/s, rad/s, and gripper units/s. -- duration: every row is `log(seconds + 0.001)`, including the initial delay. - The codec removes the log exactly once. `duration` does not mean absolute - timestamps or raw seconds. +- duration: every row is the absolute seconds from the chunk anchor to that + waypoint (row zero is the start delay). Decoding projects the rows once onto + the nearest non-decreasing clock by isotonic regression on the slack above + the fastest clock allowed by the 5 m/s translation max speed (no min speed: + rotation and gripper ride slow arms), so a backwards pair is averaged and no local + error accumulates into later rows. Provenance `timing_layout` is + `first_delay_absolute_v1`; velocity keeps `first_delay_incoming_v1`. Translation distances are Euclidean, rotation distances are SO(3) geodesic, and gripper distances are absolute value differences. No velocity smoothing, diff --git a/egomimic/eval/open_loop_sim.py b/egomimic/eval/open_loop_sim.py index 7ceb2e8f6..35e366941 100644 --- a/egomimic/eval/open_loop_sim.py +++ b/egomimic/eval/open_loop_sim.py @@ -738,7 +738,7 @@ def _cap_arc_video_trajectories( for index, steps in enumerate(prefix_lengths): rotation_cap = ( None - if rotation_distance_unit is None + if rotation_distance_unit is None or self.streams == "translation" else self.execute_fraction * rotation_distance_unit ) gripper_cap = ( diff --git a/egomimic/hydra_configs/experiment/README.md b/egomimic/hydra_configs/experiment/README.md index 9266c61b9..812a3680c 100644 --- a/egomimic/hydra_configs/experiment/README.md +++ b/egomimic/hydra_configs/experiment/README.md @@ -51,11 +51,13 @@ Each task's three entries use exactly the same filters, episode lists and split. Baseline geometry is 100 × 14 time-indexed actions. ARC uses the canonical [clock contract](../../../docs/ARC_CLOCK_CONTRACT.md) -(repository path `docs/ARC_CLOCK_CONTRACT.md`): velocity or log **interval** -seconds, first-row start delay, first-moving-stream termination. -It is not E1 or cumulative absolute seconds. Full separation is the default. -Use `abc.arc_streams=translation_rotation` for four clocks; token width -automatically changes from M × 20 to M × 18, including the model and decoder. +(repository path `docs/ARC_CLOCK_CONTRACT.md`): velocity rates or **absolute** +seconds from the chunk anchor (decoded by speed-bounded isotonic regression), +first-row start delay, first-moving-stream termination. +Full separation is the default. Use `abc.arc_streams=translation_rotation` for +four clocks (M × 18) or `translation` for one clock per arm (M × 16, rotation and +gripper ride the translation supports); token width follows automatically, +including the model and decoder. No tokenizer or detokenizer algorithm is changed by this YAML reorganization. The frozen towel/earbud ARC entries are **new canonical-codec recipes** built diff --git a/egomimic/hydra_configs/experiment/_shared/clock_settings.yaml b/egomimic/hydra_configs/experiment/_shared/clock_settings.yaml index 85bf8312b..3d1970db1 100644 --- a/egomimic/hydra_configs/experiment/_shared/clock_settings.yaml +++ b/egomimic/hydra_configs/experiment/_shared/clock_settings.yaml @@ -6,6 +6,11 @@ abc: arc_stream_dims: full_separation: 20 translation_rotation: 18 + translation: 16 + # Duration rows are absolute seconds from the anchor; velocity rows are incoming rates. + arc_timing_layouts: + velocity: first_delay_incoming_v1 + duration: first_delay_absolute_v1 arc_token_dim: ${abc.arc_stream_dims.${abc.arc_streams}} arc_token_rows: ${abc.arc_waypoints} run_provenance: @@ -17,7 +22,7 @@ run_provenance: waypoints: ${abc.arc_waypoints} clock_mode: ${abc.arc_clock_mode} streams: ${abc.arc_streams} - timing_layout: first_delay_incoming_v1 + timing_layout: ${abc.arc_timing_layouts.${abc.arc_clock_mode}} hold_window_frames: 400 gripper_stream_mode: ${oc.select:abc.arc_gripper_stream_mode,events} gripper_distance_unit: ${oc.select:abc.arc_gripper_distance,null} diff --git a/egomimic/hydra_configs/experiment/_shared/frozen_yam_arc.yaml b/egomimic/hydra_configs/experiment/_shared/frozen_yam_arc.yaml index b43705a3e..0a82b1613 100644 --- a/egomimic/hydra_configs/experiment/_shared/frozen_yam_arc.yaml +++ b/egomimic/hydra_configs/experiment/_shared/frozen_yam_arc.yaml @@ -89,5 +89,5 @@ evaluator: streams: ${abc.arc_streams} logger: wandb: - id: ${name}_s42 + id: ${name}_s${seed} tags: [rl2, hpt300, "${abc.arc_clock_mode}", "${abc.arc_streams}", open_loop, video] diff --git a/egomimic/hydra_configs/experiment/real/fold_and_stack_towels/arc_dur.yaml b/egomimic/hydra_configs/experiment/real/fold_and_stack_towels/arc_dur.yaml index 0062d26e3..5976edccc 100644 --- a/egomimic/hydra_configs/experiment/real/fold_and_stack_towels/arc_dur.yaml +++ b/egomimic/hydra_configs/experiment/real/fold_and_stack_towels/arc_dur.yaml @@ -3,6 +3,6 @@ defaults: - /experiment/real/fold_and_stack_towels/arc_vel - _self_ name: fold_and_stack_towels_arc_dur -description: Frozen since0924plus74 towels HPT300, canonical log-duration ARC M200 D0.33 R100 +description: Frozen since0924plus74 towels HPT300, canonical absolute-duration ARC M200 D0.33 R100 abc: arc_clock_mode: duration diff --git a/egomimic/hydra_configs/experiment/real/multitask/arc_dur.yaml b/egomimic/hydra_configs/experiment/real/multitask/arc_dur.yaml index a77981c78..6cf4a577b 100644 --- a/egomimic/hydra_configs/experiment/real/multitask/arc_dur.yaml +++ b/egomimic/hydra_configs/experiment/real/multitask/arc_dur.yaml @@ -3,7 +3,7 @@ defaults: - /experiment/real/multitask/arc_vel - _self_ name: multitask_arc_dur -description: HPT300 ABC four-task first-delay log-duration ARC +description: HPT300 ABC four-task first-delay absolute-duration ARC abc: arc_clock_mode: duration logger: diff --git a/egomimic/hydra_configs/experiment/real/organize_stationary/arc_dur.yaml b/egomimic/hydra_configs/experiment/real/organize_stationary/arc_dur.yaml index 6a1f951b5..ff947e3ac 100644 --- a/egomimic/hydra_configs/experiment/real/organize_stationary/arc_dur.yaml +++ b/egomimic/hydra_configs/experiment/real/organize_stationary/arc_dur.yaml @@ -3,6 +3,6 @@ defaults: - /experiment/real/organize_stationary/arc_vel - _self_ name: organize_stationary_arc_dur -description: HPT300 robot + human organize stationery, first-delay log-duration +description: HPT300 robot + human organize stationery, first-delay absolute duration abc: arc_clock_mode: duration diff --git a/egomimic/hydra_configs/experiment/real/wireless_earbuds/arc_dur.yaml b/egomimic/hydra_configs/experiment/real/wireless_earbuds/arc_dur.yaml index 3cb8a4c0a..ccf5ce8cf 100644 --- a/egomimic/hydra_configs/experiment/real/wireless_earbuds/arc_dur.yaml +++ b/egomimic/hydra_configs/experiment/real/wireless_earbuds/arc_dur.yaml @@ -3,6 +3,6 @@ defaults: - /experiment/real/wireless_earbuds/arc_vel - _self_ name: wireless_earbuds_arc_dur -description: Frozen earbuds HPT300, canonical log-duration ARC M200 D0.33 R100 +description: Frozen earbuds HPT300, canonical absolute-duration ARC M200 D0.33 R100 abc: arc_clock_mode: duration diff --git a/egomimic/rldb/AGENTS.md b/egomimic/rldb/AGENTS.md index 25fba0673..243661dac 100644 --- a/egomimic/rldb/AGENTS.md +++ b/egomimic/rldb/AGENTS.md @@ -16,7 +16,9 @@ The root AGENTS.md applies here. Dataset/experiment selection belongs in - `zarr/action_chunk_transforms.py`: shared pose, rotation, gripper, interpolation and key-routing transforms. Add general transform behavior here. - `zarr/arc_length_tokenizer.py`: Cartesian ARC with `clock_mode` velocity or - duration and `streams` full_separation (M×20) or translation_rotation (M×18). + duration (absolute seconds, speed-bounded isotonic decode) and `streams` + full_separation (M×20), translation_rotation (M×18) or translation + (M×16, one clock per arm). See `docs/ARC_CLOCK_CONTRACT.md`; row zero is a start delay. - `zarr/embodiment_resolvers.py`, `zarr/progress_sampler.py`: metadata overrides and configurable anchor sampling used by the tempo recipes. diff --git a/egomimic/rldb/zarr/arc_length_tokenizer.py b/egomimic/rldb/zarr/arc_length_tokenizer.py index a47862c4e..19f1d366b 100644 --- a/egomimic/rldb/zarr/arc_length_tokenizer.py +++ b/egomimic/rldb/zarr/arc_length_tokenizer.py @@ -43,6 +43,7 @@ from enum import Enum import numpy as np +from scipy.optimize import isotonic_regression from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Slerp @@ -1121,11 +1122,25 @@ def resolve_arc_chunking_mode(mode=None, rotation_distance_unit=None): CLOCK_MODES = ("velocity", "duration") -STREAM_MODES = ("full_separation", "translation_rotation") -LOG_CLOCK_EPS = 1e-3 -# Timing columns always follow geometry; four-stream order is the same prefix. +STREAM_MODES = ("full_separation", "translation_rotation", "translation") +# translation: one clock per arm; rotation and gripper ride its supports. +STREAM_NAMES = { + "full_separation": ("translation", "rotation", "gripper"), + "translation_rotation": ("translation", "rotation"), + "translation": ("translation",), +} +STREAM_DIMS = { + "full_separation": 20, + "translation_rotation": 18, + "translation": 16, +} +# Timing columns always follow geometry; four- and two-stream order is the same prefix. SIX_CLOCK_COLUMNS = (0, 7, 3, 10, 6, 13) CLOCK_COLUMNS = (0, 7, 3, 10) +SINGLE_CLOCK_COLUMNS = (0, 7) +# Max speed of an absolute translation clock, m/s. There is no min speed: a slow arm +# also times the rotation and gripper riding its clock, and a floor replays them fast. +MAX_SPEED = 5.0 def validate_clock_mode(mode): @@ -1148,42 +1163,95 @@ def bimanual_arc_token_shape( m = int(resampled_vector_length) if m < 2: raise ValueError("resampled_vector_length must be at least two") - return m, 20 if streams == "full_separation" else 18 + return m, STREAM_DIMS[streams] -def clock_token_seconds(token, clock_mode): - """Unlog duration targets ONCE. Velocity row zero is already seconds.""" +def stream_distances(waypoints, off, stream): + """Per-segment distance one arm's stream covers between emitted waypoints.""" + if stream == "translation": + return np.linalg.norm(np.diff(waypoints[:, off : off + 3], axis=0), axis=1) + if stream == "rotation": + return np.diff(cumulative_rotation_length(waypoints[:, off + 3 : off + 6])) + return np.abs(np.diff(waypoints[:, off + 6])) + + +def absolute_times_to_seconds(times, distances, max_speed=None): + """Absolute waypoint seconds -> start delay and incoming interval seconds. + + Elmo's speed-bounded isotonic decode (PR #266): project the predicted times + onto the nearest (least-squares) non-decreasing clock whose moving intervals + last at least distance / max_speed, by isotonic regression on the slack above + that fastest clock. A backwards pair is averaged instead of drifting later + rows late, and no local error sums into later rows. Row zero is the start + delay and stays >= 0. Without motion, or without a bound, this is plain + isotonic regression. + """ + t = np.asarray(times, dtype=np.float64) + cum = np.r_[0.0, np.cumsum(np.asarray(distances, dtype=np.float64))] + if t.shape != cum.shape or not np.isfinite(t).all(): + raise ValueError("Absolute times require one finite time per waypoint") + moving = cum[-1] > 0.0 + fastest = cum / max_speed if moving and max_speed else np.zeros_like(cum) + slack = np.maximum(isotonic_regression(t - fastest).x, 0.0) + clock = fastest + slack + return np.r_[clock[0], np.diff(clock)] + + +def clock_token_seconds(token, clock_mode, streams=None): + """Timing rows as seconds: start delay, then incoming intervals. + + Velocity row zero is already seconds. Duration rows are absolute seconds from + the chunk anchor, decoded once by :func:`absolute_times_to_seconds` with the + translation max speed; rotation and gripper clocks are only made monotone. + """ validate_clock_mode(clock_mode) value = np.array(token, dtype=np.float64) - if value.ndim < 2 or value.shape[-1] not in (18, 20): - raise ValueError("Expected an ARC token with 18 or 20 columns") + if value.ndim < 2 or value.shape[-1] not in (16, 18, 20): + raise ValueError("Expected an ARC token with 16, 18 or 20 columns") if not np.isfinite(value).all(): raise ValueError("ARC tokens must be finite") if clock_mode == "duration": - with np.errstate(over="ignore"): - value[..., 14:] = np.maximum(np.exp(value[..., 14:]) - LOG_CLOCK_EPS, 0) - if not np.isfinite(value).all(): - raise ValueError("Decoded ARC clocks must be finite") + streams = streams or {20: "full_separation", 18: "translation_rotation"}.get( + value.shape[-1], "translation" + ) + if STREAM_DIMS[validate_streams(streams)] != value.shape[-1]: + raise ValueError(f"{streams} tokens have {STREAM_DIMS[streams]} columns") + rows = value.reshape(-1, *value.shape[-2:]) + for row in rows: + for index, name in enumerate(STREAM_NAMES[streams]): + for arm, off in enumerate((0, 7)): + col = 14 + 2 * index + arm + row[:, col] = absolute_times_to_seconds( + row[:, col], + stream_distances(row, off, name), + MAX_SPEED if name == "translation" else None, + ) + value = rows.reshape(value.shape) return value def stack_arc_token(token): """Internal geometry/timing view, not a selectable serialized layout.""" value = np.asarray(token, dtype=np.float64) - if value.shape[-1] not in (18, 20): - raise ValueError("Only the explicit 18D/20D ARC contracts are supported") - cols = SIX_CLOCK_COLUMNS if value.shape[-1] == 20 else CLOCK_COLUMNS + if value.shape[-1] not in (16, 18, 20): + raise ValueError("Only the explicit 16D/18D/20D ARC contracts are supported") + cols = {20: SIX_CLOCK_COLUMNS, 18: CLOCK_COLUMNS, 16: SINGLE_CLOCK_COLUMNS}[ + value.shape[-1] + ] timing = np.zeros(value.shape[:-1] + (14,)) timing[..., cols] = value[..., 14:] return np.concatenate((value[..., :14], timing), axis=-2) class TokenizeBimanualArcLengthCartesian: - """First-delay ARC: M x (14 geometry + four or six clocks). + """First-delay ARC: M x (14 geometry + two, four or six clocks). Row zero holds start delay. Rows i>0 describe incoming interval i-1 -> i. Velocity: seconds in row zero, emitted segment distance/second thereafter. - Duration: log(seconds + 0.001) in every timing row. + Duration: absolute seconds from the chunk anchor in every timing row (row + zero is the start delay), decoded by speed-bounded isotonic regression. + ``translation`` gives each arm one clock; rotation and the gripper ride its + supports. There is no automatic interpretation of historical checkpoint layouts. """ @@ -1257,6 +1325,10 @@ def __init__( def M(self): return self.shape[0] + @property + def _stream_names(self): + return STREAM_NAMES[self.streams] + def transform(self, batch): raw = np.asarray(batch[self.action_key], dtype=np.float64) if ( @@ -1282,7 +1354,7 @@ def transform(self, batch): waypoints = self._hybrid_waypoints(raw) clocks = self._stream_targets(raw, waypoints) if self.clock_mode == "duration": - clocks = np.log(np.maximum(clocks, 0) + LOG_CLOCK_EPS) + clocks = np.cumsum(clocks, axis=0) # start delay + intervals -> absolute batch[self.output_action_key] = np.concatenate((waypoints, clocks), axis=1) return batch @@ -1327,7 +1399,7 @@ def _stream_targets(self, raw, waypoints): specs = [ ("translation", t_cum, t_targets, hold is not None), ("rotation", r_cum, r_targets, r_cum[-1] < self.zero_dist_epsilon), - ] + ][: len(self._stream_names)] if gripper is not None: cum, targets, held = gripper[arm] specs.append(("gripper", cum, targets, held)) @@ -1353,20 +1425,13 @@ def _stream_targets(self, raw, waypoints): where=intervals > 1e-12, ) clocks[name, arm] = np.r_[times[0], intervals] - names = ( - ("translation", "rotation", "gripper") - if gripper is not None - else ("translation", "rotation") + return np.stack( + [clocks[name, arm] for name in self._stream_names for arm in (0, 1)], + axis=1, ) - return np.stack([clocks[name, arm] for name in names for arm in (0, 1)], axis=1) - @staticmethod - def _distances(waypoints, off, stream): - if stream == "translation": - return np.linalg.norm(np.diff(waypoints[:, off : off + 3], axis=0), axis=1) - if stream == "rotation": - return np.diff(cumulative_rotation_length(waypoints[:, off + 3 : off + 6])) - return np.abs(np.diff(waypoints[:, off + 6])) + def _distances(self, waypoints, off, stream): + return stream_distances(waypoints, off, stream) def _native(self, token, clocks_in_seconds=False): value = np.asarray(token, dtype=np.float64) @@ -1379,7 +1444,7 @@ def _native(self, token, clocks_in_seconds=False): return ( value.copy() if clocks_in_seconds - else clock_token_seconds(value, self.clock_mode) + else clock_token_seconds(value, self.clock_mode, self.streams) ) def _intervals(self, delta, stored, horizon): @@ -1399,21 +1464,20 @@ def _intervals(self, delta, stored, horizon): def control_steps(self, token, *, max_steps=None, clocks_in_seconds=False): value = self._native(token, clocks_in_seconds) ends = [] - names = ( - ("translation", "rotation", "gripper") - if self.streams == "full_separation" - else ("translation", "rotation") - ) - for index, name in enumerate(names): + for index, name in enumerate(self._stream_names): for arm, off in enumerate((0, 7)): delta = self._distances(value, off, name) moving = delta > 1e-12 if not moving.any(): continue stored = value[1:, 14 + index * 2 + arm] + # Decoded absolute clocks are monotone: a zero interval is a move + # within one tick, not missing timing. usable = ( - stored if self.clock_mode == "duration" else np.abs(stored) - ) > 1e-8 + np.abs(stored) > 1e-8 + if self.clock_mode == "velocity" + else np.ones_like(moving) + ) if np.any(moving & ~usable): ends.append(float("inf")) else: @@ -1442,12 +1506,7 @@ def detokenize(self, arc_actions, action_horizon, *, clocks_in_seconds=False): arms = [] for arm, off in enumerate((0, 7)): indices, clocks = {}, {} - names = ( - ("translation", "rotation", "gripper") - if self.streams == "full_separation" - else ("translation", "rotation") - ) - for index, name in enumerate(names): + for index, name in enumerate(self._stream_names): col = 14 + index * 2 + arm durations = self._intervals( self._distances(value, off, name), value[1:, col], h @@ -1458,7 +1517,7 @@ def detokenize(self, arc_actions, action_horizon, *, clocks_in_seconds=False): lo, hi, a = indices["translation"] xyz = value[:, off : off + 3] xyz = xyz[lo] + a[:, None] * (xyz[hi] - xyz[lo]) - rlo, _, ra = indices["rotation"] + rlo, _, ra = indices.get("rotation", indices["translation"]) ypr = _slerp_segments_ypr(value[:, off + 3 : off + 6], rlo, ra) grip = value[:, off + 6 : off + 7] if self.streams == "full_separation": @@ -1587,14 +1646,26 @@ def _hybrid_waypoints(self, raw: np.ndarray) -> np.ndarray: ) else: grip = np.repeat(raw[0:1, offset + 6 : offset + 7], self.M, axis=0) - # Rotation always keeps its own clock: a translation hold must not - # cut the independent rotation stream short. - ypr = _slerp_segments_ypr( - raw[:, offset + 3 : offset + 6], - rotation_indices, - rotation_alpha, - ) - if rotation_cumulative[-1] < self.tokenizer.config.zero_dist_epsilon: + if "rotation" in self._stream_names: + # Rotation keeps its own clock: a translation hold must not + # cut the independent rotation stream short. + ypr = _slerp_segments_ypr( + raw[:, offset + 3 : offset + 6], + rotation_indices, + rotation_alpha, + ) + still = ( + rotation_cumulative[-1] < self.tokenizer.config.zero_dist_epsilon + ) + else: # one clock per arm: rotation at the translation supports + ypr = _slerp_segments_ypr( + raw[:, offset + 3 : offset + 6], + *_first_crossing_brackets( + translation_cumulative, translation_targets + ), + ) + still = hold is not None + if still: ypr = np.repeat(raw[0:1, offset + 3 : offset + 6], self.M, axis=0) arms.append(np.concatenate([xyz, ypr, grip], axis=-1)) return np.concatenate(arms, axis=-1) diff --git a/egomimic/robot/arc_decoder.py b/egomimic/robot/arc_decoder.py index a3123257f..74be28c3f 100644 --- a/egomimic/robot/arc_decoder.py +++ b/egomimic/robot/arc_decoder.py @@ -300,7 +300,7 @@ def _decode(self, row): (decoded, np.repeat(decoded[-1:], self.action_horizon - steps, axis=0)) ) self.last_stats = { - "replan_steps": steps, + "replan_steps": steps if self.first_stream else None, # 0: replan_every "horizon": self.action_horizon, "execute_waypoints": len(token), } diff --git a/tests/test_arc_chunking_configs.py b/tests/test_arc_chunking_configs.py index c15e3e2e5..970a584a0 100644 --- a/tests/test_arc_chunking_configs.py +++ b/tests/test_arc_chunking_configs.py @@ -170,4 +170,8 @@ def test_retained_recipe_shapes_and_clocks_agree(recipe): contract = cfg.run_provenance.action_contract assert contract.clock_mode == codec.clock_mode assert contract.streams == codec.streams - assert contract.timing_layout == "first_delay_incoming_v1" + assert contract.timing_layout == ( + "first_delay_absolute_v1" + if codec.clock_mode == "duration" + else "first_delay_incoming_v1" + ) diff --git a/tests/test_arc_clock_contract.py b/tests/test_arc_clock_contract.py index 7be8e3efd..b94c429d7 100644 --- a/tests/test_arc_clock_contract.py +++ b/tests/test_arc_clock_contract.py @@ -4,12 +4,15 @@ import pytest from egomimic.rldb.zarr.arc_length_tokenizer import ( - LOG_CLOCK_EPS, + MAX_SPEED, TokenizeBimanualArcLengthCartesian, + absolute_times_to_seconds, bimanual_arc_token_shape, clock_token_seconds, ) +ALL_STREAMS = ["full_separation", "translation_rotation", "translation"] + def codec(clock="velocity", streams="full_separation", **kwargs): options = dict( @@ -28,12 +31,12 @@ def codec(clock="velocity", streams="full_separation", **kwargs): return TokenizeBimanualArcLengthCartesian(**options) -def trajectory(): +def trajectory(xyz_step=0.01): frames = np.arange(30) raw = np.zeros((30, 14)) for col, delay, step in ( - (0, 3, 0.01), - (7, 5, 0.01), + (0, 3, xyz_step), + (7, 5, xyz_step), (3, 4, 0.1), (10, 2, 0.1), (6, 6, 0.1), @@ -45,24 +48,50 @@ def trajectory(): @pytest.mark.parametrize("clock", ["velocity", "duration"]) @pytest.mark.parametrize( - "streams,dim", [("full_separation", 20), ("translation_rotation", 18)] + "streams,dim", + [("full_separation", 20), ("translation_rotation", 18), ("translation", 16)], ) def test_contract_shape_and_first_delay(clock, streams, dim): c = codec(clock, streams) token = c.transform({"actions": trajectory()})["actions"] assert token.shape == bimanual_arc_token_shape(10, clock, streams) == (10, dim) - seconds = clock_token_seconds(token, clock) + seconds = clock_token_seconds(token, clock, streams) expected = [0.3, 0.5, 0.4, 0.2, 0.6, 0.1][: dim - 14] np.testing.assert_allclose(seconds[0, 14:], expected, atol=1e-10) - interval = [0.1, 0.1, 1.0, 1.0] if clock == "velocity" else [0.1] * 4 + interval = ([0.1, 0.1, 1.0, 1.0] if clock == "velocity" else [0.1] * 4)[: dim - 14] np.testing.assert_allclose( seconds[1:, 14:18], np.tile(interval, (9, 1)), atol=1e-10 ) -@pytest.mark.parametrize("streams", ["full_separation", "translation_rotation"]) -def test_velocity_and_duration_reconstruct_same_moving_intervals(streams): +@pytest.mark.parametrize("clock", ["velocity", "duration"]) +def test_one_clock_per_arm_carries_rotation_and_gripper_on_translation(clock): + c = codec(clock, "translation") raw = trajectory() + token = c.transform({"actions": raw})["actions"] + result = c.detokenize(token, 30) + for off in (0, 7): # gripper and each ypr angle follow the translation clock + for col in (off + 3, off + 6): + np.testing.assert_allclose( + result[:, col], + np.interp(result[:, off], token[:, off], token[:, col]), + atol=1e-9, + ) + + +def test_absolute_duration_is_elmos_speed_bounded_isotonic_projection(): + # A backwards pair keeps enough elapsed time to traverse 10 cm at 5 m/s. + seconds = absolute_times_to_seconds([0, 0.1, 0.35, 0.3, 0.4], [0.1] * 4, 5.0) + np.testing.assert_allclose(np.cumsum(seconds), [0, 0.1, 0.315, 0.335, 0.4]) + # Without bounds it is plain isotonic regression; the start delay stays >= 0. + seconds = absolute_times_to_seconds([-0.2, 0.3, 0.1, 0.6], [0.1] * 3) + np.testing.assert_allclose(np.cumsum(seconds), [0, 0.2, 0.2, 0.6]) + + +@pytest.mark.parametrize("streams", ALL_STREAMS) +@pytest.mark.parametrize("xyz_step", [0.01, 0.0005]) # 0.1 and 0.005 m/s (slow pace) +def test_velocity_and_duration_reconstruct_same_moving_intervals(streams, xyz_step): + raw = trajectory(xyz_step) a, b = codec("velocity", streams), codec("duration", streams) va = a.transform({"actions": raw})["actions"] vb = b.transform({"actions": raw})["actions"] @@ -89,13 +118,13 @@ def test_four_stream_gripper_follows_translation_not_an_independent_clock(clock) @pytest.mark.parametrize("clock", ["velocity", "duration"]) -@pytest.mark.parametrize("streams", ["full_separation", "translation_rotation"]) +@pytest.mark.parametrize("streams", ALL_STREAMS) def test_full_hold_has_constant_waypoints_and_configured_duration(clock, streams): c = codec(clock, streams, hold_window_frames=400) raw = np.tile(np.arange(14) * 0.01, (400, 1)) token = c.transform({"actions": raw})["actions"] np.testing.assert_allclose(token[:, :14], np.tile(raw[0], (10, 1))) - timing = clock_token_seconds(token, clock)[:, 14:] + timing = clock_token_seconds(token, clock, streams)[:, 14:] np.testing.assert_allclose(timing[0], 0, atol=1e-12) if clock == "velocity": np.testing.assert_array_equal(timing[1:], 0) @@ -106,7 +135,7 @@ def test_full_hold_has_constant_waypoints_and_configured_duration(clock, streams @pytest.mark.parametrize("clock", ["velocity", "duration"]) -@pytest.mark.parametrize("streams", ["full_separation", "translation_rotation"]) +@pytest.mark.parametrize("streams", ALL_STREAMS) @pytest.mark.parametrize("fraction", [0.5, 1.0]) def test_eval_rollout_parity_all_contracts(clock, streams, fraction): from egomimic.eval.open_loop_sim import OpenLoopSimEval @@ -169,18 +198,17 @@ def test_unknown_streams_rejected(streams): codec(streams=streams) -@pytest.mark.parametrize("clock", ["velocity", "duration"]) -def test_moving_interval_with_unusable_clock_cannot_win_race(clock): - c = codec(clock) +def test_moving_interval_with_unusable_clock_cannot_win_race(): + c = codec("velocity") token = c.transform({"actions": trajectory()})["actions"] - token[1:, 14:] = 0 if clock == "velocity" else np.log(LOG_CLOCK_EPS) + token[1:, 14:] = 0 with pytest.raises(ValueError, match="finite replan"): c.control_steps(token) assert c.control_steps(token, max_steps=30) == 30 @pytest.mark.parametrize("clock", ["velocity", "duration"]) -@pytest.mark.parametrize("streams", ["full_separation", "translation_rotation"]) +@pytest.mark.parametrize("streams", ALL_STREAMS) def test_preserved_ground_truth_is_not_reconstructed_from_tokens(clock, streams): c = codec(clock, streams, preserve_action_key="raw", preserve_action_rows=40) raw = trajectory() @@ -204,7 +232,12 @@ def test_nonfinite_source_and_tokens_rejected(bad): @pytest.mark.parametrize("clock", ["velocity", "duration"]) @pytest.mark.parametrize( - "streams,dim", [("full_separation", 20), ("translation_rotation", 18)] + "streams,dim", + [ + ("full_separation", 20), + ("translation_rotation", 18), + ("translation", 16), + ], ) def test_recipe_data_evaluator_and_declared_rollout_share_contract(clock, streams, dim): from hydra.utils import instantiate @@ -290,3 +323,46 @@ def normalizer(): normalizer().infer_norm_from_dataset([], **options) else: normalizer().infer_norm_from_dataset([], **options) + + +def test_rows_past_the_executed_prefix_never_change_robot_motion(): + """Eval and robot decode only the cropped prefix's clocks, agree on a noisy + predicted token, and never move an arm faster than MAX_SPEED per tick.""" + from egomimic.eval.open_loop_sim import OpenLoopSimEval + from egomimic.robot.arc_decoder import FirstStreamArcDecoder + + dt = 1 / 30 + shared = dict( + clock_mode="duration", + streams="translation", + min_distance_unit=0.22, + rotation_distance_unit=0.3, + resampled_vector_length=200, + gripper_stream_mode="events", + ) + raw = np.zeros((100, 14)) + raw[:, 0] = np.linspace(0, 0.12, 100) + raw[:, 8] = np.linspace(0, 0.08, 100) ** 1.5 + raw[:, 3] = np.linspace(0, 0.4, 100) + c = TokenizeBimanualArcLengthCartesian( + action_key="actions", output_action_key="actions", dt=dt, **shared + ) + token = c.transform({"actions": raw})["actions"] + token[:, 14:16] += np.random.default_rng(0).normal(scale=0.05, size=(200, 2)) + assert (np.diff(token[:, 14:16], axis=0) < 0).any() and (token[:, 14:16] < 0).any() + dec = FirstStreamArcDecoder(**shared, dt=dt, execute_percent=25, action_horizon=100) + ev = OpenLoopSimEval( + **shared, control_dt=dt, action_mode="arc", execute_fraction=0.25 + ) + base = dec(token)[0] + steps = dec.execution_steps() + e_out, e_steps = ev._decode_prediction_with_steps(token, max_steps=100) + assert e_steps == steps + np.testing.assert_allclose(base[:steps], e_out, atol=1e-9) + later = token.copy() + later[50:, 14:16] = -5.0 # every unexecuted waypoint "earlier" than the prefix + np.testing.assert_array_equal(dec(later)[0], base) + assert dec.execution_steps() == steps + for off in (0, 7): + step = np.linalg.norm(np.diff(base[:steps, off : off + 3], axis=0), axis=1) + assert step.max(initial=0) <= MAX_SPEED * dt + 1e-9 diff --git a/tests/test_arc_first_delay.py b/tests/test_arc_first_delay.py index 232866a3f..9e22bcbf1 100644 --- a/tests/test_arc_first_delay.py +++ b/tests/test_arc_first_delay.py @@ -4,7 +4,6 @@ import pytest from egomimic.rldb.zarr.arc_length_tokenizer import ( - LOG_CLOCK_EPS, TokenizeBimanualArcLengthCartesian, clock_token_seconds, ) @@ -59,8 +58,8 @@ def test_no_delay_has_zero_first_clock_and_incoming_intervals(layout): token = encode(codec, delayed_translation(delay=0)) native = clock_token_seconds(token, layout) np.testing.assert_allclose(native[:, 14], [0, 0.1, 0.1, 0.1], atol=1e-12) - if layout == "duration": - assert token[0, 14] == pytest.approx(np.log(LOG_CLOCK_EPS)) + if layout == "duration": # absolute seconds from the anchor + np.testing.assert_allclose(token[:, 14], [0, 0.1, 0.2, 0.3], atol=1e-12) @pytest.mark.parametrize("layout", FIRST_DELAY_LAYOUTS) @@ -113,7 +112,7 @@ def test_partial_waypoint_prefix_retains_delay_and_incoming_interval(layout): np.testing.assert_allclose(decoded, expected, atol=1e-12) -def test_log_clock_is_decoded_exactly_once(): +def test_absolute_clock_is_decoded_exactly_once(): codec = make_codec("duration") token = encode(codec, delayed_translation()) seconds = clock_token_seconds(token, codec.clock_mode) @@ -156,16 +155,25 @@ def test_gripper_can_win_the_six_stream_race(layout): np.testing.assert_array_equal(decoded[5:, 6], 1) -@pytest.mark.parametrize("layout", FIRST_DELAY_LAYOUTS) -def test_only_invalid_moving_stream_requires_a_bound(layout): - codec = make_codec(layout) +def test_only_invalid_moving_stream_requires_a_bound(): + codec = make_codec("velocity") token = encode(codec, delayed_translation()) - token[1:, 14] = 0 if layout == "velocity" else np.log(LOG_CLOCK_EPS) + token[1:, 14] = 0 with pytest.raises(ValueError, match="finite replan"): codec.control_steps(token) assert codec.control_steps(token, max_steps=20) == 20 +def test_collapsed_absolute_clock_still_moves_at_bounded_speed(): + codec = make_codec("duration") + token = encode(codec, delayed_translation()) + token[1:, 14] = 0 # every waypoint predicted at the anchor + seconds = clock_token_seconds(token, "duration")[:, 14] + assert np.all(seconds >= 0) + assert np.all(seconds[1:] >= 0.01 / 5.0 - 1e-12) # 1 cm segments, <= 5 m/s + assert 1 <= codec.control_steps(token) < 20 + + @pytest.mark.parametrize("layout", FIRST_DELAY_LAYOUTS) @pytest.mark.parametrize("fraction", [0.5, 1.0]) def test_validation_and_robot_use_identical_prefix_clocks(layout, fraction): @@ -211,6 +219,13 @@ def test_validation_and_robot_use_identical_prefix_clocks(layout, fraction): ) decoder.first_stream = 0 np.testing.assert_allclose(decoder(token)[0], codec.detokenize(token, 20)) + # Mode 0 defers the replan to the policy's fixed replan_every. + from egomimic.pipeline.action_adapter import execution_prefix + + assert decoder.execution_steps() is None + assert ( + len(execution_prefix(decoder(token)[0], decoder=decoder, replan_every=5)) == 5 + ) @pytest.mark.parametrize("layout", FIRST_DELAY_LAYOUTS) diff --git a/tests/test_time_retimer_controls.py b/tests/test_time_retimer_controls.py new file mode 100644 index 000000000..12d847ec4 --- /dev/null +++ b/tests/test_time_retimer_controls.py @@ -0,0 +1,35 @@ +"""The baseline (time) recipe's deployed decoder and its dashboard speed controls.""" + +import numpy as np +from hydra.utils import instantiate + +from egomimic.pipeline.inference_config import build_inference_config +from scripts.audit_hydra_configs import CONFIGS, compose_for_audit + + +def test_time_recipe_speed_controls_bind_to_the_deployed_retimer_and_act(): + with compose_for_audit( + CONFIGS / "experiment/real/wireless_earbuds/baseline.yaml" + ) as cfg: + profile = build_inference_config(cfg)["inference_graph"]["profiles"]["default"] + decoder = instantiate(profile["adapter"]["decoder"]) + assert type(decoder).__name__ == "TimeChunkRetimer" + targets = { + name: spec["target"]["attribute_path"] + for name, spec in profile["overrides"].items() + if spec["target"]["kind"] == "decoder_attribute" + } + assert set(targets) == {"arc_speed_percent", "arc_hold_speed_percent"} + for name, path in targets.items(): + assert hasattr(decoder, path), (name, path) + chunk = np.zeros((1, 100, 14)) + chunk[0, :, 0] = np.linspace(0, 0.3, 100) # 9 cm/s, well above hold_threshold + np.testing.assert_array_equal(decoder(chunk), chunk) # 100 % is byte-identical + setattr(decoder, targets["arc_speed_percent"], 200) + setattr(decoder, targets["arc_hold_speed_percent"], 200) + out = decoder(chunk)[0] + np.testing.assert_allclose(out[:50, 0], chunk[0, ::2, 0], atol=1e-9) + np.testing.assert_allclose( + out[50:, 0], 0.3 + ) # endpoint held, never extrapolated + assert decoder.last_stats["valid_steps"] == 50