更新mujoco强化学习训练的代码，增加可以直接进行步态移动控制训练的代码

README.md

@@ -93,6 +93,7 @@ ros2 launch guguji_ros2 gazebo.launch.py gui:=false pause:=true
 
 - Gazebo Fortress 训练后端
 - MuJoCo 训练后端
+- MuJoCo 下的前进 / 后退 / 转弯命令条件化训练配置 `guguji_rl/configs/mujoco_command_walk_ppo.yaml`
 
 如果你准备走 MuJoCo 路线，建议直接阅读：
 

docs/guguji_mujoco_rl_guide.md

@@ -28,6 +28,8 @@ MuJoCo 训练代码继续放在 `guguji_rl/` 目录，而不是单独再开一
   MuJoCo 平衡训练配置
 - `guguji_rl/configs/mujoco_walk_ppo.yaml`
   MuJoCo walking 课程训练配置
+- `guguji_rl/configs/mujoco_command_walk_ppo.yaml`
+  前进 / 后退 / 转弯命令条件化训练配置
 - `guguji_rl/scripts/export_mujoco_xml.py`
   把动态生成的 MJCF 导出到文件，方便你逐行查看
 
@@ -137,7 +139,70 @@ python scripts/train.py \
 
 训练脚本会自动按三段课程顺序继续训练，不需要你手动分三次运行。
 
-## 7. 怎么评估是不是更会往前走
+## 7. 平衡模型什么时候算“可以毕业”
+
+不要只看“训练跑完了没有”，更重要的是看它是不是已经满足下一阶段的稳定性门槛。
+
+建议至少检查下面 4 条：
+
+- 5 次左右确定性回放里，大多数 episode 都能站满 `max_episode_steps`
+- 不应该频繁出现 `terminated=True` 的摔倒终止
+- `roll / pitch` 峰值最好控制在 `0.10 ~ 0.15 rad` 以内
+- 站立命令下的 `delta_x` 最好只有厘米级甚至毫米级漂移
+
+你这次的 MuJoCo balance 实测已经满足进入下一阶段的条件：
+
+- 5 / 5 次确定性回放都站满了 `400` 步
+- 没有出现摔倒终止
+- `max_abs_roll` 大约 `0.076 rad`
+- `max_abs_pitch` 大约 `0.058 rad`
+- `min_height` 大约 `0.3325 m`
+- `delta_x` 只有约 `-0.001 ~ -0.002 m`
+
+这说明它已经不是“勉强不摔”，而是足够作为命令条件化 walking 的初始化模型。
+
+## 8. 为什么下一步要做命令条件化，而不是继续只训固定前进
+
+固定目标前进版的作用，是先把“迈腿并保持大体稳定”这件事学出来。
+
+但如果你后面要做：
+
+- 前进
+- 后退
+- 左转 / 右转
+- 原地等待
+
+那策略就必须知道“现在到底想让我做什么”。所以 observation 里必须加入命令，reward 也必须改成“跟踪当前命令”，而不是永远只奖励一个固定前进速度。
+
+这次代码里已经把三件事接好了：
+
+1. observation 新增了 `yaw_rate / cmd_vx / cmd_yaw`
+2. reward 新增了 `yaw_rate_tracking / turn_progress / command_stillness`
+3. 训练配置新增了 `commands` 段，用来采样 `stand / turn / translate / combined` 命令
+
+## 9. 命令条件化 walking 怎么跑
+
+直接基于已经训好的 balance 模型继续：
+
+```bash
+cd /home/corvin/Project/guguji_simulation/guguji_rl
+source .venv/bin/activate
+python scripts/train.py \
+  --config configs/mujoco_command_walk_ppo.yaml \
+  --init-model outputs/<你的_mujoco_balance_实验目录>/final_model.zip \
+  --device auto
+```
+
+注意，这个配置的 observation 会从 `29` 维扩展到 `32` 维，所以旧 balance 模型不能再“完全一模一样地”加载。
+
+现在 `train.py` 已经做了兼容处理：
+
+- 如果维度没变，就完整加载旧模型参数
+- 如果维度变了，就自动跳过第一层输入权重，只保留后续兼容层做 warm start
+
+所以你仍然可以复用 balance 模型里已经学到的大部分稳定控制能力。
+
+## 10. 怎么评估是不是更会往前走
 
 训练结束后，`train.py` 会自动输出：
 
@@ -154,6 +219,16 @@ python scripts/evaluate_forward_progress.py \
   --model outputs/<你的实验目录>/final_model.zip
 ```
 
+命令条件化模型推荐先固定一个前进命令再评估：
+
+```bash
+python scripts/evaluate_forward_progress.py \
+  --config configs/mujoco_command_walk_ppo.yaml \
+  --model outputs/<你的实验目录>/final_model.zip \
+  --command-forward 0.18 \
+  --command-yaw 0.0
+```
+
 这样你后面调：
 
 - 参考步态振幅
@@ -163,7 +238,7 @@ python scripts/evaluate_forward_progress.py \
 
 都可以直接看量化指标，而不是只靠肉眼猜。
 
-## 8. 策略回放
+## 11. 策略回放
 
 如果你想在 MuJoCo 里回放训练好的策略：
 
@@ -187,7 +262,19 @@ python scripts/run_policy.py \
   --render-human
 ```
 
-## 9. MuJoCo 版和 Gazebo 版有什么关系
+命令条件化模型回放时，也可以直接在命令行指定要看的命令：
+
+```bash
+python scripts/run_policy.py \
+  --config configs/mujoco_command_walk_ppo.yaml \
+  --model outputs/<你的实验目录>/final_model.zip \
+  --deterministic \
+  --render-human \
+  --command-forward 0.18 \
+  --command-yaw 0.0
+```
+
+## 12. MuJoCo 版和 Gazebo 版有什么关系
 
 这两套后端现在是并行存在的：
 
@@ -200,7 +287,7 @@ python scripts/run_policy.py \
 2. 再把比较靠谱的策略思路迁回 Gazebo
 3. 最后再往真实机器人部署链路上收敛
 
-## 10. 你后面最常改的地方
+## 13. 你后面最常改的地方
 
 如果你后面要自己继续调，我建议优先看这些文件：
 
@@ -212,6 +299,8 @@ python scripts/run_policy.py \
   调平衡训练参数
 - `guguji_rl/configs/mujoco_walk_ppo.yaml`
   调 walking 课程、目标速度和参考步态
+- `guguji_rl/configs/mujoco_command_walk_ppo.yaml`
+  调命令采样范围、转弯课程和命令跟踪奖励
 
 如果后面你愿意继续推进，最自然的下一步一般是：
 

guguji_rl/README.md

@@ -23,6 +23,7 @@
 - `scripts/run_policy.py`：把训练好的策略作为在线控制程序持续运行
 - `scripts/check_env.py`：训练前自检脚本
 - `scripts/export_mujoco_xml.py`：导出动态生成的 MuJoCo XML
+- `configs/mujoco_command_walk_ppo.yaml`：前进 / 后退 / 转弯命令条件化训练配置
 
 ## 建议工作流
 
@@ -30,10 +31,11 @@
 2. 再运行 `check_env.py` 检查 ROS 2 / Gazebo 训练接口
 3. 先跑 `balance_ppo.yaml` 训练站立平衡
 4. 再跑 `forward_transition_ppo.yaml` 做轻微前进过渡训练
-5. 最后再跑 `walk_ppo.yaml` 训练正式前进
-6. `walk_ppo.yaml` 现在已经内置了 `0.18 -> 0.22 -> 0.26` 的 walking 课程阶段
-6. 训练出模型后，先用 `evaluate_forward_progress.py` 看前进效果，再用 `run_policy.py` 在 ROS 2 系统中持续推理控制
-7. 每轮训练结束后，`train.py` 会自动输出一次 `delta_x / mean_vx` 前进评估
+5. 再跑 `walk_ppo.yaml` 训练固定目标前进
+6. 如果平衡和固定前进都比较稳定，再跑 `mujoco_command_walk_ppo.yaml` 进入“前进 / 后退 / 转弯”的命令条件化训练
+7. `walk_ppo.yaml` 现在已经内置了 `0.18 -> 0.22 -> 0.26` 的 walking 课程阶段
+8. 训练出模型后，先用 `evaluate_forward_progress.py` 看前进效果，再用 `run_policy.py` 在 ROS 2 系统中持续推理控制
+9. 每轮训练结束后，`train.py` 会自动输出一次 `delta_x / mean_vx` 前进评估
 
 ## 安装依赖
 
@@ -79,6 +81,26 @@ python scripts/train.py \
   --device auto
 ```
 
+如果你已经完成“稳定站立”，下一步建议直接切到命令条件化 walking：
+
+```bash
+python scripts/train.py \
+  --config configs/mujoco_command_walk_ppo.yaml \
+  --init-model outputs/<你的_mujoco_balance_实验目录>/final_model.zip \
+  --device auto
+```
+
+`mujoco_command_walk_ppo.yaml` 会把 observation 从原来的 `29` 维扩展成 `32` 维，新增：
+
+- 当前横摆角速度 `yaw_rate`
+- 当前命令前进速度 `cmd_vx`
+- 当前命令偏航角速度 `cmd_yaw`
+
+`train.py` 现在已经支持兼容 warm start：
+
+- 如果 observation 维度没变，会完整加载旧模型参数
+- 如果 observation 维度变了，会自动跳过第一层输入权重，只保留后续可兼容的策略权重
+
 MuJoCo 版的详细步骤建议直接看：
 
 - `/home/corvin/Project/guguji_simulation/docs/guguji_mujoco_rl_guide.md`
@@ -107,6 +129,18 @@ python3 scripts/run_policy.py \
   --deterministic
 ```
 
+命令条件化模型回放时，也可以临时固定一条命令来看策略反应：
+
+```bash
+python3 scripts/run_policy.py \
+  --config configs/mujoco_command_walk_ppo.yaml \
+  --model outputs/<你的实验目录>/final_model.zip \
+  --deterministic \
+  --render-human \
+  --command-forward 0.18 \
+  --command-yaw 0.0
+```
+
 ## 单独评估前进效果
 
 如果你想手动比较两个模型的前进能力，可以直接运行：
@@ -119,6 +153,16 @@ python3 scripts/evaluate_forward_progress.py \
   --model outputs/<你的实验目录>/final_model.zip
 ```
 
+如果是命令条件化模型，也可以临时指定评估命令：
+
+```bash
+python3 scripts/evaluate_forward_progress.py \
+  --config configs/mujoco_command_walk_ppo.yaml \
+  --model outputs/<你的实验目录>/final_model.zip \
+  --command-forward 0.18 \
+  --command-yaw 0.0
+```
+
 评估结果会直接输出每个 episode 的：
 
 - `delta_x`
@@ -155,3 +199,9 @@ python3 scripts/train.py --config configs/walk_ppo.yaml
 - `0.26 m/s`
 
 MuJoCo 版的 `mujoco_walk_ppo.yaml` 也采用同样的三段课程思路。
+
+如果你切到 `mujoco_command_walk_ppo.yaml`，课程的重点就不再只是“速度越来越快”，而是：
+
+- 命令速度范围逐步放开
+- 转弯命令范围逐步放开
+- `stand / turn / translate / combined` 这几类命令的占比逐步增加

guguji_rl/configs/mujoco_command_walk_ppo.yaml

@@ -0,0 +1,185 @@
+experiment:
+  name: mujoco_command_walk_ppo
+
+robot:
+  model_name: guguji
+  urdf_path: guguji_ros2_ws/src/guguji_ros2/urdf/guguji.urdf
+  joint_names:
+    - left_hip_pitch_joint
+    - left_knee_pitch_joint
+    - left_ankle_pitch_joint
+    - left_ankle_joint
+    - right_hip_pitch_joint
+    - right_knee_pitch_joint
+    - right_ankle_pitch_joint
+    - right_ankle_joint
+  nominal_joint_positions:
+    left_hip_pitch_joint: 0.04
+    left_knee_pitch_joint: 0.18
+    left_ankle_pitch_joint: -0.10
+    left_ankle_joint: 0.0
+    right_hip_pitch_joint: 0.04
+    right_knee_pitch_joint: 0.18
+    right_ankle_pitch_joint: -0.10
+    right_ankle_joint: 0.0
+  # 这里继续使用“参考步态 + 残差动作”，但参考步态会根据命令强弱自动放大/减弱。
+  reference_gait:
+    enabled: true
+    period: 0.76
+    stance_ratio: 0.60
+    hip_pitch_amplitude: 0.30
+    hip_pitch_bias: 0.04
+    knee_pitch_amplitude: 0.42
+    knee_pitch_bias: 0.12
+    swing_knee_scale: 1.05
+    ankle_pitch_amplitude: 0.18
+    ankle_pitch_bias: -0.04
+    push_off_ankle_scale: 0.18
+    # 当前进/转弯命令达到这些尺度时，参考步态基本进入“全幅”。
+    command_velocity_scale: 0.20
+    command_yaw_rate_scale: 0.26
+    # 转弯时让左右腿步幅出现轻微不对称，帮助策略更快学会转向。
+    command_turn_gain: 0.30
+    # 设为 0 表示站立命令时尽量不主动摆腿。
+    min_command_gait_scale: 0.0
+  action_scale: 0.08
+  action_smoothing: 0.84
+
+sim:
+  backend: mujoco
+  control_dt: 0.05
+
+mujoco:
+  timestep: 0.005
+  frame_skip: 10
+  root_body_height: 0.36
+  joint_damping: 0.55
+  joint_armature: 0.02
+  actuator_kp: 42.0
+  solver_iterations: 90
+  solver_ls_iterations: 18
+  floor_friction: [1.6, 0.06, 0.01]
+  default_friction: [0.95, 0.04, 0.01]
+  foot_friction: [2.1, 0.06, 0.01]
+  contact_margin: 0.002
+  contact_gap: 0.0005
+  render_mode: none
+  render_width: 1280
+  render_height: 720
+  reset_base_xy_noise_scale: 0.004
+  reset_base_height_noise_scale: 0.002
+  reset_joint_noise_scale: 0.008
+  reset_hold_steps: 12
+
+task:
+  # 命令条件化版本里，真正的目标速度主要由 commands 段动态采样。
+  target_forward_velocity: 0.0
+  target_yaw_rate: 0.0
+  target_base_height: null
+  max_roll_rad: 0.95
+  max_pitch_rad: 0.95
+  min_base_height: 0.19
+  termination_grace_steps: 18
+
+commands:
+  enabled: true
+  # 每隔 80 个控制步切换一次命令，既能训练“听命令”，又不至于切得太快。
+  resample_interval_steps: 80
+  forward_velocity_range: [-0.12, 0.24]
+  yaw_rate_range: [-0.30, 0.30]
+  stand_probability: 0.12
+  turn_only_probability: 0.18
+  combined_probability: 0.20
+  # 转弯原地命令时，允许一点很小的前后抖动，避免机械结构完全卡死。
+  turn_only_forward_abs_max: 0.03
+
+rewards:
+  alive_bonus: 0.8
+  velocity_tracking_scale: 4.2
+  velocity_tracking_sigma: 0.12
+  yaw_rate_tracking_scale: 2.4
+  yaw_rate_tracking_sigma: 0.16
+  forward_progress_scale: 3.2
+  turn_progress_scale: 1.3
+  command_stillness_scale: 1.1
+  hip_alternation_scale: 0.40
+  hip_target_separation: 0.34
+  hip_antiphase_sigma: 0.18
+  knee_flexion_scale: 0.30
+  knee_target: 0.26
+  knee_flexion_sigma: 0.15
+  upright_scale: 1.6
+  height_scale: 0.9
+  action_rate_penalty_scale: 0.004
+  joint_limit_penalty_scale: 0.05
+  lateral_velocity_penalty_scale: 0.10
+  # 这里延续旧字段名，但现在表示“沿命令反方向运动”的惩罚。
+  backward_velocity_penalty_scale: 1.0
+  stall_penalty_scale: 2.0
+  stall_velocity_threshold: 0.06
+  still_velocity_sigma: 0.07
+  still_yaw_rate_sigma: 0.10
+  command_deadband: 0.03
+  fall_penalty: -15.0
+
+training:
+  algorithm: ppo
+  total_timesteps: 20000
+  max_episode_steps: 500
+  seed: 42
+  device: auto
+  # 这里建议直接传入已经训好的 mujoco_balance 模型。
+  init_model_path: null
+  initial_log_std: -2.25
+  curriculum_stages:
+    - name: cmd_intro
+      total_timesteps: 12000
+      initial_log_std: -2.35
+      commands:
+        forward_velocity_range: [-0.04, 0.12]
+        yaw_rate_range: [-0.10, 0.10]
+        stand_probability: 0.32
+        turn_only_probability: 0.10
+        combined_probability: 0.06
+    - name: cmd_transition
+      total_timesteps: 15000
+      initial_log_std: -2.30
+      commands:
+        forward_velocity_range: [-0.08, 0.18]
+        yaw_rate_range: [-0.18, 0.18]
+        stand_probability: 0.20
+        turn_only_probability: 0.14
+        combined_probability: 0.12
+    - name: cmd_full
+      total_timesteps: 20000
+      initial_log_std: -2.25
+      commands:
+        forward_velocity_range: [-0.12, 0.24]
+        yaw_rate_range: [-0.30, 0.30]
+        stand_probability: 0.12
+        turn_only_probability: 0.18
+        combined_probability: 0.20
+  learning_rate: 0.0001
+  n_steps: 1024
+  batch_size: 256
+  gamma: 0.99
+  gae_lambda: 0.95
+  clip_range: 0.15
+  ent_coef: 0.0
+  vf_coef: 0.5
+  policy_net_arch: [256, 256]
+  checkpoint_freq: 10000
+  output_root: guguji_rl/outputs
+
+evaluation:
+  episodes: 3
+  deterministic: true
+  auto_forward_progress: true
+  forward_progress_episodes: 2
+  forward_progress_max_steps: 500
+  forward_progress_deterministic: true
+  # 训练后默认先看“给一个明确前进命令时能不能往前走”。
+  command_override:
+    enabled: true
+    forward_velocity: 0.18
+    yaw_rate: 0.0

guguji_rl/guguji_rl/config.py

@@ -55,6 +55,21 @@ DEFAULT_CONFIG: dict[str, Any] = {
         'spawn_pitch': 0.0,
         'spawn_yaw': 0.0,
     },
+    'commands': {
+        'enabled': False,
+        'resample_interval_steps': 0,
+        'forward_velocity_range': [0.0, 0.0],
+        'yaw_rate_range': [0.0, 0.0],
+        'stand_probability': 0.0,
+        'turn_only_probability': 0.0,
+        'combined_probability': 0.0,
+        'turn_only_forward_abs_max': 0.02,
+        'fixed_command': {
+            'enabled': False,
+            'forward_velocity': 0.0,
+            'yaw_rate': 0.0,
+        },
+    },
     'mujoco': {
         'timestep': 0.005,
         'frame_skip': 10,
@@ -81,6 +96,7 @@ DEFAULT_CONFIG: dict[str, Any] = {
     },
     'task': {
         'target_forward_velocity': 0.0,
+        'target_yaw_rate': 0.0,
         'target_base_height': None,
         'max_roll_rad': 0.6,
         'max_pitch_rad': 0.6,
@@ -90,7 +106,10 @@ DEFAULT_CONFIG: dict[str, Any] = {
         'alive_bonus': 1.0,
         'velocity_tracking_scale': 1.0,
         'velocity_tracking_sigma': 0.3,
+        'yaw_rate_tracking_scale': 0.0,
+        'yaw_rate_tracking_sigma': 0.2,
         'forward_progress_scale': 0.0,
+        'turn_progress_scale': 0.0,
         'hip_alternation_scale': 0.0,
         'hip_target_separation': 0.3,
         'hip_antiphase_sigma': 0.2,
@@ -105,6 +124,10 @@ DEFAULT_CONFIG: dict[str, Any] = {
         'backward_velocity_penalty_scale': 0.0,
         'stall_penalty_scale': 0.0,
         'stall_velocity_threshold': 0.0,
+        'command_stillness_scale': 0.0,
+        'still_velocity_sigma': 0.08,
+        'still_yaw_rate_sigma': 0.12,
+        'command_deadband': 0.03,
         'fall_penalty': -10.0,
     },
     'training': {
@@ -135,6 +158,11 @@ DEFAULT_CONFIG: dict[str, Any] = {
         'forward_progress_episodes': 2,
         'forward_progress_max_steps': 500,
         'forward_progress_deterministic': True,
+        'command_override': {
+            'enabled': False,
+            'forward_velocity': 0.0,
+            'yaw_rate': 0.0,
+        },
     },
 }
 

guguji_rl/guguji_rl/envs/gazebo_biped_env.py

@@ -8,7 +8,7 @@ import gymnasium as gym
 import numpy as np
 
 from ..config import resolve_project_path
-from ..math_utils import quaternion_xyzw_to_euler
+from ..math_utils import quaternion_xyzw_to_euler, wrap_to_pi
 from ..rewards import BipedRewardCalculator, RewardContext
 from ..ros2_interface import GugujiRos2Interface
 from ..state_types import RobotStateSnapshot
@@ -28,6 +28,7 @@ class GazeboBipedEnv(gym.Env):
         self.sim_config = config['sim']
         self.task_config = config['task']
         self.training_config = config['training']
+        self.commands_config = config.get('commands', {})
         self.configured_target_base_height = self.task_config['target_base_height']
         self.reference_gait_config = self.robot_config.get('reference_gait', {})
 
@@ -60,6 +61,36 @@ class GazeboBipedEnv(gym.Env):
             float(self.reference_gait_config.get('period', 0.9)),
             float(self.sim_config['control_dt']),
         )
+        self.commands_enabled = bool(self.commands_config.get('enabled', False))
+        forward_velocity_range = self.commands_config.get(
+            'forward_velocity_range',
+            [self.task_config['target_forward_velocity'], self.task_config['target_forward_velocity']],
+        )
+        yaw_rate_range = self.commands_config.get(
+            'yaw_rate_range',
+            [self.task_config.get('target_yaw_rate', 0.0), self.task_config.get('target_yaw_rate', 0.0)],
+        )
+        self.command_forward_velocity_range = np.sort(np.asarray(forward_velocity_range, dtype=np.float32))
+        self.command_yaw_rate_range = np.sort(np.asarray(yaw_rate_range, dtype=np.float32))
+        self.command_resample_interval_steps = int(self.commands_config.get('resample_interval_steps', 0))
+        self.command_stand_probability = float(np.clip(self.commands_config.get('stand_probability', 0.0), 0.0, 1.0))
+        self.command_turn_only_probability = float(
+            np.clip(self.commands_config.get('turn_only_probability', 0.0), 0.0, 1.0)
+        )
+        self.command_combined_probability = float(
+            np.clip(self.commands_config.get('combined_probability', 0.0), 0.0, 1.0)
+        )
+        self.command_turn_only_forward_abs_max = float(
+            max(self.commands_config.get('turn_only_forward_abs_max', 0.02), 0.0)
+        )
+        self.fixed_command_config = self.commands_config.get('fixed_command', {})
+        self.fixed_command_enabled = bool(self.fixed_command_config.get('enabled', False))
+        self.command_velocity_scale = max(float(self.reference_gait_config.get('command_velocity_scale', 0.20)), 1e-6)
+        self.command_yaw_rate_scale = max(float(self.reference_gait_config.get('command_yaw_rate_scale', 0.30)), 1e-6)
+        self.command_turn_gain = float(np.clip(self.reference_gait_config.get('command_turn_gain', 0.28), 0.0, 0.8))
+        self.min_command_gait_scale = float(
+            np.clip(self.reference_gait_config.get('min_command_gait_scale', 0.0), 0.0, 1.0)
+        )
 
         self.interface = GugujiRos2Interface(
             joint_names=self.joint_names,
@@ -92,7 +123,7 @@ class GazeboBipedEnv(gym.Env):
         self.observation_space = gym.spaces.Box(
             low=-np.inf,
             high=np.inf,
-            shape=(len(self.joint_names) * 3 + 5,),
+            shape=(len(self.joint_names) * 3 + (8 if self.commands_enabled else 5),),
             dtype=np.float32,
         )
 
@@ -103,6 +134,9 @@ class GazeboBipedEnv(gym.Env):
         self.current_action_residual = np.zeros(len(self.joint_names), dtype=np.float32)
         self.step_count = 0
         self.target_base_height = self.configured_target_base_height
+        self.current_target_forward_velocity = float(self.task_config['target_forward_velocity'])
+        self.current_target_yaw_rate = float(self.task_config.get('target_yaw_rate', 0.0))
+        self.command_steps_until_resample = 0
         self.gait_phase = 0.0
 
     def _normalized_joint_position(self, joint_position: np.ndarray) -> np.ndarray:
@@ -145,6 +179,24 @@ class GazeboBipedEnv(gym.Env):
         push_off_ankle_scale = float(self.reference_gait_config.get('push_off_ankle_scale', 0.0))
         stance_ratio = float(self.reference_gait_config.get('stance_ratio', 0.62))
         stance_ratio = float(np.clip(stance_ratio, 0.05, 0.95))
+        gait_scale = 1.0
+        direction_sign = 1.0
+        left_stride_scale = 1.0
+        right_stride_scale = 1.0
+
+        if self.commands_enabled:
+            normalized_speed = abs(self.current_target_forward_velocity) / self.command_velocity_scale
+            normalized_turn = abs(self.current_target_yaw_rate) / self.command_yaw_rate_scale
+            gait_scale = float(np.clip(max(normalized_speed, normalized_turn, self.min_command_gait_scale), 0.0, 1.0))
+
+            if self.current_target_forward_velocity < -0.03:
+                direction_sign = -1.0
+
+            normalized_yaw_command = float(
+                np.clip(self.current_target_yaw_rate / self.command_yaw_rate_scale, -1.0, 1.0)
+            )
+            left_stride_scale = max(0.2, 1.0 - self.command_turn_gain * normalized_yaw_command)
+            right_stride_scale = max(0.2, 1.0 + self.command_turn_gain * normalized_yaw_command)
 
         def gait_profile(phase: float) -> tuple[float, float, float]:
             """返回该相位下的髋、膝、踝参考轨迹形状。"""
@@ -171,17 +223,25 @@ class GazeboBipedEnv(gym.Env):
 
         for index, joint_name in enumerate(self.joint_names):
             if joint_name == 'left_hip_pitch_joint':
-                offsets[index] = hip_bias + hip_amplitude * left_hip_profile
+                offsets[index] = hip_bias + direction_sign * gait_scale * left_stride_scale * hip_amplitude * left_hip_profile
             elif joint_name == 'right_hip_pitch_joint':
-                offsets[index] = hip_bias + hip_amplitude * right_hip_profile
+                offsets[index] = (
+                    hip_bias + direction_sign * gait_scale * right_stride_scale * hip_amplitude * right_hip_profile
+                )
             elif joint_name == 'left_knee_pitch_joint':
-                offsets[index] = knee_bias + knee_amplitude * left_knee_profile
+                offsets[index] = knee_bias + gait_scale * left_stride_scale * knee_amplitude * left_knee_profile
             elif joint_name == 'right_knee_pitch_joint':
-                offsets[index] = knee_bias + knee_amplitude * right_knee_profile
+                offsets[index] = knee_bias + gait_scale * right_stride_scale * knee_amplitude * right_knee_profile
             elif joint_name == 'left_ankle_pitch_joint':
-                offsets[index] = ankle_bias + ankle_amplitude * left_ankle_profile
+                offsets[index] = (
+                    ankle_bias
+                    + direction_sign * gait_scale * left_stride_scale * ankle_amplitude * left_ankle_profile
+                )
             elif joint_name == 'right_ankle_pitch_joint':
-                offsets[index] = ankle_bias + ankle_amplitude * right_ankle_profile
+                offsets[index] = (
+                    ankle_bias
+                    + direction_sign * gait_scale * right_stride_scale * ankle_amplitude * right_ankle_profile
+                )
 
         return offsets
 
@@ -195,30 +255,128 @@ class GazeboBipedEnv(gym.Env):
             self.target_base_height = float(snapshot.base_position[2])
         return float(self.target_base_height)
 
+    def _base_motion_features(
+        self,
+        snapshot: RobotStateSnapshot,
+        previous_snapshot: RobotStateSnapshot,
+    ) -> tuple[float, float, float, float, float]:
+        dt = max(snapshot.sim_time - previous_snapshot.sim_time, self.sim_config['control_dt'], 1e-3)
+        velocity = (snapshot.base_position - previous_snapshot.base_position) / dt
+        roll, pitch, yaw = quaternion_xyzw_to_euler(snapshot.base_quaternion)
+        _, _, previous_yaw = quaternion_xyzw_to_euler(previous_snapshot.base_quaternion)
+        yaw_rate = wrap_to_pi(yaw - previous_yaw) / dt
+        return float(velocity[0]), float(velocity[1]), float(yaw_rate), float(roll), float(pitch)
+
+    def _sample_uniform_range(self, value_range: np.ndarray) -> float:
+        return float(self.np_random.uniform(float(value_range[0]), float(value_range[1])))
+
+    def _set_command_target(self, forward_velocity: float, yaw_rate: float) -> None:
+        self.current_target_forward_velocity = float(forward_velocity)
+        self.current_target_yaw_rate = float(yaw_rate)
+        self.command_steps_until_resample = int(self.command_resample_interval_steps)
+
+    def _sample_command_target(self) -> None:
+        if not self.commands_enabled:
+            self.current_target_forward_velocity = float(self.task_config['target_forward_velocity'])
+            self.current_target_yaw_rate = float(self.task_config.get('target_yaw_rate', 0.0))
+            self.command_steps_until_resample = 0
+            return
+
+        if self.fixed_command_enabled:
+            self.current_target_forward_velocity = float(
+                self.fixed_command_config.get('forward_velocity', self.task_config['target_forward_velocity'])
+            )
+            self.current_target_yaw_rate = float(
+                self.fixed_command_config.get('yaw_rate', self.task_config.get('target_yaw_rate', 0.0))
+            )
+            self.command_steps_until_resample = 0
+            return
+
+        stand_probability = self.command_stand_probability
+        turn_only_probability = self.command_turn_only_probability
+        combined_probability = self.command_combined_probability
+        translate_only_probability = max(1.0 - stand_probability - turn_only_probability - combined_probability, 0.0)
+        total_probability = stand_probability + turn_only_probability + combined_probability + translate_only_probability
+        if total_probability <= 0.0:
+            stand_probability = 0.0
+            turn_only_probability = 0.0
+            combined_probability = 0.0
+            translate_only_probability = 1.0
+            total_probability = 1.0
+
+        sample = float(self.np_random.uniform(0.0, 1.0))
+        stand_cutoff = stand_probability / total_probability
+        turn_only_cutoff = stand_cutoff + turn_only_probability / total_probability
+        translate_only_cutoff = turn_only_cutoff + translate_only_probability / total_probability
+
+        if sample < stand_cutoff:
+            self._set_command_target(0.0, 0.0)
+            return
+        if sample < turn_only_cutoff:
+            forward_velocity = float(
+                self.np_random.uniform(-self.command_turn_only_forward_abs_max, self.command_turn_only_forward_abs_max)
+            )
+            yaw_rate = self._sample_uniform_range(self.command_yaw_rate_range)
+            self._set_command_target(forward_velocity, yaw_rate)
+            return
+        if sample < translate_only_cutoff:
+            self._set_command_target(self._sample_uniform_range(self.command_forward_velocity_range), 0.0)
+            return
+
+        self._set_command_target(
+            self._sample_uniform_range(self.command_forward_velocity_range),
+            self._sample_uniform_range(self.command_yaw_rate_range),
+        )
+
+    def _advance_command_schedule(self) -> None:
+        if not self.commands_enabled or self.fixed_command_enabled or self.command_resample_interval_steps <= 0:
+            return
+
+        self.command_steps_until_resample -= 1
+        if self.command_steps_until_resample <= 0:
+            self._sample_command_target()
+
     def _build_observation(
         self,
         snapshot: RobotStateSnapshot,
         previous_snapshot: RobotStateSnapshot,
         previous_action: np.ndarray,
     ) -> np.ndarray:
-        dt = max(snapshot.sim_time - previous_snapshot.sim_time, self.sim_config['control_dt'], 1e-3)
-        velocity = (snapshot.base_position - previous_snapshot.base_position) / dt
-        roll, pitch, _ = quaternion_xyzw_to_euler(snapshot.base_quaternion)
+        forward_velocity, lateral_velocity, yaw_rate, roll, pitch = self._base_motion_features(
+            snapshot,
+            previous_snapshot,
+        )
+        if self.commands_enabled:
+            base_features = np.array(
+                [
+                    snapshot.base_position[2],
+                    roll,
+                    pitch,
+                    forward_velocity,
+                    lateral_velocity,
+                    yaw_rate,
+                    self.current_target_forward_velocity,
+                    self.current_target_yaw_rate,
+                ],
+                dtype=np.float32,
+            )
+        else:
+            base_features = np.array(
+                [
+                    snapshot.base_position[2],
+                    roll,
+                    pitch,
+                    forward_velocity,
+                    self.current_target_forward_velocity,
+                ],
+                dtype=np.float32,
+            )
         observation = np.concatenate(
             [
                 self._normalized_joint_position(snapshot.joint_position),
                 snapshot.joint_velocity,
                 previous_action,
-                np.array(
-                    [
-                        snapshot.base_position[2],
-                        roll,
-                        pitch,
-                        velocity[0],
-                        self.task_config['target_forward_velocity'],
-                    ],
-                    dtype=np.float32,
-                ),
+                base_features,
             ],
             dtype=np.float32,
         )
@@ -283,12 +441,14 @@ class GazeboBipedEnv(gym.Env):
         self.current_action_residual = np.zeros(len(self.joint_names), dtype=np.float32)
         self.step_count = 0
         self.gait_phase = 0.0
+        self._sample_command_target()
 
         observation = self._build_observation(snapshot, snapshot, self.previous_action)
         info = {
             'reset': True,
             'target_base_height': float(self.target_base_height),
-            'target_forward_velocity': float(self.task_config['target_forward_velocity']),
+            'target_forward_velocity': float(self.current_target_forward_velocity),
+            'target_yaw_rate': float(self.current_target_yaw_rate),
         }
         return observation, info
 
@@ -308,6 +468,8 @@ class GazeboBipedEnv(gym.Env):
         self.current_snapshot = self.interface.wait_for_snapshot()
         terminated = self._terminated(self.current_snapshot)
         truncated = self.step_count >= int(self.training_config['max_episode_steps'])
+        target_forward_velocity = float(self.current_target_forward_velocity)
+        target_yaw_rate = float(self.current_target_yaw_rate)
 
         reward, reward_terms = self.reward_calculator.compute(
             RewardContext(
@@ -316,12 +478,14 @@ class GazeboBipedEnv(gym.Env):
                 action=action,
                 previous_action=self.previous_action,
                 joint_limits=self.joint_limits,
-                target_forward_velocity=float(self.task_config['target_forward_velocity']),
+                target_forward_velocity=target_forward_velocity,
+                target_yaw_rate=target_yaw_rate,
                 target_base_height=float(self.target_base_height),
                 control_dt=float(self.sim_config['control_dt']),
                 terminated=terminated,
             )
         )
+        self._advance_command_schedule()
         # 训练时保留奖励分项，方便后面定位“为什么学不会走”。
         observation = self._build_observation(
             self.current_snapshot,
@@ -332,6 +496,8 @@ class GazeboBipedEnv(gym.Env):
 
         info = {
             'joint_targets': joint_targets.tolist(),
+            'target_forward_velocity': target_forward_velocity,
+            'target_yaw_rate': target_yaw_rate,
             'reward_terms': reward_terms,
         }
         return observation, reward, terminated, truncated, info

guguji_rl/guguji_rl/envs/mujoco_biped_env.py

@@ -8,7 +8,7 @@ import mujoco
 import numpy as np
 
 from ..config import resolve_project_path
-from ..math_utils import quaternion_xyzw_to_euler
+from ..math_utils import quaternion_xyzw_to_euler, wrap_to_pi
 from ..mujoco_model import build_mujoco_model_spec
 from ..rewards import BipedRewardCalculator, RewardContext
 from ..state_types import RobotStateSnapshot
@@ -27,6 +27,7 @@ class MujocoBipedEnv(gym.Env):
         self.sim_config = config['sim']
         self.task_config = config['task']
         self.training_config = config['training']
+        self.commands_config = config.get('commands', {})
         self.mujoco_config = config.get('mujoco', {})
         self.configured_target_base_height = self.task_config['target_base_height']
         self.reference_gait_config = self.robot_config.get('reference_gait', {})
@@ -52,6 +53,36 @@ class MujocoBipedEnv(gym.Env):
             1e-4,
         )
         self.termination_grace_steps = int(self.task_config.get('termination_grace_steps', 0))
+        self.commands_enabled = bool(self.commands_config.get('enabled', False))
+        forward_velocity_range = self.commands_config.get(
+            'forward_velocity_range',
+            [self.task_config['target_forward_velocity'], self.task_config['target_forward_velocity']],
+        )
+        yaw_rate_range = self.commands_config.get(
+            'yaw_rate_range',
+            [self.task_config.get('target_yaw_rate', 0.0), self.task_config.get('target_yaw_rate', 0.0)],
+        )
+        self.command_forward_velocity_range = np.sort(np.asarray(forward_velocity_range, dtype=np.float32))
+        self.command_yaw_rate_range = np.sort(np.asarray(yaw_rate_range, dtype=np.float32))
+        self.command_resample_interval_steps = int(self.commands_config.get('resample_interval_steps', 0))
+        self.command_stand_probability = float(np.clip(self.commands_config.get('stand_probability', 0.0), 0.0, 1.0))
+        self.command_turn_only_probability = float(
+            np.clip(self.commands_config.get('turn_only_probability', 0.0), 0.0, 1.0)
+        )
+        self.command_combined_probability = float(
+            np.clip(self.commands_config.get('combined_probability', 0.0), 0.0, 1.0)
+        )
+        self.command_turn_only_forward_abs_max = float(
+            max(self.commands_config.get('turn_only_forward_abs_max', 0.02), 0.0)
+        )
+        self.fixed_command_config = self.commands_config.get('fixed_command', {})
+        self.fixed_command_enabled = bool(self.fixed_command_config.get('enabled', False))
+        self.command_velocity_scale = max(float(self.reference_gait_config.get('command_velocity_scale', 0.20)), 1e-6)
+        self.command_yaw_rate_scale = max(float(self.reference_gait_config.get('command_yaw_rate_scale', 0.30)), 1e-6)
+        self.command_turn_gain = float(np.clip(self.reference_gait_config.get('command_turn_gain', 0.28), 0.0, 0.8))
+        self.min_command_gait_scale = float(
+            np.clip(self.reference_gait_config.get('min_command_gait_scale', 0.0), 0.0, 1.0)
+        )
 
         self.control_dt = float(self.sim_config.get('control_dt', 0.05))
         self.frame_skip = int(self.mujoco_config.get('frame_skip', max(int(round(self.control_dt / 0.005)), 1)))
@@ -90,7 +121,7 @@ class MujocoBipedEnv(gym.Env):
         self.observation_space = gym.spaces.Box(
             low=-np.inf,
             high=np.inf,
-            shape=(len(self.joint_names) * 3 + 5,),
+            shape=(len(self.joint_names) * 3 + (8 if self.commands_enabled else 5),),
             dtype=np.float32,
         )
 
@@ -108,6 +139,9 @@ class MujocoBipedEnv(gym.Env):
         self.current_joint_target = self.nominal_joint_targets.copy()
         self.current_action_residual = np.zeros(len(self.joint_names), dtype=np.float32)
         self.target_base_height = self.configured_target_base_height
+        self.current_target_forward_velocity = float(self.task_config['target_forward_velocity'])
+        self.current_target_yaw_rate = float(self.task_config.get('target_yaw_rate', 0.0))
+        self.command_steps_until_resample = 0
         self.step_count = 0
         self.gait_phase = 0.0
 
@@ -159,6 +193,26 @@ class MujocoBipedEnv(gym.Env):
         ankle_bias = float(self.reference_gait_config.get('ankle_pitch_bias', 0.0))
         push_off_ankle_scale = float(self.reference_gait_config.get('push_off_ankle_scale', 0.0))
         stance_ratio = float(np.clip(self.reference_gait_config.get('stance_ratio', 0.62), 0.05, 0.95))
+        gait_scale = 1.0
+        direction_sign = 1.0
+        left_stride_scale = 1.0
+        right_stride_scale = 1.0
+
+        if self.commands_enabled:
+            # 命令条件化训练里，让参考步态跟着命令强度逐渐放大：
+            # 原地命令几乎不摆腿，前进/后退/转弯命令越强，步态引导越明显。
+            normalized_speed = abs(self.current_target_forward_velocity) / self.command_velocity_scale
+            normalized_turn = abs(self.current_target_yaw_rate) / self.command_yaw_rate_scale
+            gait_scale = float(np.clip(max(normalized_speed, normalized_turn, self.min_command_gait_scale), 0.0, 1.0))
+
+            if self.current_target_forward_velocity < -0.03:
+                direction_sign = -1.0
+
+            normalized_yaw_command = float(
+                np.clip(self.current_target_yaw_rate / self.command_yaw_rate_scale, -1.0, 1.0)
+            )
+            left_stride_scale = max(0.2, 1.0 - self.command_turn_gain * normalized_yaw_command)
+            right_stride_scale = max(0.2, 1.0 + self.command_turn_gain * normalized_yaw_command)
 
         def gait_profile(phase: float) -> tuple[float, float, float]:
             phase = phase % 1.0
@@ -180,17 +234,25 @@ class MujocoBipedEnv(gym.Env):
 
         for index, joint_name in enumerate(self.joint_names):
             if joint_name == 'left_hip_pitch_joint':
-                offsets[index] = hip_bias + hip_amplitude * left_hip_profile
+                offsets[index] = hip_bias + direction_sign * gait_scale * left_stride_scale * hip_amplitude * left_hip_profile
             elif joint_name == 'right_hip_pitch_joint':
-                offsets[index] = hip_bias + hip_amplitude * right_hip_profile
+                offsets[index] = (
+                    hip_bias + direction_sign * gait_scale * right_stride_scale * hip_amplitude * right_hip_profile
+                )
             elif joint_name == 'left_knee_pitch_joint':
-                offsets[index] = knee_bias + knee_amplitude * left_knee_profile
+                offsets[index] = knee_bias + gait_scale * left_stride_scale * knee_amplitude * left_knee_profile
             elif joint_name == 'right_knee_pitch_joint':
-                offsets[index] = knee_bias + knee_amplitude * right_knee_profile
+                offsets[index] = knee_bias + gait_scale * right_stride_scale * knee_amplitude * right_knee_profile
             elif joint_name == 'left_ankle_pitch_joint':
-                offsets[index] = ankle_bias + ankle_amplitude * left_ankle_profile
+                offsets[index] = (
+                    ankle_bias
+                    + direction_sign * gait_scale * left_stride_scale * ankle_amplitude * left_ankle_profile
+                )
             elif joint_name == 'right_ankle_pitch_joint':
-                offsets[index] = ankle_bias + ankle_amplitude * right_ankle_profile
+                offsets[index] = (
+                    ankle_bias
+                    + direction_sign * gait_scale * right_stride_scale * ankle_amplitude * right_ankle_profile
+                )
 
         return offsets
 
@@ -221,30 +283,128 @@ class MujocoBipedEnv(gym.Env):
             self.target_base_height = float(snapshot.base_position[2])
         return float(self.target_base_height)
 
+    def _base_motion_features(
+        self,
+        snapshot: RobotStateSnapshot,
+        previous_snapshot: RobotStateSnapshot,
+    ) -> tuple[float, float, float, float, float]:
+        dt = max(snapshot.sim_time - previous_snapshot.sim_time, self.control_dt, 1e-3)
+        velocity = (snapshot.base_position - previous_snapshot.base_position) / dt
+        roll, pitch, yaw = quaternion_xyzw_to_euler(snapshot.base_quaternion)
+        _, _, previous_yaw = quaternion_xyzw_to_euler(previous_snapshot.base_quaternion)
+        yaw_rate = wrap_to_pi(yaw - previous_yaw) / dt
+        return float(velocity[0]), float(velocity[1]), float(yaw_rate), float(roll), float(pitch)
+
+    def _sample_uniform_range(self, value_range: np.ndarray) -> float:
+        return float(self.np_random.uniform(float(value_range[0]), float(value_range[1])))
+
+    def _set_command_target(self, forward_velocity: float, yaw_rate: float) -> None:
+        self.current_target_forward_velocity = float(forward_velocity)
+        self.current_target_yaw_rate = float(yaw_rate)
+        self.command_steps_until_resample = int(self.command_resample_interval_steps)
+
+    def _sample_command_target(self) -> None:
+        if not self.commands_enabled:
+            self.current_target_forward_velocity = float(self.task_config['target_forward_velocity'])
+            self.current_target_yaw_rate = float(self.task_config.get('target_yaw_rate', 0.0))
+            self.command_steps_until_resample = 0
+            return
+
+        if self.fixed_command_enabled:
+            self.current_target_forward_velocity = float(
+                self.fixed_command_config.get('forward_velocity', self.task_config['target_forward_velocity'])
+            )
+            self.current_target_yaw_rate = float(
+                self.fixed_command_config.get('yaw_rate', self.task_config.get('target_yaw_rate', 0.0))
+            )
+            self.command_steps_until_resample = 0
+            return
+
+        stand_probability = self.command_stand_probability
+        turn_only_probability = self.command_turn_only_probability
+        combined_probability = self.command_combined_probability
+        translate_only_probability = max(1.0 - stand_probability - turn_only_probability - combined_probability, 0.0)
+        total_probability = stand_probability + turn_only_probability + combined_probability + translate_only_probability
+        if total_probability <= 0.0:
+            stand_probability = 0.0
+            turn_only_probability = 0.0
+            combined_probability = 0.0
+            translate_only_probability = 1.0
+            total_probability = 1.0
+
+        sample = float(self.np_random.uniform(0.0, 1.0))
+        stand_cutoff = stand_probability / total_probability
+        turn_only_cutoff = stand_cutoff + turn_only_probability / total_probability
+        translate_only_cutoff = turn_only_cutoff + translate_only_probability / total_probability
+
+        if sample < stand_cutoff:
+            self._set_command_target(0.0, 0.0)
+            return
+        if sample < turn_only_cutoff:
+            forward_velocity = float(
+                self.np_random.uniform(-self.command_turn_only_forward_abs_max, self.command_turn_only_forward_abs_max)
+            )
+            yaw_rate = self._sample_uniform_range(self.command_yaw_rate_range)
+            self._set_command_target(forward_velocity, yaw_rate)
+            return
+        if sample < translate_only_cutoff:
+            self._set_command_target(self._sample_uniform_range(self.command_forward_velocity_range), 0.0)
+            return
+
+        self._set_command_target(
+            self._sample_uniform_range(self.command_forward_velocity_range),
+            self._sample_uniform_range(self.command_yaw_rate_range),
+        )
+
+    def _advance_command_schedule(self) -> None:
+        if not self.commands_enabled or self.fixed_command_enabled or self.command_resample_interval_steps <= 0:
+            return
+
+        self.command_steps_until_resample -= 1
+        if self.command_steps_until_resample <= 0:
+            self._sample_command_target()
+
     def _build_observation(
         self,
         snapshot: RobotStateSnapshot,
         previous_snapshot: RobotStateSnapshot,
         previous_action: np.ndarray,
     ) -> np.ndarray:
-        dt = max(snapshot.sim_time - previous_snapshot.sim_time, self.control_dt, 1e-3)
-        velocity = (snapshot.base_position - previous_snapshot.base_position) / dt
-        roll, pitch, _ = quaternion_xyzw_to_euler(snapshot.base_quaternion)
+        forward_velocity, lateral_velocity, yaw_rate, roll, pitch = self._base_motion_features(
+            snapshot,
+            previous_snapshot,
+        )
+        if self.commands_enabled:
+            base_features = np.array(
+                [
+                    snapshot.base_position[2],
+                    roll,
+                    pitch,
+                    forward_velocity,
+                    lateral_velocity,
+                    yaw_rate,
+                    self.current_target_forward_velocity,
+                    self.current_target_yaw_rate,
+                ],
+                dtype=np.float32,
+            )
+        else:
+            base_features = np.array(
+                [
+                    snapshot.base_position[2],
+                    roll,
+                    pitch,
+                    forward_velocity,
+                    self.current_target_forward_velocity,
+                ],
+                dtype=np.float32,
+            )
         return np.concatenate(
             [
                 self._normalized_joint_position(snapshot.joint_position),
                 snapshot.joint_velocity,
                 previous_action,
-                np.array(
-                    [
-                        snapshot.base_position[2],
-                        roll,
-                        pitch,
-                        velocity[0],
-                        self.task_config['target_forward_velocity'],
-                    ],
-                    dtype=np.float32,
-                ),
+                base_features,
             ],
             dtype=np.float32,
         )
@@ -307,6 +467,7 @@ class MujocoBipedEnv(gym.Env):
         self.previous_snapshot = snapshot
         self.previous_action = np.zeros(len(self.joint_names), dtype=np.float32)
         self.step_count = 0
+        self._sample_command_target()
 
         if self.render_mode == 'human':
             self.render()
@@ -315,7 +476,8 @@ class MujocoBipedEnv(gym.Env):
         info = {
             'reset': True,
             'target_base_height': float(self.target_base_height),
-            'target_forward_velocity': float(self.task_config['target_forward_velocity']),
+            'target_forward_velocity': float(self.current_target_forward_velocity),
+            'target_yaw_rate': float(self.current_target_yaw_rate),
         }
         return observation, info
 
@@ -335,6 +497,8 @@ class MujocoBipedEnv(gym.Env):
         self.current_snapshot = self._extract_snapshot()
         terminated = self._terminated(self.current_snapshot)
         truncated = self.step_count >= int(self.training_config['max_episode_steps'])
+        target_forward_velocity = float(self.current_target_forward_velocity)
+        target_yaw_rate = float(self.current_target_yaw_rate)
 
         reward, reward_terms = self.reward_calculator.compute(
             RewardContext(
@@ -343,12 +507,14 @@ class MujocoBipedEnv(gym.Env):
                 action=action,
                 previous_action=self.previous_action,
                 joint_limits=self.joint_limits,
-                target_forward_velocity=float(self.task_config['target_forward_velocity']),
+                target_forward_velocity=target_forward_velocity,
+                target_yaw_rate=target_yaw_rate,
                 target_base_height=float(self.target_base_height),
                 control_dt=self.control_dt,
                 terminated=terminated,
             )
         )
+        self._advance_command_schedule()
         observation = self._build_observation(
             self.current_snapshot,
             self.previous_snapshot,
@@ -362,6 +528,8 @@ class MujocoBipedEnv(gym.Env):
         info = {
             'reward_terms': reward_terms,
             'joint_targets': joint_targets,
+            'target_forward_velocity': target_forward_velocity,
+            'target_yaw_rate': target_yaw_rate,
         }
         return observation, reward, terminated, truncated, info
 

guguji_rl/guguji_rl/evaluation.py

@@ -3,6 +3,7 @@ from __future__ import annotations
 from dataclasses import dataclass
 from pathlib import Path
 from typing import Any
+import copy
 
 import numpy as np
 
@@ -72,7 +73,19 @@ def evaluate_forward_progress(
 
     from guguji_rl.envs import build_env_from_config
 
-    env = build_env_from_config(config)
+    evaluation_config = config.get('evaluation', {})
+    command_override = evaluation_config.get('command_override', {})
+    evaluation_env_config = copy.deepcopy(config)
+    if bool(command_override.get('enabled', False)):
+        evaluation_env_config.setdefault('commands', {})
+        evaluation_env_config['commands']['enabled'] = True
+        evaluation_env_config['commands']['fixed_command'] = {
+            'enabled': True,
+            'forward_velocity': float(command_override.get('forward_velocity', 0.0)),
+            'yaw_rate': float(command_override.get('yaw_rate', 0.0)),
+        }
+
+    env = build_env_from_config(evaluation_env_config)
     summary_episodes: list[ForwardEpisodeMetrics] = []
 
     try:
@@ -96,7 +109,7 @@ def evaluate_forward_progress(
             end_x = float(env.current_snapshot.base_position[0])
             end_z = float(env.current_snapshot.base_position[2])
             delta_x = end_x - start_x
-            mean_vx = delta_x / max(steps * float(config['sim']['control_dt']), 1e-6)
+            mean_vx = delta_x / max(steps * float(evaluation_env_config['sim']['control_dt']), 1e-6)
 
             summary_episodes.append(
                 ForwardEpisodeMetrics(

guguji_rl/guguji_rl/math_utils.py

@@ -26,6 +26,15 @@ def quaternion_xyzw_to_euler(quaternion: Iterable[float]) -> tuple[float, float,
     return roll, pitch, yaw
 
 
+def wrap_to_pi(angle: float) -> float:
+    """把任意弧度角包裹到 [-pi, pi]，方便计算稳定的偏航角速度。"""
+    wrapped = (angle + math.pi) % (2.0 * math.pi) - math.pi
+    # 这里额外处理一下边界，避免出现 -pi 和 pi 来回抖动。
+    if wrapped <= -math.pi:
+        return wrapped + 2.0 * math.pi
+    return wrapped
+
+
 def euler_to_quaternion_xyzw(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
     """把欧拉角转换成 xyzw 顺序四元数。"""
     half_roll = roll * 0.5

guguji_rl/guguji_rl/rewards.py

@@ -4,7 +4,7 @@ from dataclasses import dataclass
 
 import numpy as np
 
-from .math_utils import quaternion_xyzw_to_euler
+from .math_utils import quaternion_xyzw_to_euler, wrap_to_pi
 from .state_types import RobotStateSnapshot
 from .urdf_utils import JointLimit
 
@@ -17,6 +17,7 @@ class RewardContext:
     previous_action: np.ndarray
     joint_limits: list[JointLimit]
     target_forward_velocity: float
+    target_yaw_rate: float
     target_base_height: float
     control_dt: float
     terminated: bool
@@ -32,7 +33,9 @@ class BipedRewardCalculator:
         forward_velocity = float(delta_position[0] / dt)
         lateral_velocity = float(delta_position[1] / dt)
 
-        roll, pitch, _ = quaternion_xyzw_to_euler(context.current.base_quaternion)
+        roll, pitch, yaw = quaternion_xyzw_to_euler(context.current.base_quaternion)
+        _, _, previous_yaw = quaternion_xyzw_to_euler(context.previous.base_quaternion)
+        yaw_rate = float(wrap_to_pi(yaw - previous_yaw) / dt)
         upright_reward = np.exp(-4.0 * (roll * roll + pitch * pitch))
         height_error = context.current.base_position[2] - context.target_base_height
         height_reward = np.exp(-8.0 * height_error * height_error)
@@ -40,13 +43,34 @@ class BipedRewardCalculator:
         sigma = max(float(self.reward_config['velocity_tracking_sigma']), 1e-6)
         velocity_error = forward_velocity - context.target_forward_velocity
         velocity_tracking = np.exp(-(velocity_error * velocity_error) / (2.0 * sigma * sigma))
-        # 仅奖励“真正向前”的速度，避免策略通过左右晃动或后退来钻空子。
-        positive_forward_velocity = max(forward_velocity, 0.0)
-        backward_velocity = max(-forward_velocity, 0.0)
+        yaw_sigma = max(float(self.reward_config.get('yaw_rate_tracking_sigma', 0.2)), 1e-6)
+        yaw_rate_error = yaw_rate - context.target_yaw_rate
+        yaw_rate_tracking = np.exp(-(yaw_rate_error * yaw_rate_error) / (2.0 * yaw_sigma * yaw_sigma))
+
+        command_deadband = max(float(self.reward_config.get('command_deadband', 0.03)), 1e-6)
+        if abs(context.target_forward_velocity) > command_deadband:
+            command_direction = float(np.sign(context.target_forward_velocity))
+            commanded_direction_velocity = forward_velocity * command_direction
+            wrong_way_velocity = max(-commanded_direction_velocity, 0.0)
+        else:
+            commanded_direction_velocity = abs(forward_velocity)
+            wrong_way_velocity = 0.0
+
+        if abs(context.target_yaw_rate) > command_deadband:
+            command_turn_direction = float(np.sign(context.target_yaw_rate))
+            commanded_direction_yaw_rate = yaw_rate * command_turn_direction
+        else:
+            commanded_direction_yaw_rate = abs(yaw_rate)
+
+        positive_command_progress = max(commanded_direction_velocity, 0.0)
+        positive_turn_progress = max(commanded_direction_yaw_rate, 0.0)
 
         # 如果前向速度长期太低，就给一个停滞惩罚，逼着策略去迈开步子。
         stall_velocity_threshold = float(self.reward_config.get('stall_velocity_threshold', 0.0))
-        stall_penalty = max(stall_velocity_threshold - positive_forward_velocity, 0.0)
+        if abs(context.target_forward_velocity) > command_deadband:
+            stall_penalty = max(stall_velocity_threshold - positive_command_progress, 0.0)
+        else:
+            stall_penalty = 0.0
 
         action_rate_penalty = float(np.mean(np.square(context.action - context.previous_action)))
 
@@ -89,10 +113,28 @@ class BipedRewardCalculator:
                 np.exp(-((average_knee_flexion - knee_target) ** 2) / (2.0 * knee_sigma ** 2))
             )
 
+        stand_still_reward = 0.0
+        if (
+            abs(context.target_forward_velocity) <= command_deadband
+            and abs(context.target_yaw_rate) <= command_deadband
+        ):
+            still_velocity_sigma = max(float(self.reward_config.get('still_velocity_sigma', 0.08)), 1e-6)
+            still_yaw_rate_sigma = max(float(self.reward_config.get('still_yaw_rate_sigma', 0.12)), 1e-6)
+            # 当指令要求“原地保持”时，额外鼓励机体不要自己乱漂或乱转。
+            stand_still_reward = float(
+                np.exp(
+                    -((forward_velocity * forward_velocity) / (2.0 * still_velocity_sigma * still_velocity_sigma))
+                    - ((yaw_rate * yaw_rate) / (2.0 * still_yaw_rate_sigma * still_yaw_rate_sigma))
+                )
+            )
+
         reward_terms = {
             'alive_bonus': float(self.reward_config['alive_bonus']),
             'velocity_tracking': float(self.reward_config['velocity_tracking_scale']) * float(velocity_tracking),
-            'forward_progress': float(self.reward_config.get('forward_progress_scale', 0.0)) * positive_forward_velocity,
+            'yaw_rate_tracking': float(self.reward_config.get('yaw_rate_tracking_scale', 0.0)) * float(yaw_rate_tracking),
+            'forward_progress': float(self.reward_config.get('forward_progress_scale', 0.0)) * positive_command_progress,
+            'turn_progress': float(self.reward_config.get('turn_progress_scale', 0.0)) * positive_turn_progress,
+            'command_stillness': float(self.reward_config.get('command_stillness_scale', 0.0)) * stand_still_reward,
             'hip_alternation': float(self.reward_config.get('hip_alternation_scale', 0.0)) * hip_alternation_reward,
             'knee_flexion': float(self.reward_config.get('knee_flexion_scale', 0.0)) * knee_flexion_reward,
             'upright': float(self.reward_config['upright_scale']) * float(upright_reward),
@@ -100,7 +142,9 @@ class BipedRewardCalculator:
             'action_rate_penalty': -float(self.reward_config['action_rate_penalty_scale']) * action_rate_penalty,
             'joint_limit_penalty': -float(self.reward_config['joint_limit_penalty_scale']) * joint_limit_penalty,
             'lateral_velocity_penalty': -float(self.reward_config['lateral_velocity_penalty_scale']) * abs(lateral_velocity),
-            'backward_velocity_penalty': -float(self.reward_config.get('backward_velocity_penalty_scale', 0.0)) * backward_velocity,
+            # 这里延续旧字段名 backward_velocity_penalty_scale，
+            # 但在命令条件化场景里，它表达的是“沿指令反方向运动”的惩罚。
+            'backward_velocity_penalty': -float(self.reward_config.get('backward_velocity_penalty_scale', 0.0)) * wrong_way_velocity,
             'stall_penalty': -float(self.reward_config.get('stall_penalty_scale', 0.0)) * stall_penalty,
         }
 
@@ -112,6 +156,9 @@ class BipedRewardCalculator:
             reward_terms['fall_penalty'] = 0.0
 
         reward_terms['forward_velocity'] = forward_velocity
+        reward_terms['yaw_rate'] = yaw_rate
+        reward_terms['target_forward_velocity'] = float(context.target_forward_velocity)
+        reward_terms['target_yaw_rate'] = float(context.target_yaw_rate)
         reward_terms['roll'] = float(roll)
         reward_terms['pitch'] = float(pitch)
         reward_terms['base_height'] = float(context.current.base_position[2])

guguji_rl/scripts/check_env.py

@@ -43,6 +43,10 @@ def main() -> int:
     print('reset ok')
     print(f'observation shape: {observation.shape}')
     print(f'target_base_height: {info["target_base_height"]:.4f}')
+    if 'target_forward_velocity' in info:
+        print(f'target_forward_velocity: {float(info["target_forward_velocity"]):.3f}')
+    if 'target_yaw_rate' in info:
+        print(f'target_yaw_rate: {float(info["target_yaw_rate"]):.3f}')
 
     for step_index in range(args.steps):
         action = np.zeros(env.action_space.shape[0], dtype=np.float32)
@@ -51,6 +55,9 @@ def main() -> int:
         print(
             f'step={step_index} reward={reward:.3f} '
             f'vx={reward_terms["forward_velocity"]:.3f} '
+            f'yaw_rate={reward_terms.get("yaw_rate", 0.0):.3f} '
+            f'cmd_vx={reward_terms.get("target_forward_velocity", 0.0):.3f} '
+            f'cmd_yaw={reward_terms.get("target_yaw_rate", 0.0):.3f} '
             f'base_z={reward_terms["base_height"]:.3f}'
         )
         if terminated or truncated:

guguji_rl/scripts/evaluate_forward_progress.py

@@ -27,6 +27,8 @@ def parse_args() -> argparse.Namespace:
         action='store_true',
         help='是否强制使用确定性动作，默认读取配置文件',
     )
+    parser.add_argument('--command-forward', type=float, default=None, help='可选覆盖评估时的目标前进速度')
+    parser.add_argument('--command-yaw', type=float, default=None, help='可选覆盖评估时的目标偏航角速度')
     return parser.parse_args()
 
 
@@ -35,6 +37,13 @@ def main() -> int:
 
     config = load_config(resolve_input_path(SCRIPT_ROOT, args.config))
     evaluation_config = config['evaluation']
+    if args.command_forward is not None or args.command_yaw is not None:
+        evaluation_config.setdefault('command_override', {})
+        evaluation_config['command_override']['enabled'] = True
+        if args.command_forward is not None:
+            evaluation_config['command_override']['forward_velocity'] = float(args.command_forward)
+        if args.command_yaw is not None:
+            evaluation_config['command_override']['yaw_rate'] = float(args.command_yaw)
     episodes = args.episodes if args.episodes is not None else int(evaluation_config['forward_progress_episodes'])
     max_steps = args.max_steps if args.max_steps is not None else int(evaluation_config['forward_progress_max_steps'])
     deterministic = args.deterministic or bool(evaluation_config['forward_progress_deterministic'])

guguji_rl/scripts/run_policy.py

@@ -32,6 +32,8 @@ def parse_args() -> argparse.Namespace:
         action='store_true',
         help='如果当前后端是 MuJoCo，则以交互窗口方式显示策略回放',
     )
+    parser.add_argument('--command-forward', type=float, default=None, help='可选固定策略回放时的目标前进速度')
+    parser.add_argument('--command-yaw', type=float, default=None, help='可选固定策略回放时的目标偏航角速度')
     return parser.parse_args()
 
 
@@ -59,6 +61,17 @@ def main() -> int:
         config = copy.deepcopy(config)
         config.setdefault('mujoco', {})
         config['mujoco']['render_mode'] = 'human'
+    if args.command_forward is not None or args.command_yaw is not None:
+        config = copy.deepcopy(config)
+        config.setdefault('commands', {})
+        config['commands']['enabled'] = True
+        fixed_command = dict(config['commands'].get('fixed_command', {}))
+        fixed_command['enabled'] = True
+        if args.command_forward is not None:
+            fixed_command['forward_velocity'] = float(args.command_forward)
+        if args.command_yaw is not None:
+            fixed_command['yaw_rate'] = float(args.command_yaw)
+        config['commands']['fixed_command'] = fixed_command
 
     env = build_env_from_config(config)
     model = PPO.load(resolve_input_path(args.model))

guguji_rl/scripts/train.py

@@ -90,6 +90,69 @@ def sanitize_stage_name(stage_name: str) -> str:
     return sanitized.strip('_') or 'stage'
 
 
+def deep_merge_stage_override(base: dict[str, Any], override: dict[str, Any]) -> None:
+    for key, value in override.items():
+        if isinstance(value, dict) and isinstance(base.get(key), dict):
+            deep_merge_stage_override(base[key], value)
+        else:
+            base[key] = copy.deepcopy(value)
+
+
+def format_stage_target_summary(stage_config: dict[str, Any]) -> str:
+    commands_config = stage_config.get('commands', {})
+    if bool(commands_config.get('enabled', False)):
+        forward_velocity_range = commands_config.get('forward_velocity_range', [0.0, 0.0])
+        yaw_rate_range = commands_config.get('yaw_rate_range', [0.0, 0.0])
+        return (
+            'command_conditioned '
+            f'vx=[{float(forward_velocity_range[0]):.2f}, {float(forward_velocity_range[1]):.2f}] '
+            f'yaw=[{float(yaw_rate_range[0]):.2f}, {float(yaw_rate_range[1]):.2f}]'
+        )
+    return f'target_forward_velocity={float(stage_config["task"]["target_forward_velocity"]):.2f}'
+
+
+def warm_start_policy_from_checkpoint(
+    *,
+    ppo_class: type,
+    model: object,
+    init_model_path: Path,
+    device: str,
+) -> None:
+    try:
+        model.set_parameters(
+            str(init_model_path),
+            exact_match=False,
+            device=device,
+        )
+        print(f'已加载课程初始化模型: {init_model_path}')
+        return
+    except Exception as error:
+        print(f'完整参数加载失败，将改用兼容 warm start: {error}')
+
+    source_model = ppo_class.load(str(init_model_path), device=device)
+    source_state_dict = source_model.policy.state_dict()
+    target_state_dict = model.policy.state_dict()
+    matched_keys: list[str] = []
+    skipped_keys: list[str] = []
+
+    for key, source_value in source_state_dict.items():
+        target_value = target_state_dict.get(key)
+        if target_value is None or tuple(target_value.shape) != tuple(source_value.shape):
+            skipped_keys.append(key)
+            continue
+        target_state_dict[key] = source_value.detach().clone()
+        matched_keys.append(key)
+
+    model.policy.load_state_dict(target_state_dict, strict=False)
+    print(
+        '已完成兼容 warm start: '
+        f'匹配 {len(matched_keys)} 个张量，'
+        f'跳过 {len(skipped_keys)} 个形状不兼容张量。'
+    )
+    if skipped_keys:
+        print(f'跳过的典型张量: {", ".join(skipped_keys[:4])}')
+
+
 def build_curriculum_stage_configs(config: dict[str, Any]) -> list[tuple[str | None, dict[str, Any]]]:
     """把课程学习阶段展开成一组可直接训练的独立配置。"""
     raw_stages = config['training'].get('curriculum_stages') or []
@@ -106,6 +169,11 @@ def build_curriculum_stage_configs(config: dict[str, Any]) -> list[tuple[str | N
         stage_config = copy.deepcopy(config)
         stage_config['training'].pop('curriculum_stages', None)
 
+        for section_name in ('task', 'commands', 'rewards', 'robot', 'sim', 'mujoco', 'evaluation', 'training'):
+            section_override = raw_stage.get(section_name)
+            if isinstance(section_override, dict):
+                deep_merge_stage_override(stage_config[section_name], section_override)
+
         raw_name = str(raw_stage.get('name') or f'stage_{stage_index}')
         stage_name = f'{stage_index:02d}_{sanitize_stage_name(raw_name)}'
 
@@ -113,6 +181,8 @@ def build_curriculum_stage_configs(config: dict[str, Any]) -> list[tuple[str | N
         # 这样 walking 阶段就能从慢到快逐段抬升，而不用一次把目标速度顶太高。
         if 'target_forward_velocity' in raw_stage:
             stage_config['task']['target_forward_velocity'] = float(raw_stage['target_forward_velocity'])
+        if 'target_yaw_rate' in raw_stage:
+            stage_config['task']['target_yaw_rate'] = float(raw_stage['target_yaw_rate'])
         if 'total_timesteps' in raw_stage:
             stage_config['training']['total_timesteps'] = int(raw_stage['total_timesteps'])
         if 'initial_log_std' in raw_stage:
@@ -181,7 +251,7 @@ def main() -> int:
         if stage_name is not None:
             print(
                 f'开始课程阶段 {stage_index}/{len(stage_configs)}: {stage_name} '
-                f'(target_forward_velocity={stage_config["task"]["target_forward_velocity"]:.2f}, '
+                f'({format_stage_target_summary(stage_config)}, '
                 f'timesteps={int(stage_config["training"]["total_timesteps"])})'
             )
 
@@ -219,14 +289,15 @@ def main() -> int:
                 init_model_path = stage_config['training'].get('init_model_path')
                 if init_model_path:
                     resolved_init_model_path = resolve_input_path(str(init_model_path))
-                    # 这里不是直接 load 整个 PPO 对象，而是把旧模型参数灌入新模型。
-                    # 好处是：我们仍然使用当前配置文件里的超参数，只复用之前学到的策略权重。
-                    model.set_parameters(
-                        str(resolved_init_model_path),
-                        exact_match=False,
+                    # 如果 observation 维度还没变，这里会完整复用旧权重。
+                    # 如果我们给新任务增加了命令维度，这里会自动退化成“兼容 warm start”，
+                    # 尽量保留平衡模型里已经学到的站立/稳定控制能力。
+                    warm_start_policy_from_checkpoint(
+                        ppo_class=PPO,
+                        model=model,
+                        init_model_path=resolved_init_model_path,
                         device=stage_config['training']['device'],
                     )
-                    print(f"已加载课程初始化模型: {resolved_init_model_path}")
             else:
                 model.set_env(env)