Files
VibemakerSandbox/hardware/unoq/python/core/controller.py
T

381 lines
17 KiB
Python

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()