diff --git a/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py b/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py index f693ab2..645c043 100644 --- a/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py +++ b/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py @@ -43,6 +43,11 @@ def _sanitize_dof_pos(dof_pos: np.ndarray) -> np.ndarray: return clean +def _advance_streak(streak: np.ndarray, condition: np.ndarray) -> np.ndarray: + """Increment streaks where condition holds and reset the rest.""" + return np.where(condition, streak + 1, 0) + + def _scene_file(): """选择地形场景。DREAMWAQ_TERRAIN=flat|pyramid|flat_stairs。默认 FLAT。""" global _SCENE_PRINTED @@ -543,6 +548,8 @@ class DreamWaQTask(Go1WalkTask): self._consecutive_pass_count = np.zeros(self._num_envs, dtype=np.int32) self._pending_upgrade = np.zeros(self._num_envs, dtype=bool) self._upgrade_cooldown = np.zeros(self._num_envs, dtype=np.int32) + if not hasattr(self, '_consecutive_fail_count'): + self._consecutive_fail_count = np.zeros(self._num_envs, dtype=np.int32) # Cooldown episodes do not contribute to a new validation streak. cooldown_active = self._upgrade_cooldown[done_idx] > 0 @@ -568,7 +575,14 @@ class DreamWaQTask(Go1WalkTask): | (avg_orient <= -0.001) | (avg_bh <= -0.002) ) - move_down = move_down_raw & ~move_up + self._consecutive_fail_count[done_idx] = _advance_streak( + self._consecutive_fail_count[done_idx], move_down_raw + ) + move_down_ready = self._consecutive_fail_count[done_idx] >= 2 + move_down = move_down_ready & ~move_up + self._consecutive_fail_count[done_idx] = np.where( + move_down, 0, self._consecutive_fail_count[done_idx] + ) new_levels = np.where(move_up, old_levels + 1, old_levels) new_levels = np.where(move_down, new_levels - 1, new_levels) new_levels = np.clip(new_levels, 0, self._num_rows - 1) @@ -588,6 +602,7 @@ class DreamWaQTask(Go1WalkTask): self._curric_upgrade_ok = 0; self._curric_cooldown_block = 0 self._curric_down_ok = 0; self._curric_down_track = 0 self._curric_down_stability = 0; self._curric_progress_ok = 0 + self._curric_down_raw = 0 self._curric_total = 0 self._curric_log_counter += 1 self._curric_track_ok += int((avg_tracking > 0.5).sum()) @@ -599,6 +614,7 @@ class DreamWaQTask(Go1WalkTask): self._curric_upgrade_ok += int(move_up.sum()) self._curric_cooldown_block += int(cooldown_blocked.sum()) self._curric_down_ok += int(move_down.sum()) + self._curric_down_raw += int(move_down_raw.sum()) self._curric_down_track += int((avg_tracking < 0.35).sum()) self._curric_down_stability += int((~not_fallen | (avg_orient <= -0.001) | (avg_bh <= -0.002)).sum()) self._curric_progress_ok += int((distance > self._cell_size / 2.0).sum()) @@ -614,7 +630,8 @@ class DreamWaQTask(Go1WalkTask): f"streak3={pct(self._curric_streak_ok):.0f}% " f"up={pct(self._curric_upgrade_ok):.0f}% " f"cool={pct(self._curric_cooldown_block):.0f}% " - f"down={pct(self._curric_down_ok):.0f}% " + f"downRaw={pct(self._curric_down_raw):.0f}% " + f"down2={pct(self._curric_down_ok):.0f}% " f"downT={pct(self._curric_down_track):.0f}% " f"downS={pct(self._curric_down_stability):.0f}% " f"progress={pct(self._curric_progress_ok):.0f}% " diff --git a/motrix_envs/tests/test_dreamwaq_state_safety.py b/motrix_envs/tests/test_dreamwaq_state_safety.py index 0b4c7b7..3117c35 100644 --- a/motrix_envs/tests/test_dreamwaq_state_safety.py +++ b/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)