"""Web-facing session state machine for current sim2real deployment.""" from __future__ import annotations import threading import time import traceback import queue from collections import deque from dataclasses import asdict, dataclass, field from enum import Enum from pathlib import Path from typing import Any, Callable, Dict, Optional import numpy as np from tools.logger import LogBundle from input_dev.remote_uart import RemoteCommandSource class Stage(str, Enum): DISCONNECTED = "DISCONNECTED" CONNECTING = "CONNECTING" CONNECTED = "CONNECTED" ENABLING = "ENABLING" ENABLED = "ENABLED" JOINT_TEST = "JOINT_TEST" CALIBRATING = "CALIBRATING" STARTING_UP = "STARTING_UP" STAND_HOLD = "STAND_HOLD" RUNTIME = "RUNTIME" FAULTED = "FAULTED" ESTOPPED = "ESTOPPED" @dataclass class SessionStatus: stage: str = Stage.DISCONNECTED.value detail: str = "" last_event: str = "" busy: bool = False cmd: list = field(default_factory=lambda: [0.0, 0.0, 0.0]) input_mode: str = "WEB" remote_takeover_active: bool = False remote_takeover_allowed: bool = False remote_soft_estop: bool = False remote_status: Dict[str, Any] = field(default_factory=dict) last_state: Optional[Dict[str, Any]] = None log_dir: Optional[str] = None fault_reason: Optional[str] = None last_error: Optional[str] = None last_traceback: Optional[str] = None diagnostics: Dict[str, Any] = field(default_factory=dict) class RobotSession: JOINT_LABELS = LogBundle.JOINT_LABELS DEFAULT_ACTION_SCALE = np.array( [ 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0, ], dtype=np.float32, ) def __init__( self, cfg: Dict[str, Any], cfg_path: Path, driver_factory_real: Callable, driver_factory_dry: Callable, ): self.cfg = cfg self.cfg_path = cfg_path self.driver_factory_real = driver_factory_real self.driver_factory_dry = driver_factory_dry self.lock = threading.RLock() self.status = SessionStatus() self.io = None self.runner = None self.guard = None self.safety = None self.initializer = None self.stand_balance = None self.logger = None self._stop_runtime = threading.Event() self._cmd_lock = threading.Lock() self._cmd = np.zeros(3, dtype=np.float32) self._filtered_cmd = np.zeros(3, dtype=np.float32) self._last_raw_cmd = np.zeros(3, dtype=np.float32) self._estop = False self._busy_thread: Optional[threading.Thread] = None self._stand_target = None self._remote_source: Optional[RemoteCommandSource] = None self._remote_takeover_active: bool = False self._remote_soft_estop: bool = False self._remote_poll_error_count: int = 0 self._remote_last_error: Optional[str] = None self._stop_poll = threading.Event() self._poll_thread: Optional[threading.Thread] = None self._event_listeners: list = [] self._recent_events = deque(maxlen=300) self._listener_lock = threading.Lock() self._last_runtime_ts: float = 0.0 self._last_poll_ts: float = 0.0 self._last_command_ts: float = 0.0 self._runtime_loop_count: int = 0 self._runtime_overrun_count: int = 0 self._runtime_overrun_max_ms: float = 0.0 self._runtime_policy_stale_count: int = 0 self._runtime_policy_stale_max_ms: float = 0.0 self._last_loop_profile: Dict[str, float] = {} self._latest_target_info: Dict[str, Any] = {} self._latest_target: Optional[np.ndarray] = None self._poll_error_count: int = 0 self._api_error_count: int = 0 self._disconnecting: bool = False self._target_queue = queue.Queue(maxsize=1) self._state_lock = threading.Lock() self._latest_hardware_state = None self._policy_exception = None self._motor_exception = None self._status_exception = None self._runtime_error_event = threading.Event() def _policy_action_scale(self) -> np.ndarray: values = self.cfg.get("policy", {}).get("action_scale", self.DEFAULT_ACTION_SCALE.tolist()) action_scale = np.asarray(values, dtype=np.float32) if action_scale.shape != (16,): raise ValueError(f"policy.action_scale must be 16 values, got shape {action_scale.shape}") return action_scale def _diag_snapshot(self) -> Dict[str, Any]: return { "runtime_active": bool(self.status.stage == Stage.RUNTIME.value and not self._stop_runtime.is_set()), "runtime_loop_count": int(self._runtime_loop_count), "runtime_overrun_count": int(self._runtime_overrun_count), "runtime_overrun_max_ms": round(float(self._runtime_overrun_max_ms), 3), "runtime_policy_stale_count": int(self._runtime_policy_stale_count), "runtime_policy_stale_max_ms": round(float(self._runtime_policy_stale_max_ms), 3), "last_loop_profile": dict(self._last_loop_profile), "latest_target": self._latest_target_snapshot(), "filtered_cmd": self._filtered_cmd.tolist(), "raw_cmd": self._last_raw_cmd.tolist(), "last_runtime_age_s": round(time.time() - self._last_runtime_ts, 3) if self._last_runtime_ts else None, "last_poll_age_s": round(time.time() - self._last_poll_ts, 3) if self._last_poll_ts else None, "last_command_age_s": round(time.time() - self._last_command_ts, 3) if self._last_command_ts else None, "poll_thread_alive": bool(self._poll_thread and self._poll_thread.is_alive()), "busy_thread_alive": bool(self._busy_thread and self._busy_thread.is_alive()), "estop": bool(self._estop), "poll_error_count": int(self._poll_error_count), "api_error_count": int(self._api_error_count), "remote_poll_error_count": int(self._remote_poll_error_count), "remote_last_error": self._remote_last_error, "remote_takeover_active": bool(self._remote_takeover_active), "remote_soft_estop": bool(self._remote_soft_estop), "zero_cmd_suppression": ( bool(getattr(self.runner, "enable_zero_cmd_suppression", False)) if self.runner is not None else None ), "policy_path": str(getattr(self.runner, "policy_path", "")) if self.runner is not None else None, "stand_balance": self._stand_balance_snapshot(), } def _stand_balance_snapshot(self) -> Dict[str, Any]: if self.stand_balance is None: return {"enabled": False} debug = self.stand_balance.last_debug return { "enabled": bool(self.stand_balance.enabled), "pitch_compensation_enabled": bool(getattr(debug, "pitch_compensation_enabled", False)), "roll_deg": float(np.degrees(debug.roll)), "pitch_deg": float(np.degrees(debug.pitch)), "roll_rate_deg_s": float(np.degrees(debug.roll_rate)), "pitch_rate_deg_s": float(np.degrees(debug.pitch_rate)), "hip_base": float(debug.hip_base), "knee_base": float(debug.knee_base), "roll_corr": float(debug.roll_corr), "pitch_corr": float(debug.pitch_corr), "stable": bool(debug.stable), } def _policy_release_cfg(self) -> Dict[str, float]: policy_cfg = self.cfg.get("policy", {}) return { "command_hold_s": max(float(policy_cfg.get("release_command_hold_s", 0.12)), 0.0), "posture_max_err": max(float(policy_cfg.get("release_posture_max_err", 0.35)), 0.0), "target_blend_s": max(float(policy_cfg.get("release_target_blend_s", 0.30)), 1e-3), } def _compute_release_metrics(self, state: Dict[str, Any], hold_target: np.ndarray, cmd: np.ndarray) -> Dict[str, float]: joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) default_pose = np.asarray(self.runner.default_dof_pos, dtype=np.float32) hold_target = np.asarray(hold_target, dtype=np.float32) planar_cmd, yaw_cmd = self.runner.command_activation_metrics(cmd) return { "planar_cmd": float(planar_cmd), "yaw_cmd": float(yaw_cmd), "max_hold_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))), "max_default_err": float(np.max(np.abs(joint_pos[:12] - default_pose[:12]))), "max_hold_default_gap": float(np.max(np.abs(hold_target[:12] - default_pose[:12]))), } def _blend_runtime_target( self, hold_target: np.ndarray, policy_target: np.ndarray, release_alpha: float, target_blend_s: float, control_dt: float, ) -> np.ndarray: blend = min(1.0, release_alpha * (self.runner.command_release_s / max(target_blend_s, control_dt))) return ((1.0 - blend) * hold_target + blend * policy_target).astype(np.float32) def _compute_target_error_metrics( self, state: Dict[str, Any], hold_target: np.ndarray, policy_target: np.ndarray, ) -> Dict[str, float]: joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) hold_target = np.asarray(hold_target, dtype=np.float32) policy_target = np.asarray(policy_target, dtype=np.float32) return { "hold_target_max_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))), "policy_target_max_err": float(np.max(np.abs(joint_pos[:12] - policy_target[:12]))), "hold_policy_max_gap": float(np.max(np.abs(hold_target[:12] - policy_target[:12]))), } def _record_loop_profile(self, profile: Dict[str, float]) -> Dict[str, float]: compact = {k: round(float(v), 3) for k, v in profile.items()} self._last_loop_profile = compact return compact def _note_overrun(self, over_ms: float) -> None: self._runtime_overrun_count += 1 self._runtime_overrun_max_ms = max(self._runtime_overrun_max_ms, float(over_ms)) def _note_policy_stale(self, age_ms: float) -> None: self._runtime_policy_stale_count += 1 self._runtime_policy_stale_max_ms = max(self._runtime_policy_stale_max_ms, float(age_ms)) def _build_last_state( self, *, state: Dict[str, Any], target: np.ndarray, raw: np.ndarray, projected_gravity: np.ndarray, cmd: np.ndarray, loop_dt_ms: float, phase: str, safety_level: int = 0, guard_level: int = 0, safety_reason: str = "", guard_reason: str = "", zero_command: bool = True, runtime_released: bool = False, release_alpha: float = 0.0, release_active_hold_s: float = 0.0, release_max_hold_err: float = 0.0, loop_profile: Optional[Dict[str, float]] = None, extra: Optional[Dict[str, Any]] = None, ) -> Dict[str, Any]: motor_diag = state.get("motor_stale", {}) or {} extra_dict = extra or {} raw_cmd_list = extra_dict.get("raw_cmd", [0.0, 0.0, 0.0]) filtered_cmd_list = cmd.tolist() payload: Dict[str, Any] = { "joint_pos": state["joint_pos"].tolist(), "joint_vel": state["joint_vel"].tolist(), "joint_torque": state["joint_torque"].tolist(), "target": target.tolist(), "raw": raw.tolist(), "gyro": state["imu_gyro"].tolist(), "proj_gravity": projected_gravity.tolist(), "imu_age_ms": float(state["imu_age_ms"]), "imu_fresh": bool(state.get("imu_fresh", False)), "odom": state.get("odom"), "loop_dt_ms": float(loop_dt_ms), "loop_profile": loop_profile or {}, "holdover_total": int(self.io.hw.holdover_total), "safety_level": int(safety_level), "guard_level": int(guard_level), "phase": phase, "cmd": cmd.tolist(), "raw_cmd": raw_cmd_list, "filtered_cmd": filtered_cmd_list, "safety_reason": safety_reason, "guard_reason": guard_reason, "zero_command": bool(zero_command), "runtime_released": bool(runtime_released), "release_alpha": float(release_alpha), "release_active_hold_s": float(release_active_hold_s), "release_max_hold_err": float(release_max_hold_err), "per_motor_stale": motor_diag.get("per_motor_stale", [0] * 16), "motor_fresh_count": int(motor_diag.get("fresh_count", 16)), "motor_fresh_by_update_count": int(motor_diag.get("fresh_by_update_count", 0)), "motor_fresh_by_value_change": int(motor_diag.get("fresh_by_value_change", 0)), "motor_update_counts": motor_diag.get("update_counts", [0] * 16), "motor_temperatures": motor_diag.get("temperatures", [0.0] * 16), "motor_fault_codes": motor_diag.get("fault_codes", [0] * 16), "motor_mode_states": motor_diag.get("mode_states", [0] * 16), "latest_target": self._latest_target_snapshot(), } if extra: payload.update(extra) return payload def _signal_stats(self, obs: Optional[np.ndarray], raw: Optional[np.ndarray], scaled: Optional[np.ndarray]) -> Dict[str, float]: stats: Dict[str, float] = {} if obs is not None and np.asarray(obs).size: obs_arr = np.asarray(obs, dtype=np.float32) stats.update( { "obs_min": float(np.min(obs_arr)), "obs_max": float(np.max(obs_arr)), "obs_abs_max": float(np.max(np.abs(obs_arr))), } ) if raw is not None and np.asarray(raw).size: raw_arr = np.asarray(raw, dtype=np.float32) stats.update( { "raw_min": float(np.min(raw_arr)), "raw_max": float(np.max(raw_arr)), "raw_abs_max": float(np.max(np.abs(raw_arr))), } ) if scaled is not None and np.asarray(scaled).size: scaled_arr = np.asarray(scaled, dtype=np.float32) stats.update( { "scaled_min": float(np.min(scaled_arr)), "scaled_max": float(np.max(scaled_arr)), "scaled_abs_max": float(np.max(np.abs(scaled_arr))), } ) return stats def _update_latest_target(self, target: np.ndarray, source: str) -> None: target = np.asarray(target, dtype=np.float32) now = time.time() delta_max = 0.0 if self._latest_target is not None and self._latest_target.shape == target.shape: delta_max = float(np.max(np.abs(target - self._latest_target))) self._latest_target = target.copy() self._latest_target_info = { "source": source, "t": now, "delta_max": delta_max, "target_abs_max": float(np.max(np.abs(target))) if target.size else 0.0, } def _latest_target_snapshot(self) -> Dict[str, Any]: if not self._latest_target_info: return {} snap = dict(self._latest_target_info) snap["age_ms"] = (time.time() - float(snap.get("t", time.time()))) * 1000.0 return snap def _filter_command(self, raw_cmd: np.ndarray, dt: float) -> np.ndarray: raw_cmd = np.asarray(raw_cmd, dtype=np.float32) self._last_raw_cmd = raw_cmd.copy() cfg = self.cfg.get("command_filter", {}) or {} if not bool(cfg.get("enabled", False)): self._filtered_cmd = raw_cmd.copy() return raw_cmd limits = np.array( [ float(cfg.get("max_vx_acc", 1.0)), float(cfg.get("max_vy_acc", 1.0)), float(cfg.get("max_yaw_acc", 1.5)), ], dtype=np.float32, ) max_delta = np.maximum(limits * max(float(dt), 1e-3), 0.0) delta = np.clip(raw_cmd - self._filtered_cmd, -max_delta, max_delta) self._filtered_cmd = (self._filtered_cmd + delta).astype(np.float32) return self._filtered_cmd.copy() def _remote_cfg(self) -> Dict[str, Any]: return dict(self.cfg.get("remote", {}) or {}) def _build_remote_source(self) -> Optional[RemoteCommandSource]: remote_cfg = self._remote_cfg() port = str(remote_cfg.get("port") or "").strip() if not remote_cfg.get("enabled", False) or not port: return None return RemoteCommandSource( port=port, baudrate=int(remote_cfg.get("baudrate", 100000)), timeout=float(remote_cfg.get("timeout", 0.02)), axis_deadzone=int(remote_cfg.get("axis_deadzone", 50)), active_threshold=int(remote_cfg.get("active_threshold", 50)), axis_full_scale=float(remote_cfg.get("axis_full_scale", 660.0)), max_vx=float(remote_cfg.get("max_vx", self.cfg["controller"]["max_vx"])), max_vy=float(remote_cfg.get("max_vy", self.cfg["controller"]["max_vy"])), max_yaw=float(remote_cfg.get("max_yaw_rate", self.cfg["controller"]["max_yaw_rate"])), invert_vx=bool(remote_cfg.get("invert_vx", False)), invert_vy=bool(remote_cfg.get("invert_vy", False)), invert_yaw=bool(remote_cfg.get("invert_yaw", False)), ) def _remote_takeover_allowed(self) -> bool: return bool( self.status.stage == Stage.RUNTIME.value and self._remote_source is not None ) def _refresh_input_status(self) -> None: remote_status = {} if self._remote_source is not None: remote_status = self._remote_source.get_status() remote_status["available"] = True remote_status["last_error"] = self._remote_last_error else: remote_status = {"available": False, "last_error": self._remote_last_error} self.status.input_mode = "REMOTE" if self._remote_takeover_active else "WEB" self.status.remote_takeover_active = bool(self._remote_takeover_active) self.status.remote_takeover_allowed = bool(self._remote_takeover_allowed()) self.status.remote_soft_estop = bool(self._remote_soft_estop) self.status.remote_status = remote_status def _poll_remote(self) -> None: if self._remote_source is None: self._remote_soft_estop = False self._refresh_input_status() return try: remote_state = self._remote_source.poll() self._remote_last_error = None self._remote_soft_estop = bool(remote_state.estop_requested) except Exception as exc: self._remote_poll_error_count += 1 self._remote_last_error = f"{type(exc).__name__}: {exc}" self._remote_soft_estop = False if self._remote_poll_error_count <= 3 or self._remote_poll_error_count % 20 == 0: self._broadcast({"kind": "REMOTE_POLL_ERROR", "error": self._remote_last_error}) self._refresh_input_status() def _update_diag_locked(self) -> None: self._refresh_input_status() self.status.diagnostics = self._diag_snapshot() def note_api_error(self) -> None: with self.lock: self._api_error_count += 1 self._update_diag_locked() def _set_fault(self, exc: Exception, tb: str) -> None: error_text = f"{type(exc).__name__}: {exc}" with self.lock: self.status.fault_reason = error_text self.status.last_error = error_text self.status.last_traceback = tb self._update_diag_locked() def get_status(self) -> Dict[str, Any]: with self.lock: self._update_diag_locked() return asdict(self.status) def get_debug_snapshot(self) -> Dict[str, Any]: return {"status": self.get_status(), "recent_events": list(self._recent_events)} def _set(self, **kwargs): with self.lock: for key, value in kwargs.items(): setattr(self.status, key, value) self._update_diag_locked() snapshot = asdict(self.status) self._broadcast({"kind": "STATUS", **snapshot}) def _set_stage(self, stage: Stage, detail: str = ""): self._set(stage=stage.value, detail=detail) def _build_action_diag( self, *, state: Dict[str, Any], raw: np.ndarray, scaled: np.ndarray, tentative: np.ndarray, cmd: np.ndarray, zero_command: bool, runtime_released: bool, release_alpha: float, safety_details: Optional[Dict[str, Any]] = None, ) -> Dict[str, Any]: details = dict(safety_details or {}) joint_indices = list(details.get("joint_indices", [])) joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) default_pose = np.asarray(self.runner.default_dof_pos, dtype=np.float32) pos_err = tentative - joint_pos leg_offset = tentative[:12] - default_pose[:12] diag: Dict[str, Any] = { "joint_indices": joint_indices, "joint_names": [self.JOINT_LABELS[i] for i in joint_indices if 0 <= i < len(self.JOINT_LABELS)], "cmd": cmd.tolist(), "zero_command": bool(zero_command), "runtime_released": bool(runtime_released), "release_alpha": float(release_alpha), "max_raw": float(np.max(np.abs(raw))) if raw.size else 0.0, "max_scaled": float(np.max(np.abs(scaled[:12]))) if scaled.size else 0.0, "max_target": float(np.max(np.abs(tentative[:12]))) if tentative.size else 0.0, } if joint_indices: primary = int(joint_indices[0]) diag.update( { "primary_joint_index": primary, "primary_joint_name": self.JOINT_LABELS[primary], "primary_target": float(tentative[primary]), "primary_default": float(default_pose[primary]), "primary_measured": float(joint_pos[primary]), "primary_pos_err": float(pos_err[primary]), "primary_raw": float(raw[primary]), "primary_scaled": float(scaled[primary]), } ) if primary < 12: diag["primary_leg_offset"] = float(leg_offset[primary]) details.update(diag) return details def add_listener(self, q: "queue.Queue"): with self._listener_lock: self._event_listeners.append(q) for event in list(self._recent_events): try: q.put_nowait(event) except Exception: pass def remove_listener(self, q: "queue.Queue"): with self._listener_lock: if q in self._event_listeners: self._event_listeners.remove(q) def _broadcast(self, event: Dict[str, Any]): payload = dict(event) payload["t"] = time.time() self._recent_events.append(payload) with self._listener_lock: for listener in list(self._event_listeners): try: listener.put_nowait(payload) except Exception: pass def _run_async(self, fn, *args, **kwargs) -> bool: with self.lock: if self.status.busy: return False self.status.busy = True self.status.fault_reason = None self.status.last_error = None self.status.last_traceback = None self._update_diag_locked() def _wrap(): try: fn(*args, **kwargs) except Exception as exc: tb = traceback.format_exc() print(f"\n[Background Task Error] {fn.__name__}") print(tb) self._set_fault(exc, tb) self._broadcast({"kind": "BG_TASK_ERROR", "fn": fn.__name__, "error": str(exc), "traceback": tb}) self._set_stage(Stage.FAULTED, detail=str(exc)) try: if self.io: self.io.damping_brake() except Exception: pass finally: with self.lock: self.status.busy = False self._update_diag_locked() self._broadcast({"kind": "BG_TASK_DONE", "fn": fn.__name__}) self._busy_thread = threading.Thread(target=_wrap, daemon=True) self._busy_thread.start() return True def connect(self, dry_run: bool = False): return self._run_async(self._do_connect, dry_run) def _do_connect(self, dry_run: bool): if self.status.stage != Stage.DISCONNECTED.value: self._broadcast({"kind": "WARN", "msg": "already connected"}) return self._set_stage(Stage.CONNECTING, "connecting hardware") from interface.real_io import RealIO from safety.runtime_guard import RuntimeGuard from safety.safety_monitor import SafetyMonitor from startup.pose_initializer import PoseInitializer from startup.stand_balance import StandBalanceController from tools.logger import LogBundle cfg = self.cfg control_dt = 1.0 / float(cfg["control_freq"]) motor_dt = 1.0 / float(cfg.get("motor_freq", 200)) driver_factory = self.driver_factory_dry() if dry_run else self.driver_factory_real() self.logger = LogBundle(cfg["log_dir"]) self._set(log_dir=str(self.logger.dir)) self.logger.event( "CONFIG_LOADED", config_path=str(self.cfg_path), dry_run=dry_run, control_freq=cfg["control_freq"], motor_model=cfg["motor_model"], ) self.io = RealIO( driver_factory=driver_factory, motor_model=cfg["motor_model"], can1_port=cfg["can1_port"], can2_port=cfg["can2_port"], imu_lib_path=cfg.get("imu_lib_path"), control_dt=control_dt, motor_dt=motor_dt, kp_leg=cfg["controller"]["kp_leg"], kd_leg=cfg["controller"]["kd_leg"], hold_kp_leg=cfg["controller"].get("hold_kp_leg", cfg["controller"]["kp_leg"]), hold_kd_leg=cfg["controller"].get("hold_kd_leg", cfg["controller"]["kd_leg"]), kd_wheel=cfg["controller"]["kd_wheel"], debug=cfg.get("debug", False), dry_run=dry_run, ) self.guard = RuntimeGuard( max_ang_vel=cfg["safety"]["max_ang_vel"], max_tilt_z=cfg["safety"]["max_tilt_z"], imu_age_warn_ms=cfg["safety"].get("imu_age_warn_ms", 60.0), imu_age_stop_ms=cfg["safety"].get("imu_age_stop_ms", 200.0), ) self.safety = SafetyMonitor( max_target_offset=cfg["safety"]["max_target_offset"], max_ang_vel=cfg["safety"]["max_ang_vel"], max_tilt_z=cfg["safety"]["max_tilt_z"], clip_to_brake=cfg["safety"].get("clip_to_brake", 0), hard_target_offset=cfg["safety"].get("hard_target_offset", 1.2), ) self.initializer = PoseInitializer( self.io, control_dt=control_dt, transition_time_min=cfg["startup"].get("transition_time_min", 2.0), transition_time_max=cfg["startup"].get("transition_time_max", 6.0), transition_seconds_per_rad=cfg["startup"].get("transition_seconds_per_rad", 1.5), hold_time=cfg["startup"]["hold_time"], settle_pos_threshold=cfg["startup"]["settle_pos_threshold"], settle_vel_threshold=cfg["startup"]["settle_vel_threshold"], timeout_extra=cfg["startup"].get("timeout_extra", 3.0), imu_fresh_wait_s=cfg["startup"].get("imu_fresh_wait_s", 1.0), progress_log_interval=cfg["startup"]["progress_log_interval"], ramp_kp_time=cfg["startup"].get("ramp_kp_time", 1.0), soft_hold_duration=cfg["startup"].get("soft_hold_duration", 1.0), max_dev_warn=cfg["startup"].get("max_dev_warn", 1.5), max_dev_abort=cfg["startup"].get("max_dev_abort", 3.0), ) self.stand_balance = StandBalanceController(cfg.get("stand_balance", {}), control_dt=control_dt) self._remote_source = self._build_remote_source() self._remote_takeover_active = False self._remote_soft_estop = False self._remote_last_error = None class _WebEstop: def __init__(self, owner): self.owner = owner def is_estop_triggered(self): return self.owner._estop self.initializer.attach(self.logger, self.guard, _WebEstop(self)) self.io.connect(imu_timeout_ms=cfg.get("imu_start_timeout_ms", 8000)) if self._remote_source is not None: try: self._remote_source.open() self.logger.event("REMOTE_CONNECTED", port=self._remote_source.port) self._poll_remote() except Exception as exc: print(f"[RobotSession] 警告: 无法打开遥控器串口 {self._remote_source.port} ({exc})。已自动禁用遥控。") self._remote_source = None self.logger.event("CAN_IMU_CONNECTED", initial_gravity=self.io.imu.initial_gravity) self._set_stage(Stage.CONNECTED, "hardware connected") def disconnect(self): return self._run_async(self._do_disconnect) def _do_disconnect(self): if self._disconnecting: return self._disconnecting = True self._stop_runtime.set() self._stop_state_poll() time.sleep(0.05) try: if self.io: self.io.damping_brake() except Exception: pass try: if self._remote_source: self._remote_source.close() except Exception: pass try: if self._remote_source: self._remote_source.close() except Exception: pass try: if self.io: self.io.disconnect() except Exception: pass if self.logger: self.logger.event("HARDWARE_DISCONNECTED") self.logger.close() self.io = None self.runner = None self.logger = None self._stand_target = None self._remote_source = None self._remote_takeover_active = False self._remote_soft_estop = False self._remote_last_error = None self._set_stage(Stage.DISCONNECTED, "hardware disconnected") self._disconnecting = False def enable_motors(self): return self._run_async(self._do_enable) def _do_enable(self): if self.status.stage not in (Stage.CONNECTED.value, Stage.STAND_HOLD.value, Stage.FAULTED.value): return self._set_stage(Stage.ENABLING, "enabling motors") self.io.enable_motors() self.logger.event("MOTORS_ENABLED") time.sleep(0.5) self._set_stage(Stage.ENABLED, "motors enabled") self._start_state_poll() def _start_state_poll(self): self._stop_poll.clear() if self._poll_thread and self._poll_thread.is_alive(): return def _poll_loop(): while not self._stop_poll.is_set(): if self.status.stage == Stage.RUNTIME.value: time.sleep(0.2) continue try: stage = self.status.stage self._poll_remote() if stage == Stage.ENABLED.value: self.io.hw.passive_poll() state = self.io.read_state() if stage == Stage.STAND_HOLD.value and self.stand_balance is not None and self.stand_balance.enabled: self._stand_target = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) self.io.hold_pose(self._stand_target, kp_scale=1.0) self._update_latest_target(self._stand_target, "stand_balance") elif stage == Stage.STAND_HOLD.value and self._stand_target is not None: self.io.hold_pose(self._stand_target, kp_scale=1.0) self._update_latest_target(self._stand_target, "stand_hold") motor_diag = state.get("motor_stale", {}) self._last_poll_ts = time.time() self._set( last_state={ "joint_pos": state["joint_pos"].tolist(), "joint_vel": state["joint_vel"].tolist(), "joint_torque": state["joint_torque"].tolist(), "target": self._stand_target.tolist() if self._stand_target is not None else [0.0] * 16, "raw": [0.0] * 16, "gyro": state["imu_gyro"].tolist(), "proj_gravity": state["projected_gravity"].tolist(), "imu_age_ms": float(state["imu_age_ms"]), "loop_dt_ms": 0.0, "holdover_total": int(getattr(self.io.hw, "holdover_total", 0)), "safety_level": 0, "guard_level": 0, "phase": "POLL", "stand_balance": self._stand_balance_snapshot(), "latest_target": self._latest_target_snapshot(), "per_motor_stale": motor_diag.get("per_motor_stale", [0] * 16), } ) except Exception as exc: self._poll_error_count += 1 self._broadcast({"kind": "POLL_ERROR", "error": str(exc), "traceback": traceback.format_exc()}) time.sleep(0.2) self._poll_thread = threading.Thread(target=_poll_loop, daemon=True) self._poll_thread.start() def _stop_state_poll(self): self._stop_poll.set() thread = self._poll_thread if thread and thread.is_alive() and thread is not threading.current_thread(): thread.join(timeout=0.5) self._poll_thread = None def disable_motors(self): return self._run_async(self._do_disable) def _do_disable(self): self._stop_state_poll() try: self.io.damping_brake() except Exception: pass time.sleep(0.05) self.io.disable_motors() self.logger.event("MOTORS_DISABLED") self._set_stage(Stage.CONNECTED, "motors disabled") def startup(self): return self._run_async(self._do_startup) def _do_startup(self): from startup.pose_initializer import PoseInitFailed, STAND_POSE if self.status.stage != Stage.ENABLED.value: raise RuntimeError("startup requires ENABLED") self._set_stage(Stage.STARTING_UP, detail="transition to stand pose") try: target = self.initializer.transition_to_stand_from_current(target_pose=STAND_POSE) self._stand_target = target if self.stand_balance is not None and self.stand_balance.enabled: self.logger.event("STAND_BALANCE_BEGIN") self.stand_balance.reset() stable_deadline = time.perf_counter() + 6.0 while time.perf_counter() < stable_deadline: state = self.io.read_state() self._stand_target = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) self.io.hold_pose(self._stand_target, kp_scale=1.0) if self.stand_balance.is_stable(): debug = self.stand_balance.last_debug self.logger.event( "STAND_BALANCE_STABLE", roll_deg=float(np.degrees(debug.roll)), pitch_deg=float(np.degrees(debug.pitch)), pitch_corr=float(debug.pitch_corr), pitch_compensation_enabled=bool(debug.pitch_compensation_enabled), ) break time.sleep(1.0 / float(self.cfg["control_freq"])) self.logger.event("STAND_BALANCE_END") self._set_stage(Stage.STAND_HOLD, detail="stand-balance hold active") else: self._set_stage(Stage.STAND_HOLD, detail="holding stand pose with PD") except PoseInitFailed as exc: self.logger.event("POSE_INIT_FAILED", error=str(exc)) try: self.io.damping_brake() except Exception: pass self._set_stage(Stage.FAULTED, detail=str(exc)) raise def _run_policy_loop_wrap(self, control_dt: float, require_active_command: bool): from safety.runtime_guard import GuardLevel from safety.safety_monitor import SafetyLevel try: next_exec = time.perf_counter() runtime_released = not require_active_command release_cfg = self._policy_release_cfg() release_active_time = 0.0 release_block_reason = "active command required" if require_active_command else "" while not self._stop_runtime.is_set() and not self._runtime_error_event.is_set(): if getattr(self, "_debug_hang_policy", False): time.sleep(0.1) continue with self._cmd_lock: web_cmd = self._cmd.copy() self._poll_remote() raw_cmd = self._remote_source.get_command() if self._remote_takeover_active and self._remote_source is not None else web_cmd cmd = self._filter_command(raw_cmd, control_dt) with self._state_lock: state = self._latest_hardware_state if state is None: next_exec += control_dt slack = next_exec - time.perf_counter() if slack > 0: time.sleep(slack) continue obs = self.io.get_obs_policy(state, cmd, self.runner.default_dof_pos, self.runner.last_actions) zero_command = self.runner._is_zero_command(cmd, state["imu_gyro"]) if np.any(np.isnan(obs)) or np.any(np.isinf(obs)): raise ValueError("Observation vector contains NaN or Inf values") if self._remote_takeover_active and self._remote_soft_estop: raise RuntimeError("Remote soft estop triggered") if not runtime_released and zero_command: raw = np.zeros(16, dtype=np.float32) scaled = np.zeros(16, dtype=np.float32) target_hold = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if self.stand_balance is not None and self.stand_balance.enabled else self.runner.default_dof_pos.copy() tentative = target_hold.copy().astype(np.float32) policy_target = self.runner.default_dof_pos.copy() release_active_time = 0.0 release_block_reason = "zero command" runtime_blend_ratio = 0.0 else: target_hold = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if self.stand_balance is not None and self.stand_balance.enabled else self.runner.default_dof_pos.copy() release_metrics = self._compute_release_metrics(state, target_hold, cmd) if not runtime_released: release_active_time += control_dt if self.runner.is_command_active(cmd) else 0.0 active_ready = release_active_time >= release_cfg["command_hold_s"] posture_ready = release_metrics["max_hold_err"] <= release_cfg["posture_max_err"] if active_ready and posture_ready: runtime_released = True self.logger.event( "RUNTIME_COMMAND_RELEASED", cmd=cmd.tolist(), active_hold_s=release_active_time, max_hold_err=release_metrics["max_hold_err"], max_default_err=release_metrics["max_default_err"], max_hold_default_gap=release_metrics["max_hold_default_gap"], ) else: reasons = [] if not active_ready: reasons.append(f"cmd_hold<{release_cfg['command_hold_s']:.2f}s") if not posture_ready: reasons.append(f"hold_err>{release_cfg['posture_max_err']:.3f}") release_block_reason = ",".join(reasons) if not runtime_released: raw = np.zeros(16, dtype=np.float32) scaled = np.zeros(16, dtype=np.float32) tentative = target_hold.copy().astype(np.float32) policy_target = self.runner.default_dof_pos.copy() runtime_blend_ratio = 0.0 else: scaled, raw = self.runner.step(obs, control_dt) if np.any(np.isnan(raw)) or np.any(np.isinf(raw)): raise ValueError("Policy action contains NaN or Inf values") policy_target = (scaled + self.runner.default_dof_pos).astype(np.float32) tentative = self._blend_runtime_target( target_hold, policy_target, float(getattr(self.runner, "_command_release_alpha", 0.0)), release_cfg["target_blend_s"], control_dt, ) scaled = tentative - self.runner.default_dof_pos runtime_blend_ratio = min( 1.0, float(getattr(self.runner, "_command_release_alpha", 0.0)) * (self.runner.command_release_s / max(release_cfg["target_blend_s"], control_dt)), ) joint_cmd = { "time": time.perf_counter(), "target": tentative, "raw": raw, "scaled": scaled, "policy_target": policy_target, "zero_command": zero_command, "runtime_released": runtime_released, "release_alpha": float(getattr(self.runner, "_command_release_alpha", 0.0)), "release_active_time": release_active_time, "release_block_reason": release_block_reason, "runtime_blend_ratio": runtime_blend_ratio, "obs": obs, "cmd": cmd.copy(), "raw_cmd": raw_cmd.copy(), } try: self._target_queue.put_nowait(joint_cmd) except queue.Full: try: self._target_queue.get_nowait() self._target_queue.put_nowait(joint_cmd) except Exception: pass next_exec += control_dt slack = next_exec - time.perf_counter() if slack > 0: time.sleep(slack) except Exception as exc: self._policy_exception = exc self._policy_traceback = traceback.format_exc() self._runtime_error_event.set() def _run_motor_loop_wrap(self, motor_dt: float, policy_timeout_s: float, policy_stale_warn_s: float, log_every: int): from safety.runtime_guard import GuardLevel from safety.safety_monitor import SafetyLevel try: next_exec = time.perf_counter() loop_count = 0 last_target = None last_target_time = 0.0 last_stale_log_time = 0.0 last_overrun_log_time = 0.0 self._last_runtime_ts = time.time() self._runtime_loop_count = 0 self._last_loop_dt_ms = 0.0 self._last_loop_profile_snapshot = {} log_decimation = max(1, int(round(0.02 / motor_dt))) if motor_dt < 0.02 else 1 while not self._stop_runtime.is_set() and not self._runtime_error_event.is_set(): loop_t0 = time.perf_counter() loop_profile = {} def mark_profile(name: str) -> None: nonlocal profile_last now_profile = time.perf_counter() loop_profile[name] = (now_profile - profile_last) * 1000.0 profile_last = now_profile profile_last = loop_t0 state = self.io.read_state() mark_profile("read_state_ms") with self._state_lock: self._latest_hardware_state = state try: joint_cmd = self._target_queue.get_nowait() last_target = joint_cmd last_target_time = joint_cmd["time"] except queue.Empty: pass current_time = time.perf_counter() if last_target is None: actual_target = self.io.hold_pose(self._stand_target, kp_scale=1.0) self._update_latest_target(actual_target, "runtime_zero_hold") mark_profile("hold_pose_ms") if loop_count * motor_dt > 2.0: raise TimeoutError("Initial policy target wait timeout") raw = np.zeros(16, dtype=np.float32) scaled = np.zeros(16, dtype=np.float32) projected_gravity = state["projected_gravity"] safety_level = 0 guard_level = 0 safety_reason = "" guard_reason = "" zero_command = True runtime_released = False release_alpha = 0.0 release_active_time = 0.0 release_max_hold_err = 0.0 cmd = np.zeros(3, dtype=np.float32) raw_cmd = np.zeros(3, dtype=np.float32) extra = { "raw_cmd": raw_cmd.tolist(), } else: age_s = current_time - last_target_time if age_s > policy_stale_warn_s: age_ms = age_s * 1000.0 self._note_policy_stale(age_ms) if current_time - last_stale_log_time > 1.0: self.logger.event("POLICY_TARGET_STALE", age_ms=age_ms, timeout_ms=policy_timeout_s * 1000.0) last_stale_log_time = current_time if age_s > policy_timeout_s: self.logger.event("POLICY_TIMEOUT", age_ms=age_s * 1000.0) raise TimeoutError(f"Policy target age {age_s*1000.0:.1f}ms exceeds safety limit {policy_timeout_s*1000.0:.1f}ms") tentative = last_target["target"] raw = last_target["raw"] scaled = last_target["scaled"] policy_target = last_target["policy_target"] zero_command = last_target["zero_command"] runtime_released = last_target["runtime_released"] release_alpha = last_target["release_alpha"] release_active_time = last_target["release_active_time"] obs = last_target["obs"] cmd = last_target["cmd"] raw_cmd = last_target["raw_cmd"] runtime_blend_ratio = last_target["runtime_blend_ratio"] projected_gravity = state["projected_gravity"] guard_decision = self.guard.check( imu_gyro=state["imu_gyro"], projected_gravity=projected_gravity, imu_age_ms=float(state["imu_age_ms"]), estop_triggered=self._estop, extra_nan_arrays=(raw, tentative), ) mark_profile("safety_ms") if guard_decision.level == GuardLevel.STOP: self.logger.event("GUARD_STOP", phase="RUNTIME", reason=guard_decision.reason) raise RuntimeError(f"RuntimeGuard STOP: {guard_decision.reason}") safety_decision = self.safety.check( target_pose=tentative, default_pose=self.runner.default_dof_pos, imu_gyro=state["imu_gyro"], projected_gravity=projected_gravity, estop_triggered=self._estop, ) if safety_decision.level == SafetyLevel.ESTOP: self.logger.event("SAFETY_ESTOP", reason=safety_decision.message) raise RuntimeError(f"SafetyMonitor ESTOP: {safety_decision.message}") if safety_decision.level == SafetyLevel.BRAKE: safety_diag = self._build_action_diag( state=state, raw=raw, scaled=scaled, tentative=tentative, cmd=cmd, zero_command=zero_command, runtime_released=runtime_released, release_alpha=release_alpha, safety_details=safety_decision.details, ) self.logger.event("SAFETY_BRAKE", reason=safety_decision.message, details=safety_diag) raise RuntimeError(f"SafetyMonitor BRAKE: {safety_decision.message}") if safety_decision.level == SafetyLevel.CLIP and safety_decision.clipped_target is not None: scaled = safety_decision.clipped_target - self.runner.default_dof_pos tentative = safety_decision.clipped_target if runtime_released or not zero_command: actual_target = self.io.send_actions(scaled, self.runner.default_dof_pos) self._update_latest_target(actual_target, "runtime_policy") mark_profile("send_actions_ms") else: actual_target = self.io.hold_pose(tentative, kp_scale=1.0) self._update_latest_target(actual_target, "runtime_zero_hold") mark_profile("hold_pose_ms") loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0 self._last_runtime_ts = time.time() self._runtime_loop_count += 1 self._last_loop_dt_ms = loop_dt_ms self._last_loop_profile_snapshot = loop_profile.copy() release_max_hold_err = float(np.max(np.abs(state["joint_pos"][:12] - tentative[:12]))) target_metrics = self._compute_target_error_metrics(state, tentative, policy_target) extra = self._signal_stats(obs, raw, scaled) extra.update({ "raw_cmd": raw_cmd.tolist(), "max_raw": float(np.max(np.abs(raw))), "clip_joint_indices": (safety_decision.details or {}).get("joint_indices", []), "clip_joint_names": (safety_decision.details or {}).get("joint_names", []), "clip_primary_joint": (safety_decision.details or {}).get("primary_joint_name"), "clip_primary_target": (safety_decision.details or {}).get("primary_target"), "clip_primary_measured": (safety_decision.details or {}).get("primary_measured"), "clip_primary_default": (safety_decision.details or {}).get("primary_default"), "clip_primary_pos_err": (safety_decision.details or {}).get("primary_pos_err"), "clip_primary_raw": (safety_decision.details or {}).get("primary_raw"), "clip_primary_scaled": (safety_decision.details or {}).get("primary_scaled"), }) safety_level = int(safety_decision.level) guard_level = int(guard_decision.level) safety_reason = safety_decision.message guard_reason = guard_decision.reason if log_every and (loop_count % (log_every * log_decimation) == 0): motor_diag = state.get("motor_stale", {}) or {} stand_diag = self._stand_balance_snapshot() self.logger.state( phase="RUNTIME", joint_pos=state["joint_pos"], joint_vel=state["joint_vel"], joint_torque=state["joint_torque"], target_pose=actual_target, raw_action=raw, gyro=state["imu_gyro"], accel=state["imu_accel"], quat=state["quat_wxyz"], proj_gravity=projected_gravity, command=cmd, imu_age_ms=float(state["imu_age_ms"]), loop_dt_ms=loop_dt_ms, safety_level=safety_level, guard_level=guard_level, 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_leg_cmd=float(self.io.kp_leg), kd_leg_cmd=float(self.io.kd_leg), kd_wheel_cmd=float(self.io.kd_wheel), runtime_release_alpha=release_alpha, runtime_release_hold_s=release_active_time, runtime_blend_ratio=runtime_blend_ratio, hold_target_max_err=target_metrics["hold_target_max_err"], policy_target_max_err=target_metrics["policy_target_max_err"], hold_policy_max_gap=target_metrics["hold_policy_max_gap"], stand_roll_deg=float(stand_diag.get("roll_deg", 0.0)), stand_pitch_deg=float(stand_diag.get("pitch_deg", 0.0)), stand_roll_corr=float(stand_diag.get("roll_corr", 0.0)), stand_pitch_corr=float(stand_diag.get("pitch_corr", 0.0)), stand_pitch_comp_enabled=bool(stand_diag.get("pitch_compensation_enabled", False)), target_source="runtime_blend" if runtime_blend_ratio < 0.999 else "runtime_policy", clip_primary_joint=str((safety_decision.details or {}).get("primary_joint_name", "")), clip_primary_target=float((safety_decision.details or {}).get("primary_target", 0.0) or 0.0), clip_primary_measured=float((safety_decision.details or {}).get("primary_measured", 0.0) or 0.0), clip_primary_default=float((safety_decision.details or {}).get("primary_default", 0.0) or 0.0), clip_primary_pos_err=float((safety_decision.details or {}).get("primary_pos_err", 0.0) or 0.0), clip_primary_raw=float((safety_decision.details or {}).get("primary_raw", 0.0) or 0.0), clip_primary_scaled=float((safety_decision.details or {}).get("primary_scaled", 0.0) or 0.0), safety_reason=safety_reason, guard_reason=guard_reason, ) mark_profile("log_ms") with self._state_lock: self._latest_motor_diagnostics = { "actual_target": actual_target, "raw": raw, "scaled": scaled, "projected_gravity": projected_gravity, "safety_level": safety_level, "guard_level": guard_level, "safety_reason": safety_reason, "guard_reason": guard_reason, "zero_command": zero_command, "runtime_released": runtime_released, "release_alpha": release_alpha, "release_active_time": release_active_time, "release_max_hold_err": release_max_hold_err, "extra": extra, "cmd": cmd, } loop_profile["total_ms"] = (time.perf_counter() - loop_t0) * 1000.0 self._record_loop_profile(loop_profile) next_exec += motor_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)) elif slack < -motor_dt: over_ms = -slack * 1000.0 self._note_overrun(over_ms) if time.perf_counter() - last_overrun_log_time > 0.5: self.logger.event("LOOP_OVERRUN", over_ms=over_ms) last_overrun_log_time = time.perf_counter() next_exec = time.perf_counter() loop_count += 1 except Exception as exc: self._motor_exception = exc self._motor_traceback = traceback.format_exc() self._runtime_error_event.set() def _run_status_loop_wrap(self, status_dt: float): try: while not self._stop_runtime.is_set() and not self._runtime_error_event.is_set(): start_time = time.perf_counter() with self._state_lock: state = self._latest_hardware_state diag = getattr(self, "_latest_motor_diagnostics", None) if state is not None and diag is not None: loop_profile = getattr(self, "_last_loop_profile_snapshot", {}) loop_dt_ms = getattr(self, "_last_loop_dt_ms", 0.0) self._set( cmd=diag["cmd"].tolist(), last_state=self._build_last_state( state=state, target=diag["actual_target"], raw=diag["raw"], projected_gravity=diag["projected_gravity"], cmd=diag["cmd"], loop_dt_ms=loop_dt_ms, phase="RUNTIME", safety_level=diag["safety_level"], guard_level=diag["guard_level"], safety_reason=diag["safety_reason"], guard_reason=diag["guard_reason"], zero_command=diag["zero_command"], runtime_released=diag["runtime_released"], release_alpha=diag["release_alpha"], release_active_hold_s=diag["release_active_time"], release_max_hold_err=diag["release_max_hold_err"], loop_profile=loop_profile, extra=diag["extra"], ) ) elapsed = time.perf_counter() - start_time sleep_time = max(0.0, status_dt - elapsed) time.sleep(sleep_time) except Exception as exc: self._status_exception = exc self._status_traceback = traceback.format_exc() self._runtime_error_event.set() def runtime_start(self, policy_path: Optional[str] = None): return self._run_async(self._do_runtime_start, policy_path) def _do_runtime_start(self, policy_path: Optional[str]): from policy.policy_runner import PolicyRunner, resolve_policy_path from safety.runtime_guard import GuardLevel from safety.safety_monitor import SafetyLevel if self.status.stage != Stage.STAND_HOLD.value: raise RuntimeError("runtime start requires STAND_HOLD") sim2real_root = Path(__file__).resolve().parents[1] resolved_policy = resolve_policy_path(policy_path, sim2real_root) if not resolved_policy.exists(): raise RuntimeError(f"policy not found: {resolved_policy}") self.runner = PolicyRunner( resolved_policy, enable_zero_cmd_suppression=self.cfg.get("policy", {}).get("enable_zero_cmd_suppression", True), hold_zero_command_pose=self.cfg.get("policy", {}).get("hold_zero_command_pose", True), command_release_s=self.cfg.get("policy", {}).get("command_release_s", 0.35), action_scale=self._policy_action_scale(), zero_cmd_use_yaw_rate=self.cfg.get("policy", {}).get("zero_cmd_use_yaw_rate", False), clip_obs=self.cfg.get("policy", {}).get("clip_obs", 100.0), ) self.safety.reset() require_active_command = self.cfg.get("policy", {}).get("require_active_command_to_release", True) self.logger.event("POLICY_LOADED", path=str(resolved_policy)) self._stop_state_poll() control_dt = 1.0 / float(self.cfg.get("policy_freq", 50)) target = self._stand_target self.logger.event("PRIME_BEGIN") zero_cmd = np.zeros(3, dtype=np.float32) for index in range(1): if self.stand_balance is not None and self.stand_balance.enabled: state = self.io.read_state() target = self.stand_balance.compute_target(state, zero_cmd) self.io.hold_pose(target, kp_scale=1.0) else: self.io.hold_pose(target, kp_scale=1.0) state = self.io.read_state() obs = self.io.get_obs_policy(state, zero_cmd, self.runner.default_dof_pos, self.runner.last_actions) if index == 0: self.runner.reset(prime_obs=obs) time.sleep(control_dt) self.logger.event("PRIME_END") state = self.io.read_state() projected_gravity = state["projected_gravity"] if abs(projected_gravity[0]) > 0.5 or abs(projected_gravity[1]) > 0.5: error_message = ( f"IMU frame mismatch or body tilt too large: " f"gravity projection X={projected_gravity[0]:.2f}, Y={projected_gravity[1]:.2f}" ) self.logger.event("GUARD_STOP", phase="STARTUP", reason=error_message) self._set_stage(Stage.FAULTED, error_message) return self.logger.event("HISTORY_PRIMED", initial_obs=obs) self._stop_runtime.clear() self._runtime_loop_count = 0 self._runtime_overrun_count = 0 self._runtime_overrun_max_ms = 0.0 self._runtime_policy_stale_count = 0 self._runtime_policy_stale_max_ms = 0.0 self._filtered_cmd[:] = 0.0 self._last_raw_cmd[:] = 0.0 self._latest_target = None self._latest_target_info = {} self._remote_takeover_active = False self._remote_soft_estop = False self._set_stage(Stage.RUNTIME, detail="Decoupled policy & motor loop active") self.logger.event("RUNTIME_BEGIN") # 启动解耦的多 Loop # 1. 清空解耦队列与状态缓存 while not self._target_queue.empty(): try: self._target_queue.get_nowait() except Exception: break self._latest_hardware_state = state self._policy_exception = None self._motor_exception = None self._status_exception = None self._runtime_error_event.clear() # 2. 读取配置频率 motor_dt = 1.0 / float(self.cfg.get("motor_freq", 200)) status_dt = 1.0 / float(self.cfg.get("status_freq", 10)) policy_timeout_s = float(self.cfg.get("policy_timeout_ms", 150.0)) / 1000.0 policy_stale_warn_s = float(self.cfg.get("policy_stale_warn_ms", 60.0)) / 1000.0 log_every = int(self.cfg.get("log_every", 1)) # 3. 创建并启动线程 policy_thread = threading.Thread( target=self._run_policy_loop_wrap, args=(control_dt, require_active_command), name="PolicyLoop", daemon=True ) motor_thread = threading.Thread( target=self._run_motor_loop_wrap, args=(motor_dt, policy_timeout_s, policy_stale_warn_s, log_every), name="MotorLoop", daemon=True ) status_thread = threading.Thread( target=self._run_status_loop_wrap, args=(status_dt,), name="StatusLoop", daemon=True ) policy_thread.start() motor_thread.start() status_thread.start() # 4. 主工作监控忙等，直到用户触发 stop 或是线程抛出异常 try: while not self._stop_runtime.is_set() and not self._runtime_error_event.is_set(): time.sleep(0.05) if self._runtime_error_event.is_set(): for exc, tb, name in [ (self._motor_exception, getattr(self, "_motor_traceback", ""), "MotorLoop"), (self._policy_exception, getattr(self, "_policy_traceback", ""), "PolicyLoop"), (self._status_exception, getattr(self, "_status_traceback", ""), "StatusLoop"), ]: if exc is not None: raise exc finally: # 5. 确保通知所有工作线程退出并等待它们 self._stop_runtime.set() policy_thread.join(timeout=1.0) motor_thread.join(timeout=1.0) status_thread.join(timeout=1.0) if self.status.stage == Stage.RUNTIME.value: self.logger.event("RUNTIME_STOP") self._remote_takeover_active = False self._remote_soft_estop = False self._filtered_cmd[:] = 0.0 self._last_raw_cmd[:] = 0.0 self._latest_target = None self._latest_target_info = {} self._set_stage(Stage.STAND_HOLD, detail="runtime stopped, back to stand-balance hold") self._start_state_poll() def runtime_stop(self): self._stop_runtime.set() return True def estop(self): self._estop = True try: if self.io: self.io.damping_brake() except Exception: pass self._stop_runtime.set() if self.logger: self.logger.event("USER_ESTOP_WEB") self._set_stage(Stage.ESTOPPED, detail="web emergency stop") return True def reset_estop(self): self._estop = False self._set(detail="estop cleared") return True def set_remote_takeover(self, enabled: bool): enabled = bool(enabled) if enabled: if self.status.stage != Stage.RUNTIME.value: raise RuntimeError("remote takeover requires RUNTIME") if self._remote_source is None: raise RuntimeError("remote controller not configured") self._poll_remote() self._remote_takeover_active = True self._filtered_cmd[:] = 0.0 self._last_raw_cmd[:] = 0.0 if self.logger: self.logger.event("REMOTE_TAKEOVER_ENABLED", port=self._remote_source.port) self._set(detail="remote takeover enabled") return True self._remote_takeover_active = False self._remote_soft_estop = False self._filtered_cmd[:] = 0.0 self._last_raw_cmd[:] = 0.0 if self.logger: self.logger.event("REMOTE_TAKEOVER_DISABLED") self._set(detail="remote takeover disabled") return True def set_command(self, vx: float, vy: float, yaw: float): with self._cmd_lock: self._cmd[0] = float(vx) self._cmd[1] = float(vy) self._cmd[2] = float(yaw) self._last_command_ts = time.time() self._set(cmd=self._cmd.tolist()) return True def test_motor(self, leg: str, joint: str, delta_rad: float, kp: float, kd: float, duration_s: float): return self._run_async(self._do_test_motor, leg, joint, delta_rad, kp, kd, duration_s) def _do_test_motor(self, leg: str, joint: str, delta_rad: float, kp: float, kd: float, duration_s: float): if self.io is None: raise RuntimeError("hardware not connected") if self.status.stage not in (Stage.ENABLED.value, Stage.STAND_HOLD.value, Stage.FAULTED.value): raise RuntimeError("test motor requires ENABLED/STAND_HOLD/FAULTED") joint_key = (str(leg), str(joint)) if joint_key not in self.io.hw.mapper.SIM_INDEX_MAP: raise RuntimeError(f"unknown joint: {leg}_{joint}") idx = self.io.hw.mapper.SIM_INDEX_MAP[joint_key] base_pose = self.io.read_measured_pose().astype(np.float32) target_pose = base_pose.copy() target_pose[idx] += float(delta_rad) prev_stage = self.status.stage self._set_stage(Stage.JOINT_TEST, detail=f"testing {leg}_{joint}") if self.logger: self.logger.event( "JOINT_TEST_BEGIN", joint=f"{leg}_{joint}", joint_index=idx, delta_rad=float(delta_rad), kp=float(kp), kd=float(kd), duration_s=float(duration_s), start_pos=float(base_pose[idx]), target_pos=float(target_pose[idx]), ) self._stop_state_poll() next_exec = time.perf_counter() deadline = next_exec + max(float(duration_s), 0.1) samples = [] while time.perf_counter() < deadline: state = self.io.read_state() measured = float(state["joint_pos"][idx]) error = float(target_pose[idx] - measured) torque = float(state["joint_torque"][idx]) vel = float(state["joint_vel"][idx]) samples.append((measured, error, vel, torque)) self.io.hw.send_control(target_pose, float(kp), float(kd), self.io.kd_wheel) next_exec += 1.0 / float(self.cfg["control_freq"]) slack = next_exec - time.perf_counter() if slack > 0: time.sleep(slack) self.io.hold_pose(base_pose, kp_scale=1.0) final_state = self.io.read_state() final_measured = float(final_state["joint_pos"][idx]) if self.logger: self.logger.event( "JOINT_TEST_END", joint=f"{leg}_{joint}", joint_index=idx, final_pos=final_measured, final_err=float(target_pose[idx] - final_measured), max_abs_err=float(max(abs(s[1]) for s in samples) if samples else 0.0), max_abs_vel=float(max(abs(s[2]) for s in samples) if samples else 0.0), max_abs_tau=float(max(abs(s[3]) for s in samples) if samples else 0.0), ) self._set(stage=prev_stage, detail=f"joint test {leg}_{joint} done") if prev_stage in (Stage.ENABLED.value, Stage.STAND_HOLD.value): self._start_state_poll() def list_logs(self): log_root = Path(self.cfg.get("log_dir", "logs")) if not log_root.exists(): return [] out = [] for directory in sorted(log_root.iterdir(), reverse=True): if not directory.is_dir(): continue state_path = directory / "state.csv" events_path = directory / "events.jsonl" out.append( { "id": directory.name, "state_csv": state_path.exists(), "events_jsonl": events_path.exists(), "size_kb": ( (state_path.stat().st_size + events_path.stat().st_size) // 1024 if state_path.exists() and events_path.exists() else 0 ), } ) return out