From 913d5061ff548048ec1fcf0d14f32af9d0c4a632 Mon Sep 17 00:00:00 2001 From: 8x54zj-m <8x54zj-m@motrixlab.local> Date: Wed, 22 Jul 2026 16:46:32 +0800 Subject: [PATCH] Fix terrain curriculum traversal and hfield roughness --- .../motrix_envs/locomotion/go1/dreamwaq.py | 68 +++++++++++-------- .../go1/xmls/assets/dreamwaq_terrain.png | 4 +- scripts/gen_dreamwaq_terrain.py | 22 ++++-- 3 files changed, 58 insertions(+), 36 deletions(-) diff --git a/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py b/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py index 9b6ae91..f693ab2 100644 --- a/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py +++ b/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) diff --git a/motrix_envs/src/motrix_envs/locomotion/go1/xmls/assets/dreamwaq_terrain.png b/motrix_envs/src/motrix_envs/locomotion/go1/xmls/assets/dreamwaq_terrain.png index c6ed826..80a1c68 100644 --- a/motrix_envs/src/motrix_envs/locomotion/go1/xmls/assets/dreamwaq_terrain.png +++ b/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 diff --git a/scripts/gen_dreamwaq_terrain.py b/scripts/gen_dreamwaq_terrain.py index 1b89f9c..86076ca 100644 --- a/scripts/gen_dreamwaq_terrain.py +++ b/scripts/gen_dreamwaq_terrain.py @@ -22,7 +22,12 @@ BORDER_M = 5.0 PROPORTIONS = [0.1, 0.1, 0.35, 0.35, 0.1] CUM = [sum(PROPORTIONS[:i + 1]) for i in range(len(PROPORTIONS))] PLATFORM_M = 3.0 -_SLOPE_SCALE = 0.4 # 上游原值（已验证 z_scale 上限远超 0.54） +_SLOPE_SCALE = 0.25 # MotrixSim-friendly slope range +ROUGH_GRID_M = 0.20 # Correlate roughness over 20 cm, not each 5 cm pixel. +ROUGH_BASE_M = 0.01 +ROUGH_GAIN_M = 0.03 +OBSTACLE_BASE_M = 0.03 +OBSTACLE_GAIN_M = 0.10 CELL_PX = int(CELL_M / HS) # 160 BORDER_PX = int(BORDER_M / HS) # 100 @@ -61,11 +66,16 @@ def draw_slope(canvas, x0, y0, difficulty, noise=False): hi = max(edge_h, 0) hf = np.clip(hf, lo, hi).astype(np.uint16) if noise: - na = int(0.05 / VS) - n = np.random.randint(-na, na + 1, (CELL_PX, CELL_PX), dtype=np.int16) - # 噪声也只在平台外 + # Independent 5 cm samples created 10 cm jumps between adjacent + # pixels. Interpolate a 20 cm grid for physically coherent roughness. + coarse_px = max(2, int(round(ROUGH_GRID_M / HS))) + amp = ROUGH_BASE_M + ROUGH_GAIN_M * difficulty + coarse = np.random.uniform( + -amp, amp, (coarse_px, coarse_px)).astype(np.float32) + n = cv2.resize(coarse, (CELL_PX, CELL_PX), interpolation=cv2.INTER_LINEAR) n[cx - p2:cx + p2, cy - p2:cy + p2] = 0 - hf = np.clip(hf.astype(np.int32) + n, 0, 65535).astype(np.uint16) + hf = np.clip(hf.astype(np.float32) * VS + n, 0, None) + hf = np.rint(hf / VS).astype(np.uint16) canvas[y0:y0 + CELL_PX, x0:x0 + CELL_PX] += hf @@ -105,7 +115,7 @@ def draw_obstacles(canvas, x0, y0, difficulty): """离散障碍物（随机矩形块）。""" if difficulty <= 0: return - max_h = int((0.05 + 0.2 * difficulty) / VS) + max_h = int((OBSTACLE_BASE_M + OBSTACLE_GAIN_M * difficulty) / VS) if max_h <= 0: return p2 = PLATFORM_PX // 2