From 3fa284912387913851891b6e037fb916571532a2 Mon Sep 17 00:00:00 2001
From: 8x54zj-m <8x54zj-m@motrixlab.local>
Date: Wed, 22 Jul 2026 20:03:02 +0800
Subject: [PATCH] Sample forward DreamWaQ speed commands

---
 .../motrix_envs/locomotion/go1/dreamwaq.py    | 18 ++++++++------
 .../tests/test_dreamwaq_state_safety.py       | 24 +++++++++++++++++++
 2 files changed, 35 insertions(+), 7 deletions(-)

diff --git a/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py b/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py
index 645c043..8401583 100644
--- a/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py
+++ b/motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py
@@ -99,10 +99,11 @@ class DreamWaQCfg(Go1WalkNpEnvCfg):
     cell_size: float = 8.0
     border_size: float = 5.0
 
-    # 命令范围（上游: [-1,1] 对称）
+    # Forward stair commands: stand or walk at 0.5-1.0 m/s.
     @dataclass
     class Commands:
-        vel_limit = [[-1.0, -1.0, -1.0], [1.0, 1.0, 1.0]]
+        forward_speed_range = (0.5, 1.0)
+        stand_probability = 0.2
 
     commands: Commands = field(default_factory=Commands)
 
@@ -196,13 +197,16 @@ class DreamWaQTask(Go1WalkTask):
         self._hf_cache = None
         self._hm_cache = None
 
-    # ── 命令：全范围，无课程（匹配上游）──
+    # ── 命令：站立或随机速度直行 ──
 
     def resample_commands(self, num_envs: int) -> np.ndarray:
-        lim = np.array(self.cfg.commands.vel_limit, dtype=np.float32)
-        cmds = np.random.uniform(lim[0], lim[1], size=(num_envs, 3)).astype(np.float32)
-        small = np.linalg.norm(cmds[:, :2], axis=1) < 0.2
-        cmds[small, :2] = 0.0
+        cmds = np.zeros((num_envs, 3), dtype=np.float32)
+        speed_min, speed_max = self.cfg.commands.forward_speed_range
+        cmds[:, 0] = np.random.uniform(
+            speed_min, speed_max, size=num_envs
+        ).astype(np.float32)
+        stand = np.random.random(num_envs) < self.cfg.commands.stand_probability
+        cmds[stand, 0] = 0.0
         return cmds
 
     # ── 动作裁剪 + 力矩计算 ──
diff --git a/motrix_envs/tests/test_dreamwaq_state_safety.py b/motrix_envs/tests/test_dreamwaq_state_safety.py
index 3117c35..0032d99 100644
--- a/motrix_envs/tests/test_dreamwaq_state_safety.py
+++ b/motrix_envs/tests/test_dreamwaq_state_safety.py
@@ -59,6 +59,30 @@ def test_apply_action_advances_three_frame_action_history():
     np.testing.assert_array_equal(state.info["current_actions"], 3.0)
 
 
+def test_dreamwaq_commands_are_zero_or_forward_between_half_and_one(monkeypatch):
+    task = DreamWaQTask.__new__(DreamWaQTask)
+    task._cfg = SimpleNamespace(
+        commands=SimpleNamespace(
+            forward_speed_range=(0.5, 1.0), stand_probability=0.2
+        )
+    )
+    monkeypatch.setattr(
+        np.random,
+        "uniform",
+        lambda low, high, size: np.array([0.5, 0.75, 1.0], dtype=np.float32),
+    )
+    monkeypatch.setattr(
+        np.random,
+        "random",
+        lambda size: np.array([0.1, 0.2, 0.9], dtype=np.float32),
+    )
+
+    commands = task.resample_commands(3)
+
+    np.testing.assert_array_equal(commands[:, 0], [0.0, 0.75, 1.0])
+    np.testing.assert_array_equal(commands[:, 1:], 0.0)
+
+
 def test_dreamwaq_feet_air_time_matches_upstream_contact_filtering():
     task = DreamWaQTask.__new__(DreamWaQTask)
     task._cfg = SimpleNamespace(ctrl_dt=0.02)