[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,329 @@
+"""起立姿态初始化器（实测起点版本）。
+
+设计：
+  - 不再假设机器人的物理起始姿态（不再有 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)
+
+
+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,
+                 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.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(self.io.kp_leg * kp_scale),
+                kd_leg_cmd=float(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)
+            while time.perf_counter() < next_exec:
+                pass
+        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)
+
+        start_pose = self.io.read_measured_pose().astype(np.float32).copy()
+        start_pose[12:] = 0.0   # 轮子起点固定为 0 速度
+
+        # === 2. 偏差检查 ===
+        diff = np.abs(start_pose[:12] - target_pose[:12])
+        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 * (target_pose[:12] - start_pose[:12])
+            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(joint_pos[:12] - target_pose[:12])))
+            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