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

Use integrated progress for terrain curriculum

Browse Source
This commit is contained in:
8x54zj-m
2026-07-22 19:10:48 +08:00
parent 913d5061ff
commit 7358b31de1
2 changed files with 70 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

30
motrix_envs/tests/test_dreamwaq_state_safety.py
View File

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