Require consecutive failures for terrain downgrade

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,
+    _advance_streak,
+    _sanitize_dof_pos,
+)
 from motrix_envs.locomotion.go1.walk_np import Go1WalkTask
 
 
@@ -22,6 +27,16 @@ def test_sanitize_dof_pos_handles_zero_quaternion_with_nonfinite_joints():
     assert clean[1, 8] == 0.0
 
 
+def test_advance_streak_requires_consecutive_failures():
+    streak = np.array([0, 1, 3], dtype=np.int32)
+
+    streak = _advance_streak(streak, np.array([True, True, False]))
+    np.testing.assert_array_equal(streak, [1, 2, 0])
+
+    streak = _advance_streak(streak, np.array([False, True, True]))
+    np.testing.assert_array_equal(streak, [0, 3, 1])
+
+
 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)