From d22292d72619175d8dd4a279093162ca284bbc42 Mon Sep 17 00:00:00 2001 From: 8x54zj-m <8x54zj-m@motrixlab.local> Date: Wed, 22 Jul 2026 19:37:26 +0800 Subject: [PATCH] Revert "Use integrated progress for terrain curriculum" This reverts commit 7358b31de146570c99591c2fb8c363b3f16e55ef. --- .../motrix_envs/locomotion/go1/dreamwaq.py | 47 +++---------------- .../tests/test_dreamwaq_state_safety.py | 30 +----------- 2 files changed, 7 insertions(+), 70 deletions(-) diff --git a/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py b/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py index 7e2f783..f693ab2 100644 --- a/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py +++ b/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py @@ -43,19 +43,6 @@ def _sanitize_dof_pos(dof_pos: np.ndarray) -> np.ndarray: return clean -def _command_aligned_progress( - local_vel: np.ndarray, commands: np.ndarray, dt: float -) -> np.ndarray: - """Return signed displacement along each environment's current command.""" - cmd_speed = np.linalg.norm(commands, axis=1) - cmd_dir = np.zeros_like(commands) - moving = cmd_speed > 0.1 - cmd_dir[moving] = commands[moving] / cmd_speed[moving, np.newaxis] - aligned_speed = np.sum(local_vel * cmd_dir, axis=1) - aligned_speed = np.nan_to_num(aligned_speed, nan=0.0, posinf=0.0, neginf=0.0) - return np.clip(aligned_speed, -3.0, 3.0) * dt - - def _scene_file(): """选择地形场景。DREAMWAQ_TERRAIN=flat|pyramid|flat_stairs。默认 FLAT。""" global _SCENE_PRINTED @@ -446,21 +433,13 @@ class DreamWaQTask(Go1WalkTask): (self._num_envs, self.foot_check_num)) state.info["feet_air_time"] = self.update_feet_air_time(state.info) - # Accumulate commanded distance, actual progress, path length, and tracking. + # 累计命令距离和跟踪 cmd_speed = np.linalg.norm(state.info["commands"][:, :2], axis=1) state.info["ep_cmd_distance"] = state.info.get("ep_cmd_distance", np.zeros(self._num_envs, dtype=np.float32)) + cmd_speed * self.cfg.ctrl_dt state.info["ep_steps"] = state.info.get("ep_steps", np.zeros(self._num_envs, dtype=np.int32)) + 1 local_vel = self.get_local_linvel(data)[:, :2] cmd_vel = state.info["commands"][:, :2] - state.info["ep_command_progress"] = state.info.get( - "ep_command_progress", np.zeros(self._num_envs, dtype=np.float32) - ) + _command_aligned_progress(local_vel, cmd_vel, self.cfg.ctrl_dt) - path_speed = np.linalg.norm(local_vel, axis=1) - path_delta = np.nan_to_num(path_speed, nan=0.0, posinf=0.0, neginf=0.0) - state.info["ep_path_distance"] = state.info.get( - "ep_path_distance", np.zeros(self._num_envs, dtype=np.float32) - ) + np.clip(path_delta, 0.0, 3.0) * self.cfg.ctrl_dt vel_error = np.sum(np.square(cmd_vel - local_vel), axis=1) tracking = np.exp(-vel_error / self.cfg.reward_config.tracking_sigma) state.info["ep_tracking_sum"] = state.info.get("ep_tracking_sum", @@ -522,12 +501,6 @@ class DreamWaQTask(Go1WalkTask): base_pose = self._body.get_pose(state.data) base_pos = base_pose[done, :2] distance = np.linalg.norm(base_pos - old_origins, axis=1) - command_progress = old_info.get( - "ep_command_progress", np.zeros(self._num_envs, dtype=np.float32) - )[done] - path_distance = old_info.get( - "ep_path_distance", np.zeros(self._num_envs, dtype=np.float32) - )[done] # ── 多条件 Curriculum 升级 ── # 1) 速度跟踪: exp(-L2²/σ), σ=0.25, >0.5 表示等效恒定 L2 误差约 0.42m/s ep_tracking = old_info.get("ep_tracking_sum", np.zeros(self._num_envs, dtype=np.float32))[done] @@ -555,16 +528,15 @@ class DreamWaQTask(Go1WalkTask): ep_bh = old_info.get("ep_base_height", np.zeros(self._num_envs, dtype=np.float32))[done] avg_bh = np.zeros(num_reset, dtype=np.float32) avg_bh[mask] = ep_bh[mask] / ep_steps[mask].astype(np.float32) - # Commands resample during an episode, so endpoint displacement can - # cancel valid traversal. Signed command-aligned progress cannot be - # inflated by lateral jitter or reversing against the command. - progress_ok = command_progress > self._cell_size / 2.0 + # Require actual traversal of most of the current terrain cell. + # Tracking velocity while staying near the spawn point must not + # promote the curriculum. move_up_raw = ( (avg_tracking > 0.5) & not_fallen & (avg_orient > -0.0005) & (avg_bh > -0.001) - & progress_ok + & (distance > self._cell_size / 2.0) ) # ── 渐进升级：连续通过 3 次 + 冷却期 + 每次只升 1 级 ── if not hasattr(self, '_consecutive_pass_count'): @@ -616,7 +588,6 @@ 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_endpoint_ok = 0; self._curric_path_ok = 0 self._curric_total = 0 self._curric_log_counter += 1 self._curric_track_ok += int((avg_tracking > 0.5).sum()) @@ -630,9 +601,7 @@ class DreamWaQTask(Go1WalkTask): self._curric_down_ok += int(move_down.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(progress_ok.sum()) - self._curric_endpoint_ok += int((distance > self._cell_size / 2.0).sum()) - self._curric_path_ok += int((path_distance > self._cell_size / 2.0).sum()) + self._curric_progress_ok += int((distance > self._cell_size / 2.0).sum()) self._curric_total += num_reset if self._curric_log_counter % 50 == 0: def pct(n): return 100*n/max(self._curric_total,1) @@ -649,8 +618,6 @@ class DreamWaQTask(Go1WalkTask): f"downT={pct(self._curric_down_track):.0f}% " f"downS={pct(self._curric_down_stability):.0f}% " f"progress={pct(self._curric_progress_ok):.0f}% " - f"endpoint={pct(self._curric_endpoint_ok):.0f}% " - f"path={pct(self._curric_path_ok):.0f}% " f"max_init={self._max_init_level}") else: new_levels = np.random.randint(0, self._max_init_level + 1, size=num_reset, dtype=np.int32) @@ -728,8 +695,6 @@ class DreamWaQTask(Go1WalkTask): "com_displacement": np.random.uniform(-0.05, 0.05, size=(num_reset, 3)).astype(np.float32), "last_rand_step": np.zeros(num_reset, dtype=np.int32), "ep_cmd_distance": np.zeros(num_reset, dtype=np.float32), - "ep_command_progress": np.zeros(num_reset, dtype=np.float32), - "ep_path_distance": np.zeros(num_reset, dtype=np.float32), "ep_steps": np.zeros(num_reset, dtype=np.int32), "ep_tracking_sum": np.zeros(num_reset, dtype=np.float32), } diff --git a/motrix_envs/tests/test_dreamwaq_state_safety.py b/motrix_envs/tests/test_dreamwaq_state_safety.py index 508e305..0b4c7b7 100644 --- a/motrix_envs/tests/test_dreamwaq_state_safety.py +++ b/motrix_envs/tests/test_dreamwaq_state_safety.py @@ -2,12 +2,7 @@ from types import SimpleNamespace import numpy as np -from motrix_envs.locomotion.go1.dreamwaq import ( - DreamWaQCfg, - DreamWaQTask, - _command_aligned_progress, - _sanitize_dof_pos, -) +from motrix_envs.locomotion.go1.dreamwaq import DreamWaQCfg, DreamWaQTask, _sanitize_dof_pos from motrix_envs.locomotion.go1.walk_np import Go1WalkTask @@ -27,29 +22,6 @@ 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)