[software] 添加16DOF早期训练仿真与Sim2Real闭环

2026-07-21 16:15:14 +08:00
parent 9bd22225f9
commit e9e2c946b3
681 changed files with 137221 additions and 8 deletions
@@ -0,0 +1,78 @@
+"""通用运行期守护：每个控制周期调用一次，无副作用，只做检查。
+
+设计原则：
+- 守护函数本身不下发动作、不打印（除非 verbose），只返回判定
+- 调用方决定收到 GuardStop 时怎么办（damping_brake 或 raise）
+- 起立期 / 等待期 / 主循环都共用同一组检查
+"""
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Optional
+
+import numpy as np
+
+
+class GuardLevel(IntEnum):
+    OK = 0
+    WARN = 1       # 仅记录，不停
+    STOP = 2       # 主调方应立刻 damping_brake + 退出当前阶段
+
+
+@dataclass
+class GuardDecision:
+    level: GuardLevel
+    reason: str  # 触发时人类可读说明，OK 时为空
+
+
+class RuntimeGuard:
+    """启动/起立/主循环共用的安全守护。
+
+    不监控目标位置范围（那是 SafetyMonitor 的职责）。这里只关心
+    机身整体状态：是否倾倒、是否翻滚、是否检测到 NaN、用户是否按急停。
+    """
+
+    def __init__(self,
+                 max_ang_vel: float = 12.0,
+                 max_tilt_z: float = -0.30,
+                 imu_age_warn_ms: float = 60.0,
+                 imu_age_stop_ms: float = 200.0):
+        self.max_ang_vel = max_ang_vel
+        self.max_tilt_z = max_tilt_z
+        self.imu_age_warn_ms = imu_age_warn_ms
+        self.imu_age_stop_ms = imu_age_stop_ms
+
+    def check(self,
+              imu_gyro: np.ndarray,
+              projected_gravity: np.ndarray,
+              imu_age_ms: float,
+              estop_triggered: bool,
+              extra_nan_arrays: tuple = ()) -> GuardDecision:
+        # 1) 用户急停
+        if estop_triggered:
+            return GuardDecision(GuardLevel.STOP, "user E-stop")
+
+        # 2) NaN 检查（任意输入数组中出现 NaN）
+        for arr in (imu_gyro, projected_gravity, *extra_nan_arrays):
+            if arr is None:
+                continue
+            if np.any(np.isnan(arr)) or np.any(np.isinf(arr)):
+                return GuardDecision(GuardLevel.STOP, "NaN/Inf detected in observation/action")
+
+        # 3) IMU 数据陈旧
+        if imu_age_ms > self.imu_age_stop_ms:
+            return GuardDecision(GuardLevel.STOP, f"IMU stale {imu_age_ms:.0f}ms")
+        warned_imu = imu_age_ms > self.imu_age_warn_ms
+
+        # 4) 倾倒
+        if projected_gravity[2] > self.max_tilt_z:
+            return GuardDecision(GuardLevel.STOP,
+                                 f"tilt: g_z={projected_gravity[2]:.3f}")
+
+        # 5) 角速度爆表
+        ang_norm = float(np.linalg.norm(imu_gyro))
+        if ang_norm > self.max_ang_vel:
+            return GuardDecision(GuardLevel.STOP, f"ang_vel overflow: |w|={ang_norm:.2f}")
+
+        if warned_imu:
+            return GuardDecision(GuardLevel.WARN, f"IMU age {imu_age_ms:.0f}ms")
+        return GuardDecision(GuardLevel.OK, "")
@@ -0,0 +1,107 @@
+"""三级安全监控（对应方法论 97.11）。
+
+Level 0: 正常
+Level 1: 限幅（位置/速度异常）— 截断目标位置幅值，记录连续触发次数
+Level 2: 刹车（连续限幅 N 次 / IMU 角速度过大 / 倾倒）— 卸载刚度只留阻尼
+Level 3: 急停（用户触发）— 让上层断电
+
+设计原则：监控只判定，不直接关电机；返回 SafetyDecision 由上层决策。
+"""
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Any, Optional
+
+import numpy as np
+
+
+class SafetyLevel(IntEnum):
+    NORMAL = 0
+    CLIP = 1
+    BRAKE = 2
+    ESTOP = 3
+
+
+@dataclass
+class SafetyDecision:
+    level: SafetyLevel
+    message: str
+    clipped_target: Optional[np.ndarray]
+    details: Optional[dict[str, Any]] = None
+
+
+class SafetyMonitor:
+    """安全监控（按 50Hz 控制频率调用）。
+
+    Args:
+        max_target_offset: 单关节相对默认位姿的最大偏离 (rad)
+        max_ang_vel: IMU 角速度模 (rad/s)
+        max_tilt_rad: 机身重力 z 轴投影低于该值认为已严重倾倒
+        clip_to_brake: 连续 clip 多少帧升级为刹车
+    """
+
+    def __init__(self,
+                 max_target_offset: float = 0.6,
+                 max_ang_vel: float = 10.0,
+                 max_tilt_z: float = -0.3,
+                 clip_to_brake: int = 3):
+        self.max_target_offset = max_target_offset
+        self.max_ang_vel = max_ang_vel
+        self.max_tilt_z = max_tilt_z  # projected_gravity z 应当 ~ -1，明显小于 -0.3 视作倾倒
+        self.clip_to_brake = clip_to_brake
+        self.consecutive_clips = 0
+
+    def check(self,
+              target_pose: np.ndarray,
+              default_pose: np.ndarray,
+              imu_gyro: np.ndarray,
+              projected_gravity: np.ndarray,
+              estop_triggered: bool) -> SafetyDecision:
+        if estop_triggered:
+            return SafetyDecision(SafetyLevel.ESTOP, "user E-stop", None, None)
+
+        # 倾倒（projected_gravity[2] 应在 -1 附近，越接近 0 越倾斜）
+        if projected_gravity[2] > self.max_tilt_z:
+            return SafetyDecision(
+                SafetyLevel.BRAKE,
+                f"tilt detected: g_z={projected_gravity[2]:.3f}",
+                None,
+                {"g_z": float(projected_gravity[2])},
+            )
+
+        # 角速度爆表（猛烈翻滚）
+        if np.linalg.norm(imu_gyro) > self.max_ang_vel:
+            return SafetyDecision(
+                SafetyLevel.BRAKE,
+                f"angular velocity overflow: |w|={np.linalg.norm(imu_gyro):.2f}",
+                None,
+                {"ang_vel_norm": float(np.linalg.norm(imu_gyro))},
+            )
+
+        # 目标位置偏离过大 → 截断到允许范围
+        offset_leg = target_pose[:12] - default_pose[:12]
+        clipped_offset = np.clip(offset_leg, -self.max_target_offset, self.max_target_offset)
+        if not np.allclose(offset_leg, clipped_offset):
+            self.consecutive_clips += 1
+            clipped = target_pose.copy()
+            clipped[:12] = default_pose[:12] + clipped_offset
+            exceeded = np.where(np.abs(offset_leg) > self.max_target_offset)[0].tolist()
+            max_offset = float(np.max(np.abs(offset_leg)))
+            details = {
+                "joint_indices": exceeded,
+                "max_leg_offset": max_offset,
+                "consecutive_clips": int(self.consecutive_clips),
+            }
+            if self.consecutive_clips >= self.clip_to_brake:
+                return SafetyDecision(
+                    SafetyLevel.BRAKE,
+                    f"clipped {self.consecutive_clips} frames in a row",
+                    clipped,
+                    details,
+                )
+            return SafetyDecision(SafetyLevel.CLIP, "target leg offset out of range", clipped, details)
+
+        self.consecutive_clips = 0
+        return SafetyDecision(SafetyLevel.NORMAL, "", None, None)
+
+    def reset(self):
+        self.consecutive_clips = 0