381 lines
17 KiB
Python
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()
|