Backup DreamWaQ rslrl stability fixes

motrix_envs/src/motrix_envs/locomotion/go1/dreamwaq.py

@@ -121,18 +121,20 @@ class DreamWaQCfg(Go1WalkNpEnvCfg):
             "tracking_lin_vel": 1.5,
             "tracking_ang_vel": 1.0,
             "lin_vel_z": -2.0,
-            "ang_vel_xy": -0.05,
-            "orientation": -0.2,
+            "ang_vel_xy": -0.10,   # 抑制晃动
+            "orientation": -0.5,   # 强制平稳姿态
             "dof_acc": -2.5e-7,
-            "base_height": -1.0,
+            "base_height": -10.0,  # 强惩罚身高偏差，避免蹲伏或踮脚
             "feet_air_time": 0.1,
             "action_rate": -0.01,
             "joint_power": -2e-5,
-            "smoothness": -0.01,
+            "smoothness": -0.02,
             "power_distribution": -10e-6,
-            # 注意：stand_still 在上游被注释掉
+            "stand_still": -0.5,
+            "dof_pos_limits": -5.0,  # 关节限位软约束（参考 M20 修改版）
+            "collision": -1.0,       # 惩罚身体碰撞
         })
-        self.reward_config.only_positive_rewards = True
+        self.reward_config.only_positive_rewards = False  # 让坏行为负反馈直达策略
         self.reward_config.tracking_sigma = 0.25
 
 
@@ -181,12 +183,25 @@ class DreamWaQTask(Go1WalkTask):
     # ── 动作裁剪 + 力矩计算 ──
 
     def apply_action(self, actions, state):
-        """裁剪动作防止奖励计算溢出（上游 clip_actions=100）。"""
-        actions = np.clip(actions, -100.0, 100.0)
-        return super().apply_action(actions, state)
+        """裁剪动作 + 随机延迟（模拟真实部署延迟，提高 sim-to-real 泛化）。"""
+        actions = np.clip(actions, -4.0, 4.0)
+        # 随机动作延迟：0-3 个控制步（0-60ms）
+        if not hasattr(self, '_action_buffer'):
+            self._action_buffer = np.zeros((self._num_envs, 3, self._num_action), dtype=np.float32)
+            self._latency_steps = np.random.randint(0, 4, size=self._num_envs)
+        # 滑动缓冲区，新动作插入末尾
+        self._action_buffer = np.concatenate([
+            self._action_buffer[:, 1:, :], actions[:, np.newaxis, :]
+        ], axis=1)
+        # 根据每个 env 的延迟取对应历史动作
+        delayed_actions = np.zeros_like(actions)
+        for i in range(self._num_envs):
+            lat = self._latency_steps[i]
+            delayed_actions[i] = self._action_buffer[i, 2 - lat]  # buffer[-1] 是最新
+        return super().apply_action(delayed_actions, state)
 
     def _compute_torques(self, actions, data):
-        """PD 控制器 + 域随机化。力矩裁剪防溢出。"""
+        """PD 控制器 + 域随机化 + 关节限位。力矩裁剪防溢出。"""
         state = getattr(self, '_state', None)
         if state is not None:
             motor_strength = state.info.get("motor_strength",
@@ -201,8 +216,13 @@ class DreamWaQTask(Go1WalkTask):
             kd_factor = np.ones(1, dtype=np.float32)
 
         actions_scaled = actions * self.cfg.control_config.action_scale * motor_strength[:, np.newaxis]
+        # 目标角度限制在关节范围内（与 MuJoCo 硬限位对齐）
+        target = actions_scaled + self.default_angles
+        lo = self._model.joint_limits[0][np.newaxis, :]  # (1, 12)
+        hi = self._model.joint_limits[1][np.newaxis, :]
+        target = np.clip(target, lo, hi)
         torques = (self.kps * kp_factor[:, np.newaxis]) * (
-            actions_scaled + self.default_angles - self.get_dof_pos(data)
+            target - self.get_dof_pos(data)
         ) - (self.kds * kd_factor[:, np.newaxis]) * self.get_dof_vel(data)
         return np.clip(torques, -80.0, 80.0)
 
@@ -326,14 +346,31 @@ class DreamWaQTask(Go1WalkTask):
 
     def update_observation(self, state):
         data = state.data
-        # 清理物理崩溃残留的 NaN/Inf（仅在检测到时才修改，避免无谓开销）
+        # 清理物理崩溃残留的 NaN/Inf
         dv = data.dof_vel
         if np.any(~np.isfinite(dv)):
-            data.set_dof_vel(np.nan_to_num(np.array(dv), nan=0.0, posinf=0.0, neginf=0.0))
+            dv_clean = np.array(dv)
+            dv_clean[:, 6:] = np.nan_to_num(dv_clean[:, 6:], nan=0.0, posinf=0.0, neginf=0.0)
+            data.set_dof_vel(dv_clean)
         dp = data.dof_pos
         if np.any(~np.isfinite(dp)):
-            data.set_dof_pos(np.nan_to_num(np.array(dp), nan=0.0, posinf=0.0, neginf=0.0), self._model)
+            dp_clean = np.array(dp)
+            # 四元数 NaN → 整行替换为单位四元数 [0,0,0,1]
+            quat_nan_row = np.any(~np.isfinite(dp_clean[:, 3:7]), axis=1)
+            if np.any(quat_nan_row):
+                dp_clean[quat_nan_row, 3:7] = [0.0, 0.0, 0.0, 1.0]
+            # 关节位置 NaN → 0
+            dp_clean[:, 7:] = np.nan_to_num(dp_clean[:, 7:], nan=0.0, posinf=0.0, neginf=0.0)
+            data.set_dof_pos(dp_clean, self._model)
+        else:
+            quat_norm = np.linalg.norm(dp[:, 3:7], axis=1)
+            bad_quat = quat_norm < 1e-6
+            if np.any(bad_quat):
+                dp_clean = np.array(dp)
+                dp_clean[bad_quat, 3:7] = [0.0, 0.0, 0.0, 1.0]
+                data.set_dof_pos(dp_clean, self._model)
         obs = self._get_obs(data, state.info)
+        obs = np.nan_to_num(obs, nan=0.0, posinf=0.0, neginf=0.0)
 
         # 更新历史缓冲区：CENet 用不含当前帧的历史预测下一帧（论文 LVAE = MSE(õ_{t+1}, o_{t+1})）
         full_hist = state.info.get("obs_history_full",
@@ -360,8 +397,10 @@ class DreamWaQTask(Go1WalkTask):
         if do_rand.any():
             n_rand = int(do_rand.sum())
             state.info["motor_strength"][do_rand] = np.random.uniform(0.9, 1.1, size=n_rand).astype(np.float32)
-            state.info["kp_factor"][do_rand] = np.random.uniform(0.9, 1.1, size=n_rand).astype(np.float32)
-            state.info["kd_factor"][do_rand] = np.random.uniform(0.9, 1.1, size=n_rand).astype(np.float32)
+            state.info["kp_factor"][do_rand] = np.random.uniform(0.85, 1.15, size=n_rand).astype(np.float32)
+            state.info["kd_factor"][do_rand] = np.random.uniform(0.85, 1.15, size=n_rand).astype(np.float32)
+            # 动作延迟随机化（每个 rand 周期重新分配延迟步数）
+            self._latency_steps[do_rand] = np.random.randint(0, 4, size=n_rand)
             state.info["friction_coeff"][do_rand] = np.random.uniform(0.2, 1.25, size=n_rand).astype(np.float32)
             state.info["added_mass"][do_rand] = np.random.uniform(-1.0, 2.0, size=n_rand).astype(np.float32)
             state.info["com_displacement"][do_rand] = np.random.uniform(-0.05, 0.05, size=(n_rand, 3)).astype(np.float32)
@@ -369,6 +408,8 @@ class DreamWaQTask(Go1WalkTask):
 
         # 特权观测
         state.info["privileged_obs"] = self._get_privileged_obs(data, obs)
+        state.info["privileged_obs"] = np.nan_to_num(
+            state.info["privileged_obs"], nan=0.0, posinf=0.0, neginf=0.0)
         state.info["base_vel"] = self.get_local_linvel(data)
 
         # 足部接触
@@ -429,7 +470,9 @@ class DreamWaQTask(Go1WalkTask):
                     cy = half_y - self._border - row * self._cell_size - self._cell_size / 2
                     all_origins[row, col] = [cx, cy]
             self._terrain_origins = all_origins
-            self._max_init_level = 0  # 上游原值
+            # 跟踪课程进度：动态 max_init_level，避免将已学会的机器人拉回平地
+            self._max_init_level = 0
+            self._level_history = []
 
         if num_reset > 0 and self._init_done and state is not None and hasattr(state, 'info'):
             old_info = state.info
@@ -439,16 +482,117 @@ 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)
-            move_up = distance > 3.0
-            cmd_speed = np.linalg.norm(old_commands[:, :2], axis=1)
-            required_dist = cmd_speed * (self.cfg.max_episode_steps * self.cfg.ctrl_dt) * 0.5
-            move_down = (distance < required_dist) & ~move_up
+            # ── 多条件 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]
+            ep_steps = old_info.get("ep_steps", np.zeros(self._num_envs, dtype=np.int32))[done]
+            mask = ep_steps > 0
+            avg_tracking = np.zeros(num_reset, dtype=np.float32)
+            avg_tracking[mask] = ep_tracking[mask] / ep_steps[mask].astype(np.float32)
+            # 调试：打印 avg_tracking 分布（单次）
+            if not hasattr(self, '_dbg_tracking_printed'):
+                self._dbg_tracking_printed = True
+                valid = avg_tracking[mask]
+                if len(valid) > 0:
+                    print(f"[Curric-DBG] avg_tracking: mean={np.mean(valid):.4f} "
+                          f"min={np.min(valid):.4f} p10={np.percentile(valid,10):.4f} "
+                          f"p50={np.percentile(valid,50):.4f} p90={np.percentile(valid,90):.4f} "
+                          f"max={np.max(valid):.4f} >0.5={100*np.mean(valid>0.5):.0f}%")
+            # 2) 存活率: 必须自然结束(truncated), 而非摔倒终止(terminated)
+            term = state.terminated[done] if hasattr(state, 'terminated') else np.zeros(num_reset, dtype=bool)
+            not_fallen = ~term  # True=活到 episode 结束
+            # 3) 姿态: avg_orient 保留 dt，-0.0005 对应等效倾角约 13 deg
+            ep_orient = old_info.get("ep_orientation", np.zeros(self._num_envs, dtype=np.float32))[done]
+            avg_orient = np.zeros(num_reset, dtype=np.float32)
+            avg_orient[mask] = ep_orient[mask] / ep_steps[mask].astype(np.float32)
+            # 4) 身高: avg_base_height 保留 dt，-0.001 对应 RMS 高度误差约 7cm
+            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²
+            move_up_raw = (
+                (avg_tracking > 0.5)
+                & not_fallen
+                & (avg_orient > -0.0005)
+                & (avg_bh > -0.001)
+            )
+            # ── 渐进升级：连续通过 3 次 + 冷却期 + 每次只升 1 级 ──
+            if not hasattr(self, '_consecutive_pass_count'):
+                self._consecutive_pass_count = np.zeros(self._num_envs, dtype=np.int32)
+                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）
+            self._consecutive_pass_count[done_idx] = np.where(
+                move_up_raw,
+                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)
+            streak_ready = self._consecutive_pass_count[done_idx] >= 3
+            cooldown_blocked = streak_ready & (self._upgrade_cooldown[done_idx] > 0)
+            # 连续 3 次通过 且 不在冷却期 → 升级（取被 reset env 的当前状态）
+            can_upgrade = streak_ready & (self._upgrade_cooldown[done_idx] == 0)
+            # 升级后设置 5 episode 冷却期，并从新等级重新累计连续通过次数。
+            self._upgrade_cooldown[done_idx] = np.where(
+                can_upgrade, 5, self._upgrade_cooldown[done_idx])
+            self._consecutive_pass_count[done_idx] = np.where(
+                can_upgrade, 0, self._consecutive_pass_count[done_idx])
+
+            move_up = can_upgrade
+            move_down_raw = (
+                (~not_fallen)
+                | (avg_tracking < 0.35)
+                | (avg_orient <= -0.001)
+                | (avg_bh <= -0.002)
+            )
+            move_down = move_down_raw & ~move_up
             new_levels = np.where(move_up, old_levels + 1, old_levels)
             new_levels = np.where(move_down, new_levels - 1, new_levels)
-            at_max = new_levels >= self._num_rows
-            if at_max.any():
-                new_levels[at_max] = np.random.randint(0, self._num_rows, size=int(at_max.sum()))
             new_levels = np.clip(new_levels, 0, self._num_rows - 1)
+            # 跟踪 level 分布，动态更新 max_init_level（P90 - 2）
+            self._level_history.extend(new_levels.tolist())
+            if len(self._level_history) > 500:
+                self._level_history = self._level_history[-500:]
+            if len(self._level_history) >= 10:
+                sorted_lv = sorted(self._level_history)
+                self._max_init_level = max(0, sorted_lv[int(len(sorted_lv) * 0.9)] - 2)
+            # ── 调试日志: curriculum 各条件通过率 ──
+            if not hasattr(self, '_curric_log_counter'):
+                self._curric_log_counter = 0
+                self._curric_track_ok = 0; self._curric_survive_ok = 0
+                self._curric_orient_ok = 0; self._curric_bh_ok = 0
+                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_log_counter += 1
+            self._curric_track_ok += int((avg_tracking > 0.5).sum())
+            self._curric_survive_ok += int(not_fallen.sum())
+            self._curric_orient_ok += int((avg_orient > -0.0005).sum())
+            self._curric_bh_ok += int((avg_bh > -0.001).sum())
+            self._curric_raw_all_ok += int(move_up_raw.sum())
+            self._curric_streak_ok += int(streak_ready.sum())
+            self._curric_upgrade_ok += int(move_up.sum())
+            self._curric_cooldown_block += int(cooldown_blocked.sum())
+            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_total += num_reset
+            if self._curric_log_counter % 50 == 0:
+                def pct(n): return 100*n/max(self._curric_total,1)
+                print(f"[Curric] n={self._curric_total:5d} "
+                      f"track={pct(self._curric_track_ok):.0f}% "
+                      f"survive={pct(self._curric_survive_ok):.0f}% "
+                      f"orient={pct(self._curric_orient_ok):.0f}% "
+                      f"bh={pct(self._curric_bh_ok):.0f}% "
+                      f"rawALL={pct(self._curric_raw_all_ok):.0f}% "
+                      f"streak3={pct(self._curric_streak_ok):.0f}% "
+                      f"up={pct(self._curric_upgrade_ok):.0f}% "
+                      f"cool={pct(self._curric_cooldown_block):.0f}% "
+                      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"max_init={self._max_init_level}")
         else:
             new_levels = np.random.randint(0, self._max_init_level + 1, size=num_reset, dtype=np.int32)
         self._init_done = True
@@ -545,12 +689,32 @@ class DreamWaQTask(Go1WalkTask):
 
         return obs, info
 
+    # ── 新增奖励（参考 M20 修改版）──
+
+    def _reward_dof_pos_limits(self, data):
+        """关节软限位惩罚——接近关节极限时扣分。"""
+        lo = self._model.joint_limits[0]
+        hi = self._model.joint_limits[1]
+        margin = 0.1  # rad 软边界
+        pos = self.get_dof_pos(data)
+        lower = np.clip(lo + margin - pos, 0, None)
+        upper = np.clip(pos - (hi - margin), 0, None)
+        return np.sum(lower + upper, axis=1)
+
+    def _reward_collision(self, data):
+        """身体碰撞惩罚——膝/肩触地扣分。"""
+        cquerys = self._model.get_contact_query(data)
+        penal = cquerys.is_colliding(self.termination_check)
+        return np.any(penal.reshape(self._num_envs, -1), axis=1).astype(np.float32)
+
     def update_terminated(self, state):
         """基类接触终止 + hfield 边界终止（防止走出地形导致 NaN）。"""
         state = super().update_terminated(state)
+        if self._model.num_hfields == 0:
+            return state  # flat 场景无 hfield
+        hf = self._model.get_hfield(0)
         pose = self._body.get_pose(state.data)
         base_xy = pose[:, :2]
-        hf = self._model.get_hfield(0)
         b = hf.bound
         out_of_bounds = (
             (base_xy[:, 0] < b[0]) | (base_xy[:, 0] > b[3]) |
@@ -577,7 +741,9 @@ class DreamWaQTask(Go1WalkTask):
             "joint_power": self._reward_joint_power(data),
             "smoothness": self._reward_smoothness(info),
             "power_distribution": self._reward_power_distribution(data),
-            # stand_still 在上游被注释掉
+            "stand_still": self._reward_stand_still(data, commands),
+            "dof_pos_limits": self._reward_dof_pos_limits(data),
+            "collision": self._reward_collision(data),
         }
 
     def update_feet_air_time(self, info: dict):
@@ -606,6 +772,13 @@ class DreamWaQTask(Go1WalkTask):
         rew *= np.linalg.norm(commands[:, :2], axis=1) > 0.1
         return rew
 
+    def _reward_stand_still(self, data, commands):
+        """惩罚有命令但速度接近零的'卡住'行为。"""
+        cmd_norm = np.linalg.norm(commands[:, :2], axis=1)
+        vel_norm = np.linalg.norm(self.get_local_linvel(data)[:, :2], axis=1)
+        stifled = (cmd_norm > 0.2) & (vel_norm < 0.1)  # 有命令但基本不动
+        return stifled.astype(np.float32)
+
     def update_reward(self, state):
         """存储各项奖励到 TensorBoard + 更新 state.reward。
 

motrix_envs/src/motrix_envs/locomotion/go1/xmls/go1_motor_actuator.xml

@@ -49,19 +49,10 @@
           <geom size="0.046 0.02" quat="1 1 0 0" type="cylinder"/>
         </default>
         <default class="thigh1">
-          <geom size="0.015" fromto="-0.02 0 0 -0.02 0 -0.16"/>
-        </default>
-        <default class="thigh2">
-          <geom size="0.015" fromto="0 0 0 -0.02 0 -0.1"/>
-        </default>
-        <default class="thigh3">
-          <geom size="0.015" fromto="-0.02 0 -0.16 0 0 -0.2"/>
+          <geom type="capsule" size="0.02" fromto="0 0 0 0 0 -0.213"/>
         </default>
         <default class="calf1">
-          <geom size="0.01" fromto="0 0 0 0.02 0 -0.13"/>
-        </default>
-        <default class="calf2">
-          <geom size="0.01" fromto="0.02 0 -0.13 0 0 -0.2"/>
+          <geom type="capsule" size="0.015" fromto="0 0 0 0 0 -0.213"/>
         </default>
         <default class="foot">
           <geom type="sphere" size="0.023" pos="0 0 -0.213" solimp="0.9 .99 0.001" priority="10" condim="3"/>
@@ -97,20 +88,20 @@
         <inertial pos="-0.0049166 0.00762615 -8.865e-05" quat="0.507341 0.514169 0.495027 0.482891" mass="0.68" diaginertia="0.000734064 0.000468438 0.000398719"/>
         <joint class="abduction" name="FR_hip_joint"/>
         <geom class="visual" mesh="hip" quat="1 0 0 0"/>
-        <!-- <geom name="fr_hip" class="hip_right1"/> -->
+        <geom name="fr_hip" class="hip_right1"/> 
         <body name="FR_thigh" pos="0 -0.08 0">
           <inertial pos="-0.00304722 0.019315 -0.0305004" quat="0.65243 -0.0272313 0.0775126 0.753383" mass="1.009" diaginertia="0.00478717 0.00460903 0.000709268"/>
           <joint class="hip" name="FR_thigh_joint"/>
           <geom class="visual" mesh="thigh_mirror"/>
-          <!-- <geom name="fr_thigh1" class="thigh1"/> -->
-          <!-- <geom name="fr_thigh2" class="thigh2"/> -->
-          <!-- <geom name="fr_thigh3" class="thigh3"/> -->
+          <geom name="fr_thigh1" class="thigh1"/> 
+          <!-- removed: <geom name="fr_thigh2" class="thigh2"/> (simplified to single capsule) --> 
+          <!-- removed: <geom name="fr_thigh3" class="thigh3"/> (simplified to single capsule) --> 
           <body name="FR_calf" pos="0 0 -0.213">
             <inertial pos="0.00429862 0.000976676 -0.146197" quat="0.691246 0.00357467 0.00511118 0.722592" mass="0.195862" diaginertia="0.00149767 0.00148468 3.58427e-05"/>
             <joint class="knee" name="FR_calf_joint"/>
             <geom class="visual" mesh="calf"/>
-            <!-- <geom name="fr_calf1" class="calf1"/> -->
-            <!-- <geom name="fr_calf2" class="calf2"/> -->
+            <geom name="fr_calf1" class="calf1"/> 
+            <!-- removed: <geom name="fr_calf2" class="calf2"/> (simplified to single capsule) --> 
             <geom name="FR_foot" class="foot"/>
             <site name="FR" pos="0 0 -0.213" type="sphere" size="0.023" group="5"/>
           </body>
@@ -120,20 +111,20 @@
         <inertial pos="-0.0049166 -0.00762615 -8.865e-05" quat="0.482891 0.495027 0.514169 0.507341" mass="0.68" diaginertia="0.000734064 0.000468438 0.000398719"/>
         <joint class="abduction" name="FL_hip_joint"/>
         <geom class="visual" mesh="hip"/>
-        <!-- <geom name="fl_hip" class="hip_left1"/> -->
+        <geom name="fl_hip" class="hip_left1"/> 
         <body name="FL_thigh" pos="0 0.08 0">
           <inertial pos="-0.00304722 -0.019315 -0.0305004" quat="0.753383 0.0775126 -0.0272313 0.65243" mass="1.009" diaginertia="0.00478717 0.00460903 0.000709268"/>
           <joint class="hip" name="FL_thigh_joint"/>
           <geom class="visual" mesh="thigh"/>
-          <!-- <geom name="fl_thigh1" class="thigh1"/> -->
-          <!-- <geom name="fl_thigh2" class="thigh2"/> -->
-          <!-- <geom name="fl_thigh3" class="thigh3"/> -->
+          <geom name="fl_thigh1" class="thigh1"/> 
+          <!-- removed: <geom name="fl_thigh2" class="thigh2"/> (simplified to single capsule) --> 
+          <!-- removed: <geom name="fl_thigh3" class="thigh3"/> (simplified to single capsule) --> 
           <body name="FL_calf" pos="0 0 -0.213">
             <inertial pos="0.00429862 0.000976676 -0.146197" quat="0.691246 0.00357467 0.00511118 0.722592" mass="0.195862" diaginertia="0.00149767 0.00148468 3.58427e-05"/>
             <joint class="knee" name="FL_calf_joint"/>
             <geom class="visual" mesh="calf"/>
-            <!-- <geom name="fl_calf1" class="calf1"/> -->
-            <!-- <geom name="fl_calf2" class="calf2"/> -->
+            <geom name="fl_calf1" class="calf1"/> 
+            <!-- removed: <geom name="fl_calf2" class="calf2"/> (simplified to single capsule) --> 
             <geom name="FL_foot" class="foot"/>
             <site name="FL" pos="0 0 -0.213" type="sphere" size="0.023" group="5"/>
           </body>
@@ -143,20 +134,20 @@
         <inertial pos="0.0049166 0.00762615 -8.865e-05" quat="0.495027 0.482891 0.507341 0.514169" mass="0.68" diaginertia="0.000734064 0.000468438 0.000398719"/>
         <joint class="abduction" name="RR_hip_joint"/>
         <geom class="visual" quat="0 0 0 -1" mesh="hip"/>
-        <!-- <geom name="rr_hip" class="hip_right1"/> -->
+        <geom name="rr_hip" class="hip_right1"/> 
         <body name="RR_thigh" pos="0 -0.08 0">
           <inertial pos="-0.00304722 0.019315 -0.0305004" quat="0.65243 -0.0272313 0.0775126 0.753383" mass="1.009" diaginertia="0.00478717 0.00460903 0.000709268"/>
           <joint class="hip" name="RR_thigh_joint"/>
           <geom class="visual" mesh="thigh_mirror"/>
-          <!-- <geom name="rr_thigh1" class="thigh1"/>
-          <geom name="rr_thigh2" class="thigh2"/>
-          <geom name="rr_thigh3" class="thigh3"/> -->
+          <geom name="rr_thigh1" class="thigh1"/>
+          <!-- removed: <geom name="rr_thigh2" class="thigh2"/> (simplified to single capsule) -->
+          <!-- removed: <geom name="rr_thigh3" class="thigh3"/> (simplified to single capsule) --> 
           <body name="RR_calf" pos="0 0 -0.213">
             <inertial pos="0.00429862 0.000976676 -0.146197" quat="0.691246 0.00357467 0.00511118 0.722592" mass="0.195862" diaginertia="0.00149767 0.00148468 3.58427e-05"/>
             <joint class="knee" name="RR_calf_joint"/>
             <geom class="visual" mesh="calf"/>
-            <!-- <geom name="rr_calf1" class="calf1"/>
-            <geom name="rr_calf2" class="calf2"/> -->
+            <geom name="rr_calf1" class="calf1"/>
+            <!-- removed: <geom name="rr_calf2" class="calf2"/> (simplified to single capsule) --> 
             <geom name="RR_foot" class="foot"/>
             <site name="RR" pos="0 0 -0.213" type="sphere" size="0.023" group="5"/>
           </body>
@@ -166,20 +157,20 @@
         <inertial pos="0.0049166 -0.00762615 -8.865e-05" quat="0.514169 0.507341 0.482891 0.495027" mass="0.68" diaginertia="0.000734064 0.000468438 0.000398719"/>
         <joint class="abduction" name="RL_hip_joint"/>
         <geom class="visual" quat="0 0 1 0" mesh="hip"/>
-        <!-- <geom name="rl_hip" class="hip_left1"/> -->
+        <geom name="rl_hip" class="hip_left1"/> 
         <body name="RL_thigh" pos="0 0.08 0">
           <inertial pos="-0.00304722 -0.019315 -0.0305004" quat="0.753383 0.0775126 -0.0272313 0.65243" mass="1.009" diaginertia="0.00478717 0.00460903 0.000709268"/>
           <joint class="hip" name="RL_thigh_joint"/>
           <geom class="visual" mesh="thigh"/>
-          <!-- <geom name="rl_thigh1" class="thigh1"/>
-          <geom name="rl_thigh2" class="thigh2"/>
-          <geom name="rl_thigh3" class="thigh3"/> -->
+          <geom name="rl_thigh1" class="thigh1"/>
+          <!-- removed: <geom name="rl_thigh2" class="thigh2"/> (simplified to single capsule) -->
+          <!-- removed: <geom name="rl_thigh3" class="thigh3"/> (simplified to single capsule) --> 
           <body name="RL_calf" pos="0 0 -0.213">
             <inertial pos="0.00429862 0.000976676 -0.146197" quat="0.691246 0.00357467 0.00511118 0.722592" mass="0.195862" diaginertia="0.00149767 0.00148468 3.58427e-05"/>
             <joint class="knee" name="RL_calf_joint"/>
             <geom class="visual" mesh="calf"/>
-            <!-- <geom name="rl_calf1" class="calf1"/>
-            <geom name="rl_calf2" class="calf2"/> -->
+            <geom name="rl_calf1" class="calf1"/>
+            <!-- removed: <geom name="rl_calf2" class="calf2"/> (simplified to single capsule) --> 
             <geom name="RL_foot" class="foot"/>
             <site name="RL" pos="0 0 -0.213" type="sphere" size="0.023" group="5"/>
           </body>

motrix_envs/src/motrix_envs/np/env.py

@@ -182,17 +182,19 @@ class NpEnv(ABEnv):
         pass
 
     def physics_step(self):
+        self._physics_crashed_this_step = False
         # motrixsim.SceneModel.step only supports single step, so we loop
         try:
             for _ in range(self._cfg.sim_substeps):
                 self._model.step(self._state.data)
-        except Exception as e:
+        except BaseException as e:
             # Rust panic / 物理崩溃 → 标记终止 + 清除腐蚀数据
             n = self._state.data.shape[0]
             self._state.terminated[:] = True
             self._state.reward[:] = 0.0
             # 强制重置物理状态，防止 NaN 传播到 observations/rewards
             self._state.data.reset(self._model)
+            self._physics_crashed_this_step = True
             if not hasattr(self, '_physics_crash_count'):
                 self._physics_crash_count = 0
             self._physics_crash_count += 1
@@ -213,6 +215,9 @@ class NpEnv(ABEnv):
         self._state = self.apply_action(actions, self._state)
         assert self._state is not None, "apply_action must return a valid NpEnvState"
         self.physics_step()
+        if getattr(self, "_physics_crashed_this_step", False):
+            self._reset_done_envs()
+            return self._state
         self._state = self.update_state(self._state)
         self._state.info["steps"] += 1
         self._update_truncate()