cyy/Motrixlab
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix terrain curriculum traversal and hfield roughness

Browse Source
This commit is contained in:
8x54zj-m
2026-07-22 16:46:32 +08:00
parent a0e8501d11
commit 913d5061ff
3 changed files with 58 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py
View File

@@ -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)

4
motrix_envs/src/motrix_envs/locomotion/go1/xmls/assets/dreamwaq_terrain.png
View File

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

22
scripts/gen_dreamwaq_terrain.py
View File

@@ -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