108 lines
3.9 KiB
Python
108 lines
3.9 KiB
Python
"""三级安全监控（对应方法论 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
|