Use integrated progress for terrain curriculum

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.
+            # Commands resample during an episode, so endpoint displacement can
-            # Tracking velocity while staying near the spawn point must not
+            # cancel valid traversal. Signed command-aligned progress cannot be
-            # promote the curriculum.
+            # 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),
         }

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)