from __future__ import annotations import threading import time from pathlib import Path from typing import Any import numpy as np from hal.base import HardwareBridge, HardwareFrame from .config import ( apply_imu_calibration, rad_to_ticks, raw_vel_to_rad_s, save_hardware_profile, ticks_to_rad, ) from .observations import build_observation, decode_action from .policy_runner import PolicySet from .state_machine import LifecycleState, MicroduckStateMachine class MicroduckController: """50Hz real-time control loop coordinating HAL, 61D observations, ONNX policies, and safety.""" def __init__( self, base_dir: str | Path, manifest: dict[str, Any], profile: dict[str, Any], hal: HardwareBridge, policies: PolicySet, custom_script: dict[str, Any] | None = None, profile_path: str | Path | None = None, ) -> None: self.base_dir = Path(base_dir) self.profile_path = Path(profile_path) if profile_path else self.base_dir / "config" / "hardware_profile.json" self.manifest = manifest self.profile = profile self.hal = hal self.policies = policies self.joint_names: list[str] = list(manifest["joint_names"]) self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32) self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02)) self.state_machine = MicroduckStateMachine(manifest, custom_script=custom_script) self._lock = threading.RLock() self._running = False self._thread: threading.Thread | None = None self.last_action = np.zeros(14, dtype=np.float32) self._last_q_rad: np.ndarray | None = None self._vel_lpf = np.zeros(14, dtype=np.float64) self._last_remote_input_s: float = 0.0 self._last_frame: HardwareFrame | None = None self._last_gyro = np.zeros(3, dtype=np.float64) self._last_grav = np.array([0.0, 0.0, -1.0], dtype=np.float64) self._last_q = self.default_pose.astype(np.float64).copy() self._last_targets = self.default_pose.astype(np.float64).copy() self._ctrl_hz = 0.0 self._last_error: str | None = None self._calib_target_ticks: list[int] | None = None self._calib_torque_on = False def connect(self) -> None: with self._lock: self.hal.connect() self.hal.configure(self.joint_names, self.profile) self._last_frame = self.hal.sync_step( enable_torque=False, target_ticks=rad_to_ticks(self.default_pose, self.joint_names, self.profile), ) self._last_gyro, self._last_grav = apply_imu_calibration( self._last_frame.gyro_raw, self._last_frame.proj_gravity_raw, self.profile ) self._last_q = ticks_to_rad(self._last_frame.ticks, self.joint_names, self.profile) def start(self) -> None: if self._running: return self.connect() self._running = True self._thread = threading.Thread(target=self._loop, name="microduck-50hz", daemon=True) self._thread.start() def stop(self) -> None: self._running = False if self._thread is not None: self._thread.join(timeout=2.0) self._thread = None with self._lock: try: self.state_machine.emergency_stop() self.hal.set_torque(False) finally: self.hal.close() def _loop(self) -> None: next_t = time.monotonic() hz_t0 = next_t ticks = 0 while self._running: now = time.monotonic() try: self.step_once(dt=self.ctrl_dt, now_s=now) self._last_error = None except Exception as exc: self._last_error = str(exc) with self._lock: self.state_machine.emergency_stop() try: self.hal.set_torque(False) except Exception: pass ticks += 1 if now - hz_t0 >= 0.5: self._ctrl_hz = ticks / max(now - hz_t0, 1e-6) ticks = 0 hz_t0 = now next_t += self.ctrl_dt sleep_s = next_t - time.monotonic() if sleep_s > 0: time.sleep(sleep_s) else: next_t = time.monotonic() def set_remote_input( self, norm_x: float, norm_wz: float, speed_scale: float | None = None, now_s: float | None = None, ) -> None: with self._lock: scale = ( float(speed_scale) if speed_scale is not None else float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5)) ) self.state_machine.set_velocity_cmd(norm_x, norm_wz, speed_scale=scale) self._last_remote_input_s = time.monotonic() if now_s is None else float(now_s) def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None: with self._lock: self.state_machine.set_head_target(neck_pitch, head_pitch, head_yaw, head_roll) def set_auto_stand_on_fall(self, enabled: bool) -> None: with self._lock: self.profile.setdefault("safety", {})["auto_stand_on_fall"] = bool(enabled) save_hardware_profile(self.profile_path, self.profile, self.joint_names) def set_speed_scale(self, scale: float) -> None: with self._lock: clamped = max(0.1, min(1.0, float(scale))) self.profile.setdefault("safety", {})["cmd_vel_scale"] = clamped save_hardware_profile(self.profile_path, self.profile, self.joint_names) def request_stand(self) -> None: with self._lock: if "walk" not in self.policies.policies: raise RuntimeError("No control policies loaded yet — deploy a bundle from the website first") self._calib_torque_on = False self._calib_target_ticks = None soft_start_s = float(self.profile.get("safety", {}).get("soft_start_s", 1.5)) self.state_machine.request_stand(self._last_q, soft_start_s=soft_start_s) def emergency_stop(self) -> None: with self._lock: self._calib_torque_on = False self._calib_target_ticks = None self.state_machine.emergency_stop() self.last_action.fill(0) self.hal.set_torque(False) def trigger_action(self, action: str) -> bool: with self._lock: return self.state_machine.trigger_action(action) def reload_runtime( self, manifest: dict[str, Any], policies: PolicySet, profile: dict[str, Any] | None = None, custom_script: dict[str, Any] | None = None, ) -> None: with self._lock: self.state_machine.emergency_stop() self.hal.set_torque(False) self.manifest = manifest self.policies = policies self.joint_names = list(manifest["joint_names"]) self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32) self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02)) self.state_machine.update_manifest(manifest, custom_script=custom_script) if profile is not None: self.profile = profile self.hal.configure(self.joint_names, self.profile) self.last_action.fill(0) self._last_error = None def update_profile(self, profile: dict[str, Any]) -> dict[str, Any]: with self._lock: self.profile = save_hardware_profile(self.profile_path, profile, self.joint_names) self.hal.configure(self.joint_names, self.profile) return self.profile def capture_joint_zero(self, joint_name: str) -> dict[str, Any]: with self._lock: if joint_name not in self.joint_names: raise ValueError(f"Unknown joint '{joint_name}'") idx = self.joint_names.index(joint_name) if self._last_frame is None: raise RuntimeError("No hardware frame captured yet") neutral = int(self.profile["servo"]["neutral_tick"]) current_tick = int(self._last_frame.ticks[idx]) self.profile["joints"][joint_name]["zero_offset_ticks"] = current_tick - neutral return self.update_profile(self.profile) def test_single_joint(self, joint_name: str, target_rad: float) -> list[int]: with self._lock: if joint_name not in self.joint_names: raise ValueError(f"Unknown joint '{joint_name}'") if self.state_machine.lifecycle not in (LifecycleState.IDLE, LifecycleState.CALIBRATION): raise RuntimeError("Emergency stop first before testing individual joints") self.state_machine.enter_calibration() idx = self.joint_names.index(joint_name) pose = self._last_q.copy() pose[idx] = float(target_rad) ticks = rad_to_ticks(pose, self.joint_names, self.profile) self._calib_target_ticks = ticks self._calib_torque_on = True return ticks def exit_calibration(self) -> None: with self._lock: self._calib_torque_on = False self._calib_target_ticks = None self.state_machine.exit_calibration() self.hal.set_torque(False) def calibrate_imu_zero(self, samples: int = 200) -> dict[str, Any]: with self._lock: bias = self.hal.calibrate_imu_raw(samples=samples) self.profile["imu"]["gyro_bias_rad_s"] = [float(b) for b in bias] self.update_profile(self.profile) return self.profile["imu"] def ping_servos(self, max_id: int = 20) -> list[int]: with self._lock: return self.hal.ping_servos(max_id=max_id) def _compute_joint_vel(self, q_rad: np.ndarray, raw_speeds: list[int], dt: float) -> np.ndarray: servo_cfg = self.profile["servo"] if servo_cfg.get("vel_source") == "servo": return raw_vel_to_rad_s(raw_speeds, self.joint_names, self.profile) if self._last_q_rad is None: self._last_q_rad = q_rad.copy() self._vel_lpf.fill(0.0) return self._vel_lpf.copy() fd = (q_rad - self._last_q_rad) / max(dt, 1e-4) self._last_q_rad = q_rad.copy() alpha = float(servo_cfg.get("vel_lpf_alpha", 0.4)) self._vel_lpf = (1.0 - alpha) * self._vel_lpf + alpha * fd return self._vel_lpf.copy() def step_once(self, dt: float | None = None, now_s: float | None = None) -> dict[str, Any]: with self._lock: dt_s = self.ctrl_dt if dt is None else float(dt) now = time.monotonic() if now_s is None else float(now_s) # Enforce mobile WS heartbeat timeout while walking ws_timeout_s = float(self.profile.get("safety", {}).get("ws_timeout_ms", 500)) / 1000.0 if self._last_remote_input_s > 0.0 and (now - self._last_remote_input_s) > ws_timeout_s: self.state_machine.set_velocity_cmd(0.0, 0.0) servo_cfg = self.profile["servo"] speed_limit = int(servo_cfg.get("speed_limit", 0)) acc = int(servo_cfg.get("acc", 50)) # Determine command to send for THIS cycle from current state + previous sensor state auto_stand = bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False)) if self.state_machine.lifecycle == LifecycleState.CALIBRATION: enable_torque = self._calib_torque_on cmd_ticks = ( list(self._calib_target_ticks) if self._calib_target_ticks is not None else rad_to_ticks(self._last_q, self.joint_names, self.profile) ) decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand) else: decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand) if decision.zero_last_action: self.last_action.fill(0) if decision.lifecycle == LifecycleState.SOFT_START: q_interp = ( 1.0 - decision.soft_start_alpha ) * self.state_machine.soft_start_q0 + decision.soft_start_alpha * self.default_pose self._last_targets = q_interp cmd_ticks = rad_to_ticks(q_interp, self.joint_names, self.profile) enable_torque = True elif decision.lifecycle == LifecycleState.ACTIVE and decision.policy_key is not None: policy = self.policies.policies.get(decision.policy_key) or self.policies.policies["walk"] q_vel = self._vel_lpf obs = build_observation( self._last_gyro, self._last_grav, self._last_q, q_vel, self.last_action, decision.command_13, self.default_pose, ) raw_act = policy.infer(obs) self.last_action = raw_act targets = decode_action(raw_act, self.default_pose, policy.action_scale) self._last_targets = targets.astype(np.float64) cmd_ticks = rad_to_ticks(targets, self.joint_names, self.profile) enable_torque = True elif decision.lifecycle == LifecycleState.ACTIVE and decision.recovery_state == "fallen": cmd_ticks = rad_to_ticks(self._last_targets, self.joint_names, self.profile) enable_torque = True else: cmd_ticks = rad_to_ticks(self._last_q, self.joint_names, self.profile) enable_torque = False frame = self.hal.sync_step( enable_torque=enable_torque, target_ticks=cmd_ticks, speed_limit=speed_limit, acc=acc, ) self._last_frame = frame self._last_gyro, self._last_grav = apply_imu_calibration( frame.gyro_raw, frame.proj_gravity_raw, self.profile ) self._last_q = ticks_to_rad(frame.ticks, self.joint_names, self.profile) self._compute_joint_vel(self._last_q, frame.raw_speeds, dt_s) return self._build_status_unlocked(decision, enable_torque) def _build_status_unlocked(self, decision: Any | None = None, torque_enabled: bool | None = None) -> dict[str, Any]: frame = self._last_frame life = self.state_machine.lifecycle.value active_policy = decision.policy_key if decision else None if torque_enabled is None: torque_enabled = life in (LifecycleState.SOFT_START.value, LifecycleState.ACTIVE.value) or self._calib_torque_on return { "hal": self.hal.name, "policy_backend": self.policies.backend, "lifecycle": life, "mode_label": decision.mode_label if decision else self.state_machine._mode_label(), "skill_mode": self.state_machine.skill_mode, "active_policy": active_policy, "recovery_state": self.state_machine.recovery_state, "loco": self.state_machine.loco, "rollers_enabled": self.state_machine.rollers_enabled, "head_mode": self.state_machine.head_mode, "torque_enabled": bool(torque_enabled), "auto_stand_on_fall": bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False)), "speed_scale": float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5)), "ctrl_hz": round(self._ctrl_hz, 1), "voltage_v": round(frame.voltage_v, 2) if frame else 0.0, "max_temp_c": round(frame.max_temp_c, 1) if frame else 0.0, "imu_ok": frame.imu_ok if frame else False, "servo_err_mask": frame.servo_err_mask if frame else 0, "gyro": [round(float(x), 4) for x in self._last_gyro], "projected_gravity": [round(float(x), 4) for x in self._last_grav], "ticks": list(frame.ticks) if frame else [2048] * 14, "joint_pos_rad": [round(float(x), 4) for x in self._last_q], "loaded_skills": sorted(self.policies.policies.keys()), "custom_move": self.manifest.get("custom_move"), "error": self._last_error, } def get_status(self) -> dict[str, Any]: with self._lock: return self._build_status_unlocked()