From bb2c352d593553f7fa414dcab330bbaa717e8d64 Mon Sep 17 00:00:00 2001 From: shobhitagnihotri69 Date: Wed, 7 Oct 2026 09:35:18 +0530 Subject: [PATCH 1/3] fix(checks): duration-weight final_pose_speed and retain on sparse streams --- src/hflow/checks.py | 31 +++++++++++++++++++++---------- tests/test_checks.py | 44 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 65 insertions(+), 10 deletions(-) diff --git a/src/hflow/checks.py b/src/hflow/checks.py index bb9a4fe9..aef1709a 100644 --- a/src/hflow/checks.py +++ b/src/hflow/checks.py @@ -1420,17 +1420,28 @@ def _measure_trajectory_metrics( # Was the arm still moving when recording stopped? A high ratio means the # episode was cut mid-motion, which matters for anything learning an - # end-of-task pose. - final_window_mask = measured & ( - (profile.stamps_ns[-1] - profile.stamps_ns[:-1]) / 1e9 <= final_pose_window_s - ) + # end-of-task pose. Steps ending in the window represent the settling motion; + # weighted by duration so irregular sampling does not skew it. + step_ends_ns = profile.stamps_ns[1:] + window_start_ns = profile.stamps_ns[-1] - int(final_pose_window_s * 1e9) + final_window_mask = measured & (step_ends_ns >= window_start_ns) + if not np.any(final_window_mask) and np.any(measured): + last_measured = int(np.flatnonzero(measured)[-1]) + final_window_mask = np.zeros_like(measured) + final_window_mask[last_measured] = True + if np.any(final_window_mask): - final_speed = float(np.mean(profile.speeds[final_window_mask])) - measurements[f"{topic}/final_pose_speed"] = final_speed - if mean_velocity > 0: - # Omitted when mean_velocity == 0.0: the ratio is undefined for a - # fully motionless episode. - measurements[f"{topic}/final_pose_unsettled_ratio"] = final_speed / mean_velocity + final_durations_s = profile.step_durations_s[final_window_mask] + final_valid_s = float(np.sum(final_durations_s)) + if final_valid_s > 0: + final_speed = float( + np.sum(profile.speeds[final_window_mask] * final_durations_s) / final_valid_s + ) + measurements[f"{topic}/final_pose_speed"] = final_speed + if mean_velocity > 0: + # Omitted when mean_velocity == 0.0: the ratio is undefined for a + # fully motionless episode. + measurements[f"{topic}/final_pose_unsettled_ratio"] = final_speed / mean_velocity return CheckResult(measurements=measurements) diff --git a/tests/test_checks.py b/tests/test_checks.py index 2f26b7f0..435a11b8 100644 --- a/tests/test_checks.py +++ b/tests/test_checks.py @@ -1240,6 +1240,50 @@ def test_trajectory_metrics_omits_unsettled_ratio_for_a_motionless_episode( assert "/joint_states/final_pose_unsettled_ratio" not in result.measurements +def test_trajectory_metrics_emits_final_pose_speed_for_low_frequency_stream( + tmp_path: Path, +) -> None: + """A stream with sample period exceeding final_pose_window_s (e.g. 1 Hz) + must still emit final_pose_speed and final_pose_unsettled_ratio for the final step. + """ + source = synthesize_episode( + tmp_path / "low_freq.mcap", + SyntheticEpisodeSpec( + duration_s=5.0, + cameras=(), + joint_hz=1.0, + joint_jump_at_s=None, + ), + ) + with hflow.Episode(source) as episode: + result = asyncio.run(trajectory_metrics(episode)) + + speed = result.measurements.get("/joint_states/final_pose_speed") + assert isinstance(speed, float) and speed > 0 + ratio = result.measurements.get("/joint_states/final_pose_unsettled_ratio") + assert isinstance(ratio, float) and ratio > 0 + + +def test_trajectory_metrics_time_weights_final_pose_speed_identically_to_mean_velocity( + tmp_path: Path, +) -> None: + """When the final window spans the full episode, duration-weighting guarantees + final_pose_speed matches mean_velocity identically, so unsettled ratio is 1.0. + """ + source = synthesize_episode( + tmp_path / "short.mcap", + SyntheticEpisodeSpec( + duration_s=0.4, + cameras=(), + joint_jump_at_s=None, + ), + ) + with hflow.Episode(source) as episode: + result = asyncio.run(trajectory_metrics(episode, final_pose_window_s=0.5)) + + assert result.measurements["/joint_states/final_pose_unsettled_ratio"] == pytest.approx(1.0) + + @pytest.fixture(scope="module") def camera_less_episode(tmp_path_factory: pytest.TempPathFactory) -> Path: """An episode with no camera topics. From f9a671e62ae5b1fa0edfcc24825feca9aba0216a Mon Sep 17 00:00:00 2001 From: shobhitagnihotri69 Date: Wed, 7 Oct 2026 11:47:35 +0530 Subject: [PATCH 2/3] refactor(checks): weight final_pose_speed by window overlap duration --- src/hflow/checks.py | 21 +++++++++++++-------- tests/test_checks.py | 30 ++++++++++++++++++++++++++++++ 2 files changed, 43 insertions(+), 8 deletions(-) diff --git a/src/hflow/checks.py b/src/hflow/checks.py index aef1709a..0a2999d3 100644 --- a/src/hflow/checks.py +++ b/src/hflow/checks.py @@ -1420,22 +1420,27 @@ def _measure_trajectory_metrics( # Was the arm still moving when recording stopped? A high ratio means the # episode was cut mid-motion, which matters for anything learning an - # end-of-task pose. Steps ending in the window represent the settling motion; - # weighted by duration so irregular sampling does not skew it. - step_ends_ns = profile.stamps_ns[1:] - window_start_ns = profile.stamps_ns[-1] - int(final_pose_window_s * 1e9) - final_window_mask = measured & (step_ends_ns >= window_start_ns) + # end-of-task pose. Weighted by each step's overlap with the final window, + # so irregular sampling or a boundary-crossing step does not skew it. + final_window_start_ns = profile.stamps_ns[-1] - int(final_pose_window_s * 1e9) + final_overlap_s = ( + np.minimum(profile.stamps_ns[1:], profile.stamps_ns[-1]) + - np.maximum(profile.stamps_ns[:-1], final_window_start_ns) + ) / 1e9 + final_window_mask = measured & (final_overlap_s > 0) if not np.any(final_window_mask) and np.any(measured): last_measured = int(np.flatnonzero(measured)[-1]) final_window_mask = np.zeros_like(measured) final_window_mask[last_measured] = True + final_weights_s = profile.step_durations_s[final_window_mask] + else: + final_weights_s = final_overlap_s[final_window_mask] if np.any(final_window_mask): - final_durations_s = profile.step_durations_s[final_window_mask] - final_valid_s = float(np.sum(final_durations_s)) + final_valid_s = float(np.sum(final_weights_s)) if final_valid_s > 0: final_speed = float( - np.sum(profile.speeds[final_window_mask] * final_durations_s) / final_valid_s + np.sum(profile.speeds[final_window_mask] * final_weights_s) / final_valid_s ) measurements[f"{topic}/final_pose_speed"] = final_speed if mean_velocity > 0: diff --git a/tests/test_checks.py b/tests/test_checks.py index 435a11b8..4aff78f5 100644 --- a/tests/test_checks.py +++ b/tests/test_checks.py @@ -1284,6 +1284,36 @@ def test_trajectory_metrics_time_weights_final_pose_speed_identically_to_mean_ve assert result.measurements["/joint_states/final_pose_unsettled_ratio"] == pytest.approx(1.0) +def test_trajectory_metrics_weights_by_window_overlap(monkeypatch: pytest.MonkeyPatch) -> None: + """Steps crossing the window boundary are weighted by their overlap with the window.""" + from unittest.mock import MagicMock + + import hflow.checks as checks_module + + # Window is 0.5s (t in [1.5s, 2.0s]). + # Step 0: [0.0s, 1.7s], speed 1.0 -> overlaps [1.5s, 1.7s] for 0.2s + # Step 1: [1.7s, 1.9s], speed 10.0 -> overlaps [1.7s, 1.9s] for 0.2s + # Step 2: [1.9s, 2.0s], speed 100.0 -> overlaps [1.9s, 2.0s] for 0.1s + # Total overlap: 0.2 + 0.2 + 0.1 = 0.5s. + # Weighted speed: (1.0*0.2 + 10.0*0.2 + 100.0*0.1) / 0.5 = 24.4 + profile = checks_module._TrajectoryProfile( + stamps_ns=np.array([0, 1_700_000_000, 1_900_000_000, 2_000_000_000]), + step_durations_s=np.array([1.7, 0.2, 0.1]), + speeds=np.array([1.0, 10.0, 100.0]), + curvatures=np.empty(0), + non_finite_sample_count=0, + scale_source="raw", + ) + monkeypatch.setattr( + checks_module, + "_trajectory_profile", + lambda episode, topic, field, dimension_scales: profile, + ) + mock_ep = MagicMock(spec=hflow.Episode) + result = checks_module._measure_trajectory_metrics(mock_ep, final_pose_window_s=0.5) + assert result.measurements["/joint_states/final_pose_speed"] == pytest.approx(24.4) + + @pytest.fixture(scope="module") def camera_less_episode(tmp_path_factory: pytest.TempPathFactory) -> Path: """An episode with no camera topics. From a12f142de8b2f0847012356a7d139f50917d8801 Mon Sep 17 00:00:00 2001 From: shobhitagnihotri69 Date: Wed, 7 Oct 2026 13:22:00 +0530 Subject: [PATCH 3/3] fix(checks): omit final_pose_speed when final window is unmeasurable --- src/hflow/checks.py | 9 +-------- tests/test_checks.py | 35 +++++++++++++++++++++++++++++++++++ 2 files changed, 36 insertions(+), 8 deletions(-) diff --git a/src/hflow/checks.py b/src/hflow/checks.py index 0a2999d3..8b307b5b 100644 --- a/src/hflow/checks.py +++ b/src/hflow/checks.py @@ -1428,15 +1428,8 @@ def _measure_trajectory_metrics( - np.maximum(profile.stamps_ns[:-1], final_window_start_ns) ) / 1e9 final_window_mask = measured & (final_overlap_s > 0) - if not np.any(final_window_mask) and np.any(measured): - last_measured = int(np.flatnonzero(measured)[-1]) - final_window_mask = np.zeros_like(measured) - final_window_mask[last_measured] = True - final_weights_s = profile.step_durations_s[final_window_mask] - else: - final_weights_s = final_overlap_s[final_window_mask] - if np.any(final_window_mask): + final_weights_s = final_overlap_s[final_window_mask] final_valid_s = float(np.sum(final_weights_s)) if final_valid_s > 0: final_speed = float( diff --git a/tests/test_checks.py b/tests/test_checks.py index 4aff78f5..625a8ce7 100644 --- a/tests/test_checks.py +++ b/tests/test_checks.py @@ -1314,6 +1314,41 @@ def test_trajectory_metrics_weights_by_window_overlap(monkeypatch: pytest.Monkey assert result.measurements["/joint_states/final_pose_speed"] == pytest.approx(24.4) +def test_trajectory_metrics_omits_final_pose_speed_when_final_window_is_unmeasurable( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """When all steps overlapping the final window are non-finite (unmeasurable), + final_pose_speed and final_pose_unsettled_ratio must be omitted rather than + falling back to earlier steps in the episode. + """ + from unittest.mock import MagicMock + + import hflow.checks as checks_module + + # Window is 0.5s (t in [1.5s, 2.0s]). + # Step 0: [0.0s, 1.4s], speed 1.0 (measurable, ends before window starts at 1.5s) + # Step 1: [1.4s, 2.0s], speed NaN (overlaps window [1.5s, 2.0s], but non-finite) + profile = checks_module._TrajectoryProfile( + stamps_ns=np.array([0, 1_400_000_000, 2_000_000_000]), + step_durations_s=np.array([1.4, 0.6]), + speeds=np.array([1.0, np.nan]), + curvatures=np.empty(0), + non_finite_sample_count=1, + scale_source="raw", + ) + monkeypatch.setattr( + checks_module, + "_trajectory_profile", + lambda episode, topic, field, dimension_scales: profile, + ) + mock_ep = MagicMock(spec=hflow.Episode) + result = checks_module._measure_trajectory_metrics(mock_ep, final_pose_window_s=0.5) + assert "/joint_states/final_pose_speed" not in result.measurements + assert "/joint_states/final_pose_unsettled_ratio" not in result.measurements + mean = result.measurements.get("/joint_states/mean_velocity") + assert isinstance(mean, float) and mean > 0 + + @pytest.fixture(scope="module") def camera_less_episode(tmp_path_factory: pytest.TempPathFactory) -> Path: """An episode with no camera topics.