Fix terrain curriculum traversal and hfield roughness

motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py

@@ -279,6 +279,25 @@ class DreamWaQTask(Go1WalkTask):
 
     # ── 高度测量 ──
 
+    def _ensure_hfield_cache(self) -> bool:
+        """Load hfield metadata before reset-time terrain height queries."""
+        if self._hm_cache is not None:
+            return True
+        try:
+            hf = self._model.get_hfield(0)
+            self._hf_cache = hf
+            self._hm_cache = hf.height_matrix
+            nr, nc = self._hm_cache.shape
+            b = hf.bound
+            self._h_xmin, self._h_ymin = b[0], b[1]
+            self._h_xmax, self._h_ymax = b[3], b[4]
+            self._h_nr, self._h_nc = nr, nc
+            cfg_base = getattr(self.cfg, "hfield_z_base", None)
+            self._h_zbase = float(cfg_base) if cfg_base is not None else float(-self._hm_cache[0, 0])
+            return True
+        except Exception:
+            return False
+
     def _get_heights(self, data: mtx.SceneData) -> np.ndarray:
         n = data.shape[0]
         nx, ny = len(self._hx), len(self._hy)
@@ -286,25 +305,12 @@ class DreamWaQTask(Go1WalkTask):
         base_pos = pose[:, 0:3]
         yaw = quaternion.get_yaw(pose[:, 3:7])
         cos_yaw, sin_yaw = np.cos(yaw), np.sin(yaw)
-        try:
-            if self._hm_cache is None:
-                hf = self._model.get_hfield(0)
-                self._hf_cache = hf
-                hm = hf.height_matrix
-                self._hm_cache = hm
-                nr, nc = hm.shape
-                b = hf.bound
-                self._h_xmin, self._h_ymin = b[0], b[1]
-                self._h_xmax, self._h_ymax = b[3], b[4]
-                self._h_nr, self._h_nc = nr, nc
-                cfg_base = getattr(self.cfg, "hfield_z_base", None)
-                self._h_zbase = float(cfg_base) if cfg_base is not None else float(-hm[0, 0])
-            hm, nr, nc = self._hm_cache, self._h_nr, self._h_nc
-            xmin, ymin, xmax, ymax = self._h_xmin, self._h_ymin, self._h_xmax, self._h_ymax
-            z_base = self._h_zbase
-            w, h = xmax - xmin, ymax - ymin
-        except Exception:
+        if not self._ensure_hfield_cache():
             return np.zeros((n, nx * ny), dtype=np.float32)
+        hm, nr, nc = self._hm_cache, self._h_nr, self._h_nc
+        xmin, ymin, xmax, ymax = self._h_xmin, self._h_ymin, self._h_xmax, self._h_ymax
+        z_base = self._h_zbase
+        w, h = xmax - xmin, ymax - ymin
 
         heights = np.zeros((n, nx * ny), dtype=np.float32)
         idx = 0
@@ -324,13 +330,12 @@ class DreamWaQTask(Go1WalkTask):
 
     def _sample_terrain_height(self, xy: np.ndarray, radius: float = 0.0) -> np.ndarray:
         n = xy.shape[0]
-        try:
-            hm, nr, nc = self._hm_cache, self._h_nr, self._h_nc
-            xmin, ymin, xmax, ymax = self._h_xmin, self._h_ymin, self._h_xmax, self._h_ymax
-            z_base = self._h_zbase
-            w, h = xmax - xmin, ymax - ymin
-        except Exception:
+        if not self._ensure_hfield_cache():
             return np.zeros(n, dtype=np.float32)
+        hm, nr, nc = self._hm_cache, self._h_nr, self._h_nc
+        xmin, ymin, xmax, ymax = self._h_xmin, self._h_ymin, self._h_xmax, self._h_ymax
+        z_base = self._h_zbase
+        w, h = xmax - xmin, ymax - ymin
         col = np.clip(np.nan_to_num((xy[:, 0] - xmin) / max(w, 1e-6) * (nc - 1), nan=0).astype(np.int32), 0, nc - 1)
         row = np.clip(np.nan_to_num((ymax - xy[:, 1]) / max(h, 1e-6) * (nr - 1), nan=0).astype(np.int32), 0, nr - 1)
         if radius <= 0:
@@ -523,12 +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)
-            # orient: -0.5*dt*sin²(theta); bh: -10*dt*z_err²
+            # 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)
+                & (distance > self._cell_size / 2.0)
             )
             # ── 渐进升级：连续通过 3 次 + 冷却期 + 每次只升 1 级 ──
             if not hasattr(self, '_consecutive_pass_count'):
@@ -536,9 +544,10 @@ class DreamWaQTask(Go1WalkTask):
                 self._pending_upgrade = np.zeros(self._num_envs, dtype=bool)
                 self._upgrade_cooldown = np.zeros(self._num_envs, dtype=np.int32)
 
-            # 每 env: 连续通过 +1, 失败重置（仅更新被 reset 的 env）
+            # Cooldown episodes do not contribute to a new validation streak.
+            cooldown_active = self._upgrade_cooldown[done_idx] > 0
             self._consecutive_pass_count[done_idx] = np.where(
-                move_up_raw,
+                move_up_raw & ~cooldown_active,
                 self._consecutive_pass_count[done_idx] + 1, 0)
             # 冷却期以该 env 自己完成的 episode 为单位，而不是全局 reset 批次数。
             self._upgrade_cooldown[done_idx] = np.maximum(0, self._upgrade_cooldown[done_idx] - 1)
@@ -578,7 +587,8 @@ class DreamWaQTask(Go1WalkTask):
                 self._curric_raw_all_ok = 0; self._curric_streak_ok = 0
                 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_total = 0
+                self._curric_down_stability = 0; self._curric_progress_ok = 0
+                self._curric_total = 0
             self._curric_log_counter += 1
             self._curric_track_ok += int((avg_tracking > 0.5).sum())
             self._curric_survive_ok += int(not_fallen.sum())
@@ -591,6 +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((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)
@@ -606,6 +617,7 @@ class DreamWaQTask(Go1WalkTask):
                       f"down={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}% "
                       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)

motrix_envs/src/motrix_envs/locomotion/go1/xmls/assets/dreamwaq_terrain.png

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:5fe0435e385f736b46a910b53d30135d7f6280c0d8daa2bd2f64b7df7962d998
-size 1439313
+oid sha256:a8289226407a014f04e4d5e9d7f526c2c154d50d1fe91865fce3451c212b31cb
+size 912324