Improve VBot navigation robustness via domain randomization and reward reweighting

[Copilot]

VBot robot dog exhibits 30-40% success rate on navigation tasks due to instability - small perturbations cause falls. Target: 70-80% success with <32° body tilt tolerance.

Changes

Domain Randomization (cfg.py)

Added DomainRandomization dataclass with systematic parameter variation:

• Robot dynamics: mass ±20%, friction ±50%, damping ±20%
• Environment: gravity ±10%, lateral wind ±0.1N
• Initial conditions: joint position ±0.05rad, velocity ±0.02rad/s, random push 30% probability

@dataclass
class DomainRandomization:
    init_qpos_noise_scale: float = 0.05
    init_qvel_noise_scale: float = 0.02
    random_push_prob: float = 0.3
    random_push_scale: float = 0.5  # m/s

Reward Function Reweighting (cfg.py)

Increased stability penalties to prioritize posture control:

• orientation: -0.05 → -0.20 (4x increase)
• Added lin_vel_z: -0.30 (suppress vertical oscillation)
• Added ang_vel_xy: -0.15 (penalize roll/pitch rates)
• Added contact_stability: 0.1 (reward 2+ feet on ground)
• Added action_smoothness: -0.01 (penalize jerky motions)

Implementation (vbot_section001_np.py)

reset(): Apply randomization to 12 actuated DOFs

if hasattr(cfg, 'domain_randomization'):
    dr = cfg.domain_randomization
    qpos_noise = np.random.uniform(-dr.init_qpos_noise_scale, 
                                    dr.init_qpos_noise_scale, 
                                    (num_envs, 12))
    dof_pos[:, -12:] += qpos_noise

_compute_reward(): Integrate new stability terms with configurable weights

reward = (
    progress_reward + arrival_bonus + velocity_reward
    + orientation_penalty * reward_scales["orientation"]      # -0.20
    + lin_vel_z_penalty * reward_scales["lin_vel_z"]         # -0.30
    + ang_vel_xy_penalty * reward_scales["ang_vel_xy"]       # -0.15
    + contact_stability_reward * reward_scales["contact_stability"]  # 0.1
    + action_diff * reward_scales["action_smoothness"]       # -0.01
)

Testing & Validation

• verify_implementation.sh: Grep-based code verification (no runtime dependencies)
• test_robustness.py: Success rate and stability angle measurement across trials
• validate_config.py: Configuration parameter validation

Expected Impact

• Success rate: 30-40% → 70-80% (+40pp)
• Body tilt stability: unstable → <32°
• Compute overhead: +10% (acceptable for robustness gain)

Notes

• Backward compatible: old configs remain functional via hasattr() checks
• Graceful degradation: foot contact sensors fall back to zero reward on failure
• All randomization parameters exposed in config for tuning

Original prompt

问题描述

VBot 机器狗模型虽然已收敛并具备行走能力，但在测试集上的鲁棒性不足：

• 10次测试中仅有 3-4 次成功完成
• 其余均因步态失稳导致跌倒
• 容错能力差，小扰动即导致失败

优化目标

通过引入域随机化（Domain Randomization）和调整姿态惩罚权重来提升鲁棒性：

• 成功率目标：70-80%（从 30-40% 提升）
• 稳定性指标：身体倾斜角控制在 ±32° 以内
• 步态平稳性：足部接触稳定，运动平滑

修复内容

1️⃣ 配置层优化 (cfg.py)

1.1 添加域随机化配置

@dataclass
class DomainRandomization:
    # 机器人参数随机化
    mass_scale_range = [0.8, 1.2]              # 质量 ±20%
    friction_scale_range = [0.5, 1.5]          # 摩擦系数 ±50%
    dof_damping_scale_range = [0.8, 1.2]       # 关节阻尼 ±20%
    
    # 环境参数随机化
    gravity_scale_range = [0.9, 1.1]           # 重力加速度 ±10%
    wind_force_range = [-0.1, 0.1]             # 侧风 ±0.1N
    
    # 初始条件随机化
    init_qpos_noise_scale = 0.05                # 初始关节位置噪声
    init_qvel_noise_scale = 0.02                # 初始速度噪声
    random_push_prob = 0.3                      # 30%概率随机推力
    random_push_scale = 0.5                     # 推力大小 ±0.5m/s

1.2 调整奖励函数权重（姿态稳定性提升）

@dataclass
class RewardConfig:
    scales: dict[str, float] = field(
        default_factory=lambda: {
            # ===== 导航任务核心奖励（保持不变）=====
            "position_tracking": 2.0,
            "fine_position_tracking": 2.0,
            "heading_tracking": 1.0,
            "forward_velocity": 0.5,

            # ===== 姿态稳定性奖励（权重提升 4倍）=====
            "orientation": -0.20,               # 从 -0.05 → -0.20 ⬆️
            "lin_vel_z": -0.30,                 # 新增：Z轴垂直速度惩罚 ⬆️
            "ang_vel_xy": -0.15,                # 新增：横滚/俯仰角速度惩罚 ⬆️
            
            # ===== 新增：步态稳定性奖励 =====
            "foot_air_time": 0.1,               # 鼓励规律足部触地
            "contact_stability": 0.1,           # 奖励稳定接触
            "action_smoothness": -0.01,         # 平滑动作过渡
        }
    )

2️⃣ 环境实现层 (vbot_section001_np.py)

2.1 在 reset() 中实现域随机化

def reset(self, data: mtx.SceneData, done: np.ndarray = None):
    cfg: VBotSection001EnvCfg = self._cfg
    num_envs = data.shape[0]
    
    # ... 既有初始化代码 ...
    
    # ===== 域随机化：初始条件噪声 =====
    if hasattr(cfg, 'domain_randomization'):
        # 初始关节位置噪声
        qpos_noise = np.random.uniform(
            -cfg.domain_randomization.init_qpos_noise_scale,
            cfg.domain_randomization.init_qpos_noise_scale,
            (num_envs, dof_pos.shape[1])
        )
        dof_pos += qpos_noise
        
        # 初始速度噪声
        qvel_noise = np.random.uniform(
            -cfg.domain_randomization.init_qvel_noise_scale,
            cfg.domain_randomization.init_qvel_noise_scale,
            (num_envs, dof_vel.shape[1])
        )
        dof_vel += qvel_noise
        
        # 随机推力（30%概率）
        if np.random.rand() < cfg.domain_randomization.random_push_prob:
            push_xy = np.random.uniform(
                -cfg.domain_randomization.random_push_scale,
                cfg.domain_randomization.random_push_scale,
                (num_envs, 2)
            )
            dof_vel[:, 3:5] = push_xy

2.2 添加新奖励项计算

def _compute_reward(self, state: NpEnvState) -> np.ndarray:
    """计算包含鲁棒性奖励的总奖励"""
    data = state.data
    num_envs = self._num_envs
    
    # ... 既有奖励计算 ...
    
    # ===== 新增：足部接触稳定性 =====
    foot_contacts = self._model.get_contact_query(data).is_colliding(
        self.foot_contact_check
    )
    stable_contacts = np.sum(foot_contacts.reshape(num_envs, 4), axis=1) >= 2
    contact_stability_reward = (
        stable_contacts.astype(np.float32) * 
        self.cfg.reward_config.scales.get("contact_stability", 0.1)
    )
    
    # ===== 新增：动作平滑性 =====
    last_actions = state.info.get("last_actions", np.zeros((num_envs, self._num_action)))
    current_actions = state.action
    action_diff = np.mean(np.abs(current_actions - last_actions), axis=1)
    action_smoothness_reward = (
        -action_diff * self.cfg.reward_config.scales.get("action_smoothness", 0.01)
    )
    
    # ===== 新增：Z轴线速度和XY角速度惩罚 =====
    root_vel = self._extract_root_state(data)[2]  # 获取速度
    lin_vel_z_penalty = np.square(root_vel[:, 2])
    
    gyro = self._model.get_sensor_value(self._cfg.sensor.base_gyro, data)
    ang_vel_xy_penalty = np.sum(np.square(gyro[:, :2]), axis=1)
    
    # 合并所有奖励
    total_reward = (
        position_reward + orientation_reward + 
        contact_stability_reward + action_smoothness_reward -
        lin_vel_z_penalty * self.cfg.reward_config.scales.get("lin_vel_z", 0.3) -
        ang_vel_xy_penalty * self.cfg.reward_config.scales.get("ang_vel_xy", 0.15)
    )
    
    return total_reward

2.3 步态恢复机制（可选高级功能）

def _apply_recovery_control(self, state: NpEnvState) -> np.ndarray:
    """
    当机器狗倾斜过度时，自动触发恢复动作
    """
    data = state.data
    root_pos, root_quat, root_vel = self._extract_root_state(data)
    
    # 计算倾斜角度
    gravity = self._compute_projected_gravity(root_quat)
    
    # 检测倾斜：如果重力向量偏离竖直方向超过20度
    is_tilted = np.sqrt(gravity[:, 0]**2 + gravity[:, 1]**2) > np.sin(np.deg2rad(20))
    
    if np.any(is_tilted):
        recover...

</details>



<!-- START COPILOT CODING AGENT SUFFIX -->

*This pull request was created from Copilot chat.*
>

<!-- START COPILOT CODING AGENT TIPS -->
---

💡 You can make Copilot smarter by setting up custom instructions, customizing its development environment and configuring Model Context Protocol (MCP) servers. Learn more [Copilot coding agent tips](https://gh.io/copilot-coding-agent-tips) in the docs.