[real] 整理 Python Sim2Real v2

2026-07-27 16:45:09 +08:00
parent 55094bf682
commit 3bf6eb0c4c
84 changed files with 13361 additions and 5 deletions
@@ -0,0 +1,351 @@
+"""起立姿态初始化器（实测起点版本）。
+
+设计：
+  - 不再假设机器人的物理起始姿态（不再有 CRAWL_POSE / GROUND_POSE 起点）
+  - enable 后从 io.read_measured_pose() 读 16 关节实测，直接作为插值起点
+  - 余弦插值到 STAND_POSE，transition_time 根据最大偏差自适应
+  - 全程 RuntimeGuard 守护（空格急停/倾倒/翻滚/NaN/IMU 陈旧）
+  - 50Hz 写 LogBundle CSV（phase 字段标识阶段）
+
+Phase 流程：
+  STARTUP_SOFT_HOLD  — 软起步保持实测姿态，kp 从 0.125 渐升到 1.0
+  STARTUP_TRANSITION — 实测起点 → STAND 余弦插值
+  STARTUP_HOLD_AFTER — 站稳后保持 1 秒
+"""
+import time
+from typing import Optional
+
+import numpy as np
+
+from safety.runtime_guard import GuardLevel, RuntimeGuard
+from tools.logger import LogBundle
+from tools.math_utils import get_gravity_orientation
+
+
+# 仅作为目标姿态使用（训练侧 default_dof_pos）
+STAND_POSE = np.array([
+    0.0, 0.9, -1.8,
+    0.0, 0.9, -1.8,
+    0.0, 0.9, -1.8,
+    0.0, 0.9, -1.8,
+    0.0, 0.0, 0.0, 0.0,
+], dtype=np.float32)
+
+
+def _periodic_leg_delta(src: np.ndarray, dst: np.ndarray) -> np.ndarray:
+    """Return shortest signed leg-joint delta from src to dst."""
+    delta = np.asarray(dst[:12], dtype=np.float32) - np.asarray(src[:12], dtype=np.float32)
+    return ((delta + np.pi) % (2.0 * np.pi) - np.pi).astype(np.float32)
+
+
+class PoseInitFailed(RuntimeError):
+    """起立流程触发安全停止。main.py 捕获后立即 damping_brake。"""
+
+
+class PoseInitializer:
+    def __init__(self, real_io, control_dt: float = 0.02,
+                 transition_time_min: float = 2.0,
+                 transition_time_max: float = 6.0,
+                 transition_seconds_per_rad: float = 1.5,
+                 hold_time: float = 1.0,
+                 settle_pos_threshold: float = 0.12,
+                 settle_vel_threshold: float = 0.6,
+                 timeout_extra: float = 3.0,
+                 imu_fresh_wait_s: float = 1.0,
+                 progress_log_interval: float = 0.5,
+                 ramp_kp_time: float = 1.0,
+                 soft_hold_duration: float = 1.0,
+                 max_dev_warn: float = 1.5,
+                 max_dev_abort: float = 3.0):
+        """
+        Args:
+            transition_time_min/max/_per_rad: 自适应公式
+                t = clip(min, max, max_dev * seconds_per_rad)
+            timeout_extra: 起立超时 = transition_time + timeout_extra
+            soft_hold_duration: 起立前先在实测姿态保持几秒，期间 kp ramp-up
+            max_dev_warn: 最大偏差超过此值打警告（仅日志）
+            max_dev_abort: 最大偏差超过此值直接 PoseInitFailed（拒绝起立）
+        """
+        self.io = real_io
+        self.control_dt = control_dt
+        self.transition_time_min = transition_time_min
+        self.transition_time_max = transition_time_max
+        self.transition_seconds_per_rad = transition_seconds_per_rad
+        self.hold_time = hold_time
+        self.settle_pos_threshold = settle_pos_threshold
+        self.settle_vel_threshold = settle_vel_threshold
+        self.timeout_extra = timeout_extra
+        self.imu_fresh_wait_s = max(float(imu_fresh_wait_s), 0.0)
+        self.progress_log_interval = progress_log_interval
+        self.ramp_kp_time = ramp_kp_time
+        self.soft_hold_duration = soft_hold_duration
+        self.max_dev_warn = max_dev_warn
+        self.max_dev_abort = max_dev_abort
+
+        self.logger: Optional[LogBundle] = None
+        self.guard: Optional[RuntimeGuard] = None
+        self.keyboard = None
+
+    def attach(self, logger: LogBundle, guard: RuntimeGuard, keyboard):
+        self.logger = logger
+        self.guard = guard
+        self.keyboard = keyboard
+
+    # ---- 通用每周期工作 ----
+    def _tick(self, phase: str, sim_target: np.ndarray, kp_scale: float, next_exec: float):
+        """读状态 → guard 检查 → 写日志 → 锁帧。返回 (state_dict, next_exec)。
+        若 guard.STOP，立即抛 PoseInitFailed。"""
+        loop_t0 = time.perf_counter()
+
+        state = self.io.read_state()
+        proj_g = get_gravity_orientation(state["quat_wxyz"])
+
+        guard_dec = None
+        if self.guard is not None:
+            estop = bool(self.keyboard and self.keyboard.is_estop_triggered())
+            guard_dec = self.guard.check(
+                imu_gyro=state["imu_gyro"],
+                projected_gravity=proj_g,
+                imu_age_ms=float(state["imu_age_ms"]),
+                estop_triggered=estop,
+                extra_nan_arrays=(sim_target, state["joint_pos"], state["joint_vel"]),
+            )
+
+        if self.logger is not None:
+            motor_diag = state.get("motor_stale", {})
+            self.logger.state(
+                phase=phase,
+                joint_pos=state["joint_pos"],
+                joint_vel=state["joint_vel"],
+                joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
+                target_pose=sim_target,
+                raw_action=None,
+                gyro=state["imu_gyro"],
+                accel=state["imu_accel"],
+                quat=state["quat_wxyz"],
+                proj_gravity=proj_g,
+                command=np.zeros(3, dtype=np.float32),
+                imu_age_ms=float(state["imu_age_ms"]),
+                loop_dt_ms=(time.perf_counter() - loop_t0) * 1000.0,
+                safety_level=0,
+                guard_level=int(guard_dec.level) if guard_dec else 0,
+                holdover=int(motor_diag.get("holdover_this_frame", 0)),
+                stale_max=int(motor_diag.get("stale_max", 0)),
+                fresh_count=int(motor_diag.get("fresh_count", 16)),
+                kp_scale=kp_scale,
+                nan_flag=int(np.any(np.isnan(state["joint_pos"]))),
+                kp_leg_cmd=float(getattr(self.io, "hold_kp_leg", self.io.kp_leg) * kp_scale),
+                kd_leg_cmd=float(getattr(self.io, "hold_kd_leg", self.io.kd_leg)),
+                kd_wheel_cmd=float(self.io.kd_wheel),
+                target_source="startup_hold",
+                guard_reason=guard_dec.reason if guard_dec else "",
+            )
+
+        if guard_dec is not None and guard_dec.level == GuardLevel.STOP:
+            if self.logger:
+                self.logger.event("GUARD_STOP", phase=phase, reason=guard_dec.reason)
+            raise PoseInitFailed(f"[{phase}] {guard_dec.reason}")
+
+        next_exec += self.control_dt
+        slack = next_exec - time.perf_counter()
+        if slack > 0:
+            coarse = slack - 0.002
+            if coarse > 0:
+                time.sleep(coarse)
+            micro_slack = next_exec - time.perf_counter()
+            if micro_slack > 0:
+                time.sleep(min(micro_slack, 0.001))
+        else:
+            next_exec = time.perf_counter()
+        return state, next_exec
+
+    # ---- 主入口：从实测姿态起立到 STAND ----
+    def transition_to_stand_from_current(self,
+                                          target_pose: Optional[np.ndarray] = None
+                                          ) -> np.ndarray:
+        """完整起立流程：
+          1. 读实测起点
+          2. 偏差检查（warn / abort）
+          3. SOFT_HOLD：保持实测姿态 + kp ramp-up
+          4. TRANSITION：余弦插值到 target，transition_time 自适应
+          5. HOLD_AFTER：保持 1 秒
+        返回最终 target_pose（供主循环使用）。
+        """
+        if target_pose is None:
+            target_pose = STAND_POSE.copy()
+        target_pose = target_pose.astype(np.float32).copy()
+        target_pose[12:] = 0.0
+
+        # === 1. 读实测起点（要求电机反馈完整）===
+        ok, missing = self.io.wait_feedback_ready(max_attempts=20, poll_interval=0.05)
+        if not ok:
+            msg = f"feedback incomplete: {len(missing)} motors no response: {missing[:4]}"
+            if self.logger:
+                self.logger.event("STARTUP_NO_FEEDBACK",
+                                  missing=[m[2] for m in missing])
+            raise PoseInitFailed(msg)
+
+        last_imu_age = -1.0
+        imu_deadline = time.perf_counter() + self.imu_fresh_wait_s
+        while self.imu_fresh_wait_s > 0.0 and time.perf_counter() < imu_deadline:
+            state = self.io.read_state()
+            last_imu_age = float(state.get("imu_age_ms", 1e9))
+            if last_imu_age <= 60.0:
+                break
+            time.sleep(self.control_dt)
+        else:
+            if self.imu_fresh_wait_s > 0.0 and self.logger:
+                self.logger.event("STARTUP_IMU_STALE_WARN", imu_age_ms=last_imu_age)
+
+        start_pose = self.io.read_measured_pose().astype(np.float32).copy()
+        start_pose[12:] = 0.0   # 轮子起点固定为 0 速度
+
+        # === 2. 偏差检查 ===
+        startup_delta = _periodic_leg_delta(start_pose, target_pose)
+        diff = np.abs(startup_delta)
+        max_dev = float(np.max(diff))
+        max_dev_joint = int(np.argmax(diff))
+        transition_time = float(np.clip(
+            max_dev * self.transition_seconds_per_rad,
+            self.transition_time_min, self.transition_time_max
+        ))
+        timeout = transition_time + self.timeout_extra
+
+        if self.logger:
+            self.logger.event(
+                "STARTUP_PLAN",
+                start_pose_leg=start_pose[:12].tolist(),
+                target_pose_leg=target_pose[:12].tolist(),
+                max_dev=max_dev,
+                max_dev_joint_idx=max_dev_joint,
+                transition_time=transition_time,
+                timeout=timeout,
+            )
+        print(f"[PoseInit] 实测起点最大偏差 {max_dev:.3f} rad (关节 idx={max_dev_joint}); "
+              f"transition_time={transition_time:.2f}s")
+
+        if max_dev > self.max_dev_abort:
+            raise PoseInitFailed(
+                f"实测起点偏差过大 ({max_dev:.2f} rad > abort 阈值 "
+                f"{self.max_dev_abort})；请检查电机是否在合理姿势"
+            )
+        if max_dev > self.max_dev_warn:
+            print(f"[PoseInit] WARNING 偏差 {max_dev:.2f} rad > {self.max_dev_warn}; "
+                  f"起立可能比较剧烈")
+            if self.logger:
+                self.logger.event("STARTUP_LARGE_DEV", max_dev=max_dev)
+
+        # === 3. SOFT_HOLD：实测姿态 + kp ramp-up ===
+        if self.logger:
+            self.logger.event("STARTUP_SOFT_HOLD_BEGIN",
+                              duration=self.soft_hold_duration,
+                              ramp_kp_time=self.ramp_kp_time,
+                              ramp_kp_min=0.125)
+        n = max(1, int(self.soft_hold_duration / max(self.control_dt, 1e-3)))
+        next_exec = time.perf_counter()
+        t0 = next_exec
+        ramp_min = 0.125
+        for i in range(n):
+            elapsed = time.perf_counter() - t0
+            if elapsed < self.ramp_kp_time:
+                kp_scale = ramp_min + (1.0 - ramp_min) * (elapsed / self.ramp_kp_time)
+            else:
+                kp_scale = 1.0
+            self.io.hold_pose(start_pose, kp_scale=kp_scale)
+            _s, next_exec = self._tick("STARTUP_SOFT_HOLD", start_pose, kp_scale, next_exec)
+        if self.logger:
+            self.logger.event("STARTUP_SOFT_HOLD_END")
+
+        # === 4. TRANSITION：余弦插值 ===
+        if self.logger:
+            self.logger.event("STARTUP_TRANSITION_BEGIN",
+                              transition_time=transition_time, timeout=timeout)
+        print(f"[PoseInit] 起立: transition={transition_time:.2f}s, "
+              f"hold={self.hold_time}s, timeout={timeout:.2f}s")
+
+        t0 = time.perf_counter()
+        last_log = t0
+        reached = False
+        hold_start: Optional[float] = None
+        next_exec = t0
+
+        while True:
+            now = time.perf_counter()
+            elapsed = now - t0
+            phase = min(1.0, elapsed / max(transition_time, 1e-3))
+
+            if elapsed > timeout:
+                if self.logger:
+                    self.logger.event("STARTUP_TIMEOUT", elapsed=elapsed)
+                raise PoseInitFailed(
+                    f"transition timeout after {elapsed:.2f}s, target not reached"
+                )
+
+            blend = 0.5 - 0.5 * np.cos(np.pi * phase)
+            blended = start_pose.astype(np.float32).copy()
+            blended[:12] = start_pose[:12] + blend * startup_delta
+            blended[12:] = 0.0
+            self.io.hold_pose(blended, kp_scale=1.0)
+            state, next_exec = self._tick("STARTUP_TRANSITION", blended, 1.0, next_exec)
+
+            joint_pos = state["joint_pos"]
+            joint_vel = state["joint_vel"]
+            pos_err = float(np.max(np.abs(_periodic_leg_delta(joint_pos, target_pose))))
+            vel_err = float(np.max(np.abs(joint_vel[:12])))
+
+            if now - last_log >= self.progress_log_interval:
+                msg = (f"[PoseInit] phase={phase*100:5.1f}% | "
+                       f"max_pos_err={pos_err:.3f} | max_vel={vel_err:.3f}")
+                print(msg)
+                if self.logger:
+                    self.logger.event("STARTUP_PROGRESS",
+                                      phase=phase, pos_err=pos_err, vel_err=vel_err)
+                last_log = now
+
+            if (phase >= 1.0
+                    and pos_err <= self.settle_pos_threshold
+                    and vel_err <= self.settle_vel_threshold):
+                if not reached:
+                    reached = True
+                    hold_start = now
+                    if self.logger:
+                        self.logger.event("STARTUP_REACHED",
+                                          pos_err=pos_err, vel_err=vel_err)
+                    print(f"[PoseInit] 已到位，保持 {self.hold_time:.2f}s")
+                elif hold_start is not None and now - hold_start >= self.hold_time:
+                    break
+            elif phase >= 1.0:
+                reached = False
+                hold_start = None
+
+        # === 5. HOLD_AFTER ===
+        if self.logger:
+            self.logger.event("STARTUP_HOLD_AFTER_BEGIN", duration=self.hold_time)
+        n_hold = max(1, int(self.hold_time / max(self.control_dt, 1e-3)))
+        next_exec = time.perf_counter()
+        for _ in range(n_hold):
+            self.io.hold_pose(target_pose, kp_scale=1.0)
+            _s, next_exec = self._tick("STARTUP_HOLD_AFTER", target_pose, 1.0, next_exec)
+
+        if self.logger:
+            self.logger.event("STARTUP_TRANSITION_END")
+        print("[PoseInit] 默认站姿初始化完成")
+        return target_pose
+
+    # ---- 等用户回车（外部调用，期间持续保持） ----
+    def hold_until_user_confirm(self, target_pose: np.ndarray, evt) -> bool:
+        """阻塞循环到 evt.is_set()，期间持续 PD 保持站姿、跑 guard、写日志。
+        返回 True 正常确认，False 因 guard.STOP 中止。"""
+        if self.logger:
+            self.logger.event("WAIT_USER_BEGIN")
+        next_exec = time.perf_counter()
+        while not evt.is_set():
+            self.io.hold_pose(target_pose, kp_scale=1.0)
+            try:
+                _s, next_exec = self._tick("WAIT_USER", target_pose, 1.0, next_exec)
+            except PoseInitFailed as e:
+                print(f"[PoseInit] WAIT_USER 期间触发停止: {e}")
+                return False
+        if self.logger:
+            self.logger.event("WAIT_USER_END")
+        return True
@@ -0,0 +1,141 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any, Dict
+
+import numpy as np
+
+
+@dataclass
+class StandBalanceDebug:
+    roll: float
+    pitch: float
+    roll_rate: float
+    pitch_rate: float
+    hip_base: float
+    knee_base: float
+    roll_corr: float
+    pitch_corr: float
+    pitch_compensation_enabled: bool
+    target: list[float]
+    stable: bool
+
+
+class StandBalanceController:
+    def __init__(self, cfg: Dict[str, Any], control_dt: float):
+        self.enabled = bool(cfg.get("enabled", True))
+        self.control_dt = float(control_dt)
+        self.height = float(cfg.get("height", 0.33))
+        self.kp_roll = float(cfg.get("kp_roll", 0.85))
+        self.pitch_compensation_enabled = bool(cfg.get("pitch_compensation_enabled", True))
+        self.kp_pitch = float(cfg.get("kp_pitch", 0.70))
+        self.kd_roll_rate = float(cfg.get("kd_roll_rate", 0.03))
+        self.kd_pitch_rate = float(cfg.get("kd_pitch_rate", 0.025))
+        self.pitch_deadband = float(np.radians(cfg.get("pitch_deadband_deg", 0.0)))
+        self.pitch_corr_clip = float(cfg.get("pitch_corr_clip", 0.12))
+        self.pitch_corr_filter_alpha = float(np.clip(cfg.get("pitch_corr_filter_alpha", 1.0), 0.0, 1.0))
+        self.pitch_front_sign = float(cfg.get("pitch_front_sign", -1.0))
+        self.lateral_lean_gain = float(cfg.get("lateral_lean_gain", 0.0))
+        self.hip_abduction_clip = float(cfg.get("hip_abduction_clip", 0.45))
+        self.hip_pitch_clip = tuple(cfg.get("hip_pitch_clip", [-1.0, 2.5]))
+        self.knee_clip = tuple(cfg.get("knee_clip", [-2.6, -0.3]))
+        self.stable_roll_deg = float(cfg.get("stable_roll_deg", 6.0))
+        self.stable_pitch_deg = float(cfg.get("stable_pitch_deg", 8.0))
+        self.stable_gyro_deg_s = float(cfg.get("stable_gyro_deg_s", 45.0))
+        self.enter_hold_s = float(cfg.get("enter_hold_s", 1.0))
+
+        self.profile_h = np.asarray(
+            cfg.get("profile_h", [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]),
+            dtype=np.float32,
+        )
+        self.profile_hip = np.asarray(
+            cfg.get("profile_hip", [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]),
+            dtype=np.float32,
+        )
+        self.profile_knee = np.asarray(
+            cfg.get("profile_knee", [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]),
+            dtype=np.float32,
+        )
+        self._stable_time = 0.0
+        self._pitch_corr_filtered = 0.0
+        self._last_debug = StandBalanceDebug(0.0, 0.0, 0.0, 0.0, 0.9, -1.8, 0.0, 0.0, False, [], False)
+
+    @property
+    def last_debug(self) -> StandBalanceDebug:
+        return self._last_debug
+
+    def reset(self) -> None:
+        self._stable_time = 0.0
+        self._pitch_corr_filtered = 0.0
+
+    def _estimate_roll_pitch(self, projected_gravity: np.ndarray) -> tuple[float, float]:
+        gx, gy, gz = [float(v) for v in projected_gravity]
+        roll = float(np.arctan2(-gy, max(1e-6, -gz)))
+        pitch = float(np.arctan2(gx, np.sqrt(max(1e-6, gy * gy + gz * gz))))
+        return roll, pitch
+
+    def _base_leg_pose(self) -> tuple[float, float]:
+        h_clamp = float(np.clip(self.height, float(self.profile_h[0]), float(self.profile_h[-1])))
+        hip = float(np.interp(h_clamp, self.profile_h, self.profile_hip))
+        knee = float(np.interp(h_clamp, self.profile_h, self.profile_knee))
+        return hip, knee
+
+    def compute_target(self, state: Dict[str, Any], command: np.ndarray | None = None) -> np.ndarray:
+        projected_gravity = np.asarray(state["projected_gravity"], dtype=np.float32)
+        imu_gyro = np.asarray(state["imu_gyro"], dtype=np.float32)
+        cmd = np.zeros(3, dtype=np.float32) if command is None else np.asarray(command, dtype=np.float32)
+
+        hip_base, knee_base = self._base_leg_pose()
+        roll, pitch = self._estimate_roll_pitch(projected_gravity)
+        roll_rate = float(imu_gyro[0])
+        pitch_rate = float(imu_gyro[1])
+
+        roll_corr = -self.kp_roll * roll - self.kd_roll_rate * roll_rate
+        if self.pitch_compensation_enabled:
+            pitch_for_ctrl = 0.0 if abs(pitch) < self.pitch_deadband else pitch
+            pitch_corr_raw = -self.kp_pitch * pitch_for_ctrl - self.kd_pitch_rate * pitch_rate
+            pitch_corr_raw = float(np.clip(pitch_corr_raw, -self.pitch_corr_clip, self.pitch_corr_clip))
+            alpha = self.pitch_corr_filter_alpha
+            pitch_corr = (1.0 - alpha) * self._pitch_corr_filtered + alpha * pitch_corr_raw
+            self._pitch_corr_filtered = pitch_corr
+        else:
+            pitch_corr = 0.0
+            self._pitch_corr_filtered = 0.0
+        lateral_lean = self.lateral_lean_gain * float(cmd[1])
+
+        target = np.zeros(16, dtype=np.float32)
+        for leg_idx in range(4):
+            side = 1.0 if leg_idx in (0, 2) else -1.0
+            fore_aft = self.pitch_front_sign if leg_idx in (0, 1) else -self.pitch_front_sign
+            target[leg_idx * 3 + 0] = float(
+                np.clip(side * roll_corr + lateral_lean, -self.hip_abduction_clip, self.hip_abduction_clip)
+            )
+            target[leg_idx * 3 + 1] = float(
+                np.clip(hip_base + fore_aft * pitch_corr, self.hip_pitch_clip[0], self.hip_pitch_clip[1])
+            )
+            target[leg_idx * 3 + 2] = float(np.clip(knee_base, self.knee_clip[0], self.knee_clip[1]))
+        target[12:] = 0.0
+
+        stable = (
+            abs(np.degrees(roll)) <= self.stable_roll_deg
+            and abs(np.degrees(pitch)) <= self.stable_pitch_deg
+            and max(abs(np.degrees(roll_rate)), abs(np.degrees(pitch_rate))) <= self.stable_gyro_deg_s
+        )
+        self._stable_time = self._stable_time + self.control_dt if stable else 0.0
+        self._last_debug = StandBalanceDebug(
+            roll=roll,
+            pitch=pitch,
+            roll_rate=roll_rate,
+            pitch_rate=pitch_rate,
+            hip_base=hip_base,
+            knee_base=knee_base,
+            roll_corr=roll_corr,
+            pitch_corr=pitch_corr,
+            pitch_compensation_enabled=self.pitch_compensation_enabled,
+            target=target.tolist(),
+            stable=stable,
+        )
+        return target
+
+    def is_stable(self) -> bool:
+        return self._stable_time >= self.enter_hold_s