diff --git a/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py b/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py index f693ab2..7e2f783 100644 --- a/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py +++ b/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py @@ -43,6 +43,19 @@ 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 @@ -433,13 +446,21 @@ 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", @@ -501,6 +522,12 @@ 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] @@ -528,15 +555,16 @@ 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) - # Require actual traversal of most of the current terrain cell. - # Tracking velocity while staying near the spawn point must not - # promote the curriculum. + # 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 move_up_raw = ( (avg_tracking > 0.5) & not_fallen & (avg_orient > -0.0005) & (avg_bh > -0.001) - & (distance > self._cell_size / 2.0) + & progress_ok ) # ── 渐进升级：连续通过 3 次 + 冷却期 + 每次只升 1 级 ── if not hasattr(self, '_consecutive_pass_count'): @@ -588,6 +616,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_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()) @@ -601,7 +630,9 @@ 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((distance > self._cell_size / 2.0).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_total += num_reset if self._curric_log_counter % 50 == 0: def pct(n): return 100*n/max(self._curric_total,1) @@ -618,6 +649,8 @@ 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) @@ -695,6 +728,8 @@ 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 0b4c7b7..508e305 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, + _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)