Use integrated progress for terrain curriculum

motrix_envs/tests/test_dreamwaq_state_safety.py

@@ -2,7 +2,12 @@ from types import SimpleNamespace
 
 import numpy as np
 
-from motrix_envs.locomotion.go1.dreamwaq import DreamWaQCfg, DreamWaQTask, _sanitize_dof_pos
+from motrix_envs.locomotion.go1.dreamwaq import (
+    DreamWaQCfg,
+    DreamWaQTask,
+    _command_aligned_progress,
+    _sanitize_dof_pos,
+)
 from motrix_envs.locomotion.go1.walk_np import Go1WalkTask
 
 
@@ -22,6 +27,29 @@ def test_sanitize_dof_pos_handles_zero_quaternion_with_nonfinite_joints():
     assert clean[1, 8] == 0.0
 
 
+def test_command_aligned_progress_follows_current_command_without_jitter_credit():
+    local_vel = np.array(
+        [[1.0, 0.0], [-1.0, 0.0], [0.0, 1.0], [-0.5, 0.0]], dtype=np.float32
+    )
+    commands = np.array(
+        [[1.0, 0.0], [-1.0, 0.0], [1.0, 0.0], [1.0, 0.0]], dtype=np.float32
+    )
+
+    progress = _command_aligned_progress(local_vel, commands, dt=0.02)
+
+    np.testing.assert_allclose(progress, [0.02, 0.02, 0.0, -0.01])
+
+
+def test_command_aligned_progress_ignores_standing_commands():
+    progress = _command_aligned_progress(
+        np.array([[2.0, 0.0]], dtype=np.float32),
+        np.array([[0.05, 0.0]], dtype=np.float32),
+        dt=0.02,
+    )
+
+    np.testing.assert_array_equal(progress, [0.0])
+
+
 def test_apply_action_advances_three_frame_action_history():
     task = Go1WalkTask.__new__(Go1WalkTask)
     task.get_dof_vel = lambda data: np.ones((1, 12), dtype=np.float32)