feat: add Microduck UNO Q deployment and hotspot skill control

This commit is contained in:
2026-11-19 13:49:08 +08:00
parent 2b0c395508
commit 164a397726
99 changed files with 12488 additions and 4 deletions
+1
View File
@@ -0,0 +1 @@
"""Microduck Arduino UNO Q Python runtime package."""
+1
View File
@@ -0,0 +1 @@
"""Core configuration, observation pipeline, ONNX runner, and control loop."""
+176
View File
@@ -0,0 +1,176 @@
from __future__ import annotations
import json
import math
from pathlib import Path
from typing import Any
import numpy as np
TWO_PI = 2.0 * math.pi
def validate_robot_manifest(manifest: dict[str, Any]) -> dict[str, Any]:
if not isinstance(manifest, dict):
raise ValueError("robot_manifest must be a JSON object")
if manifest.get("obs_size") != 61:
raise ValueError(f"Unsupported obs_size {manifest.get('obs_size')}; expected 61")
if manifest.get("action_size") != 14:
raise ValueError(f"Unsupported action_size {manifest.get('action_size')}; expected 14")
if manifest.get("command_size") != 13:
raise ValueError(f"Unsupported command_size {manifest.get('command_size')}; expected 13")
joint_names = manifest.get("joint_names")
default_pose = manifest.get("default_pose")
if not isinstance(joint_names, list) or len(joint_names) != 14:
raise ValueError("robot_manifest.joint_names must contain 14 joints")
if not isinstance(default_pose, list) or len(default_pose) != 14:
raise ValueError("robot_manifest.default_pose must contain 14 floats")
if not all(isinstance(v, (int, float)) and math.isfinite(float(v)) for v in default_pose):
raise ValueError("robot_manifest.default_pose contains non-finite value")
skills = manifest.get("skills")
if not isinstance(skills, dict):
raise ValueError("robot_manifest.skills must be an object")
for required in ("walk", "stand"):
if required not in skills or not isinstance(skills[required], dict) or not skills[required].get("path"):
raise ValueError(f"robot_manifest.skills missing required '{required}' policy")
return manifest
def load_robot_manifest(path: str | Path) -> dict[str, Any]:
with Path(path).open("r", encoding="utf-8") as f:
data = json.load(f)
return validate_robot_manifest(data)
def validate_hardware_profile(profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]:
if not isinstance(profile, dict):
raise ValueError("hardware_profile must be a JSON object")
servo = profile.get("servo")
if not isinstance(servo, dict):
raise ValueError("hardware_profile.servo missing")
ticks_per_rev = int(servo.get("ticks_per_rev", 0))
if ticks_per_rev <= 0:
raise ValueError("hardware_profile.servo.ticks_per_rev must be > 0")
imu = profile.get("imu")
if not isinstance(imu, dict):
raise ValueError("hardware_profile.imu missing")
axis_map = np.asarray(imu.get("axis_map", []), dtype=np.float64)
if axis_map.shape != (3, 3) or not np.all(np.isfinite(axis_map)):
raise ValueError("hardware_profile.imu.axis_map must be a 3x3 finite matrix")
joints = profile.get("joints")
if not isinstance(joints, dict):
raise ValueError("hardware_profile.joints missing")
expected = joint_names or list(joints.keys())
if len(expected) != 14:
raise ValueError("hardware_profile.joints must configure all 14 joints")
seen_ids: set[int] = set()
for name in expected:
if name not in joints:
raise ValueError(f"hardware_profile.joints missing '{name}'")
cfg = joints[name]
jid = int(cfg["id"])
if not (0 <= jid <= 253) or jid in seen_ids:
raise ValueError(f"Invalid or duplicate servo id {jid} for joint {name}")
seen_ids.add(jid)
if int(cfg["sign"]) not in (-1, 1):
raise ValueError(f"Joint {name} sign must be -1 or 1")
if float(cfg["min_rad"]) > float(cfg["max_rad"]):
raise ValueError(f"Joint {name} min_rad > max_rad")
return profile
def load_hardware_profile(path: str | Path, joint_names: list[str] | None = None) -> dict[str, Any]:
with Path(path).open("r", encoding="utf-8") as f:
data = json.load(f)
return validate_hardware_profile(data, joint_names=joint_names)
def save_hardware_profile(path: str | Path, profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]:
validated = validate_hardware_profile(profile, joint_names=joint_names)
target = Path(path)
target.parent.mkdir(parents=True, exist_ok=True)
tmp = target.with_suffix(".json.tmp")
with tmp.open("w", encoding="utf-8") as f:
json.dump(validated, f, indent=2, ensure_ascii=False)
f.write("\n")
tmp.replace(target)
return validated
def rad_to_ticks(q_rad: np.ndarray | list[float], joint_names: list[str], profile: dict[str, Any]) -> list[int]:
servo = profile["servo"]
ticks_per_rev = int(servo["ticks_per_rev"])
neutral = int(servo["neutral_tick"])
rad_per_tick = TWO_PI / ticks_per_rev
joints_cfg = profile["joints"]
max_tick = ticks_per_rev - 1
out: list[int] = []
for idx, name in enumerate(joint_names):
cfg = joints_cfg[name]
angle = float(q_rad[idx])
if not math.isfinite(angle):
raise ValueError(f"Non-finite angle for joint {name}")
angle = max(float(cfg["min_rad"]), min(float(cfg["max_rad"]), angle))
tick = round(neutral + int(cfg["zero_offset_ticks"]) + int(cfg["sign"]) * (angle / rad_per_tick))
tick = max(0, min(max_tick, tick))
out.append(int(tick))
return out
def ticks_to_rad(ticks: np.ndarray | list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray:
servo = profile["servo"]
ticks_per_rev = int(servo["ticks_per_rev"])
neutral = int(servo["neutral_tick"])
rad_per_tick = TWO_PI / ticks_per_rev
joints_cfg = profile["joints"]
out = np.zeros(len(joint_names), dtype=np.float64)
for idx, name in enumerate(joint_names):
cfg = joints_cfg[name]
zero = neutral + int(cfg["zero_offset_ticks"])
out[idx] = int(cfg["sign"]) * (float(ticks[idx]) - zero) * rad_per_tick
return out
def raw_vel_to_rad_s(raw_vel: list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray:
"""Decode Feetech signed-magnitude speed ticks (bit 15 = negative) to rad/s."""
servo = profile["servo"]
scale = float(servo.get("vel_scale_rad_s_per_unit", TWO_PI / int(servo["ticks_per_rev"])))
joints_cfg = profile["joints"]
out = np.zeros(len(joint_names), dtype=np.float64)
for idx, name in enumerate(joint_names):
raw = int(raw_vel[idx])
mag = raw & 0x7FFF
signed = -mag if (raw & 0x8000) else mag
out[idx] = int(joints_cfg[name]["sign"]) * signed * scale
return out
def apply_imu_calibration(
gyro_raw: np.ndarray | list[float],
grav_raw: np.ndarray | list[float],
profile: dict[str, Any],
) -> tuple[np.ndarray, np.ndarray]:
imu = profile["imu"]
axis_map = np.asarray(imu["axis_map"], dtype=np.float64)
gyro_bias = np.asarray(imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0]), dtype=np.float64)
g_in = np.asarray(gyro_raw, dtype=np.float64)
p_in = np.asarray(grav_raw, dtype=np.float64)
if g_in.shape != (3,) or p_in.shape != (3,):
raise ValueError("IMU vectors must be 3D")
if not (np.all(np.isfinite(g_in)) and np.all(np.isfinite(p_in))):
raise ValueError("IMU readings contain non-finite values")
gyro = axis_map @ (g_in - gyro_bias)
grav = axis_map @ p_in
norm = float(np.linalg.norm(grav))
if norm > 1e-6:
grav = grav / norm
return gyro, grav
+380
View File
@@ -0,0 +1,380 @@
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()
+252
View File
@@ -0,0 +1,252 @@
from __future__ import annotations
import io
import json
import shutil
import stat
import tempfile
import threading
import time
import zipfile
from pathlib import Path, PurePosixPath
from typing import Any
from .config import save_hardware_profile, validate_hardware_profile, validate_robot_manifest
from .controller import MicroduckController
from .policy_runner import PolicySet
MAX_ZIP_FILES = 512
MAX_ZIP_UNCOMPRESSED_BYTES = 128 * 1024 * 1024
def _safe_rel_path(member_name: str) -> PurePosixPath:
if not member_name or "\\" in member_name:
raise ValueError(f"Unsafe zip member path: {member_name!r}")
if member_name.startswith("/") or ":" in member_name:
raise ValueError(f"Absolute or drive path forbidden in zip: {member_name!r}")
parts = PurePosixPath(member_name).parts
if not parts or any(part in ("..", ".", "") for part in parts):
raise ValueError(f"Path traversal forbidden in zip: {member_name!r}")
return PurePosixPath(*parts)
class DeployService:
"""Atomic, validated hot-reload deployment of Microduck manifest + ONNX policies."""
def __init__(
self,
base_dir: str | Path,
controller: MicroduckController,
policy_backend: str = "auto",
) -> None:
self.base_dir = Path(base_dir)
self.controller = controller
self.policy_backend = policy_backend
self._lock = threading.Lock()
self.last_deploy: dict[str, Any] | None = self._read_last_deploy()
def _read_last_deploy(self) -> dict[str, Any] | None:
meta_path = self.base_dir / "config" / ".last_deploy.json"
if not meta_path.is_file():
return None
try:
with meta_path.open("r", encoding="utf-8") as f:
return json.load(f)
except Exception:
return None
def _extract_bundle_to_stage(self, raw_bytes: bytes, stage_dir: Path) -> None:
if not raw_bytes:
raise ValueError("Empty deployment bundle")
try:
zf = zipfile.ZipFile(io.BytesIO(raw_bytes), "r")
except zipfile.BadZipFile as exc:
raise ValueError("Deployment payload is not a valid ZIP archive") from exc
with zf:
infos = zf.infolist()
if len(infos) > MAX_ZIP_FILES:
raise ValueError(f"Too many files in deployment zip ({len(infos)} > {MAX_ZIP_FILES})")
total_size = 0
entries: list[tuple[zipfile.ZipInfo, PurePosixPath]] = []
for info in infos:
if info.is_dir():
if info.filename.rstrip("/"):
_safe_rel_path(info.filename.rstrip("/"))
continue
mode = (info.external_attr >> 16) & 0o170000
if mode == stat.S_IFLNK:
raise ValueError(f"Symlinks are forbidden in deployment zip: {info.filename!r}")
total_size += max(0, int(info.file_size))
if total_size > MAX_ZIP_UNCOMPRESSED_BYTES:
raise ValueError("Deployment zip exceeds maximum uncompressed size (128 MB)")
entries.append((info, _safe_rel_path(info.filename)))
if not entries:
raise ValueError("Deployment zip contains no files")
top_dirs = {p.parts[0] for _, p in entries if len(p.parts) > 1}
has_root_manifest = any(
str(p) in ("config/robot_manifest.json", "robot_manifest.json") for _, p in entries
)
strip_prefix: str | None = None
if not has_root_manifest and len(top_dirs) == 1:
only_top = next(iter(top_dirs))
if any(
str(p) in (f"{only_top}/config/robot_manifest.json", f"{only_top}/robot_manifest.json")
for _, p in entries
):
strip_prefix = only_top
for info, rel in entries:
if strip_prefix and rel.parts[0] == strip_prefix:
if len(rel.parts) == 1:
continue
rel = PurePosixPath(*rel.parts[1:])
dest = (stage_dir / Path(*rel.parts)).resolve()
if not dest.is_relative_to(stage_dir.resolve()):
raise ValueError(f"Escaping path rejected: {info.filename!r}")
dest.parent.mkdir(parents=True, exist_ok=True)
with zf.open(info, "r") as src, dest.open("wb") as dst:
shutil.copyfileobj(src, dst)
def apply_zip_bytes(self, raw_bytes: bytes, overwrite_profile: bool = False) -> dict[str, Any]:
with self._lock, tempfile.TemporaryDirectory(prefix="microduck_deploy_") as tmp:
stage = Path(tmp) / "stage"
backup = Path(tmp) / "backup"
stage.mkdir(parents=True, exist_ok=True)
backup.mkdir(parents=True, exist_ok=True)
self._extract_bundle_to_stage(raw_bytes, stage)
manifest_src = stage / "config" / "robot_manifest.json"
if not manifest_src.is_file():
alt = stage / "robot_manifest.json"
if alt.is_file():
manifest_src = alt
else:
raise ValueError("Deployment zip missing config/robot_manifest.json")
try:
manifest = validate_robot_manifest(json.loads(manifest_src.read_text(encoding="utf-8")))
except json.JSONDecodeError as exc:
raise ValueError(f"Invalid robot_manifest.json: {exc}") from exc
# Normalize into stage/config & verify every referenced ONNX exists & is non-empty
(stage / "config").mkdir(parents=True, exist_ok=True)
(stage / "config" / "robot_manifest.json").write_text(
json.dumps(manifest, indent=2) + "\n", encoding="utf-8"
)
stage_policies: dict[str, Path] = {}
for key, spec in manifest["skills"].items():
rel = spec.get("path")
if not rel:
continue
rel_posix = _safe_rel_path(str(rel))
pol_file = (stage / Path(*rel_posix.parts)).resolve()
if not pol_file.is_relative_to(stage.resolve()) or not pol_file.is_file():
raise ValueError(f"Deployment zip missing policy '{key}' at '{rel}'")
if pol_file.stat().st_size == 0:
raise ValueError(f"Policy '{key}' at '{rel}' is empty")
stage_policies[key] = pol_file
# Optional custom move script
custom_script: dict[str, Any] | None = None
script_src = stage / "config" / "custom_move_script.json"
if not script_src.is_file():
alt_script = stage / "custom_move_script.json"
if alt_script.is_file():
script_src = alt_script
if script_src.is_file():
try:
custom_script = json.loads(script_src.read_text(encoding="utf-8"))
except json.JSONDecodeError as exc:
raise ValueError(f"Invalid custom_move_script.json: {exc}") from exc
if not isinstance(custom_script, dict) or not isinstance(custom_script.get("keyframes"), list):
raise ValueError("custom_move_script.json must contain a 'keyframes' list")
elif isinstance(manifest.get("custom_move"), dict) and manifest["custom_move"].get("slot") == "script":
raise ValueError("Manifest references custom_move script, but config/custom_move_script.json is missing")
# Optional hardware profile
target_profile_path = self.base_dir / "config" / "hardware_profile.json"
incoming_profile: dict[str, Any] | None = None
profile_src = stage / "config" / "hardware_profile.json"
if not profile_src.is_file():
alt_prof = stage / "hardware_profile.json"
if alt_prof.is_file():
profile_src = alt_prof
if (overwrite_profile or not target_profile_path.is_file()) and profile_src.is_file():
try:
incoming_profile = validate_hardware_profile(
json.loads(profile_src.read_text(encoding="utf-8")),
joint_names=manifest["joint_names"],
)
except json.JSONDecodeError as exc:
raise ValueError(f"Invalid hardware_profile.json: {exc}") from exc
# Pre-validate ONNX graph & metadata inside stage before touching live files
PolicySet.load_from_manifest(stage, manifest, backend=self.policy_backend)
# Backup existing config + policies for atomic rollback
live_config = self.base_dir / "config"
live_policies = self.base_dir / "policies"
live_config.mkdir(parents=True, exist_ok=True)
live_policies.mkdir(parents=True, exist_ok=True)
for sub in ("config", "policies"):
src_dir = self.base_dir / sub
if src_dir.is_dir():
shutil.copytree(src_dir, backup / sub, dirs_exist_ok=True)
try:
shutil.copy2(stage / "config" / "robot_manifest.json", live_config / "robot_manifest.json")
target_script = live_config / "custom_move_script.json"
if custom_script is not None:
target_script.write_text(json.dumps(custom_script, indent=2) + "\n", encoding="utf-8")
elif target_script.is_file():
target_script.unlink()
for _, src_pol in stage_policies.items():
rel_to_stage = src_pol.relative_to(stage)
dst_pol = self.base_dir / rel_to_stage
dst_pol.parent.mkdir(parents=True, exist_ok=True)
shutil.copy2(src_pol, dst_pol)
if incoming_profile is not None:
save_hardware_profile(target_profile_path, incoming_profile, joint_names=manifest["joint_names"])
live_policies_set = PolicySet.load_from_manifest(
self.base_dir, manifest, backend=self.policy_backend
)
self.controller.reload_runtime(
manifest=manifest,
policies=live_policies_set,
profile=incoming_profile,
custom_script=custom_script,
)
except Exception:
# Roll back live config + policies on reload failure
for sub in ("config", "policies"):
bak_dir = backup / sub
live_dir = self.base_dir / sub
if bak_dir.is_dir():
shutil.copytree(bak_dir, live_dir, dirs_exist_ok=True)
raise
deploy_meta = {
"ok": True,
"deployed_at_s": round(time.time(), 3),
"robot_id": manifest.get("robot_id", "microduck"),
"loaded_skills": sorted(live_policies_set.policies.keys()),
"profile_overwritten": incoming_profile is not None,
"custom_move": manifest.get("custom_move"),
}
(live_config / ".last_deploy.json").write_text(
json.dumps(deploy_meta, indent=2) + "\n", encoding="utf-8"
)
self.last_deploy = deploy_meta
return deploy_meta
+85
View File
@@ -0,0 +1,85 @@
from __future__ import annotations
from typing import Any
import numpy as np
def build_observation(
gyro: np.ndarray,
projected_gravity: np.ndarray,
joint_pos_rad: np.ndarray,
joint_vel_rad_s: np.ndarray,
last_action: np.ndarray,
command_13: np.ndarray,
default_pose: np.ndarray,
) -> np.ndarray:
"""Assemble the exact 61D float32 observation expected by Microduck ONNX policies.
Layout:
[0:3] base_ang_vel (gyro, rad/s)
[3:6] projected_gravity (trunk_base frame, unit vector pointing down: [0,0,-1] upright)
[6:20] joint_pos_rel (joint_pos_rad - default_pose)
[20:34] joint_vel (rad/s)
[34:48] last_action (previous raw actor output)
[48:61] command (13D: [vx, vy, wz, head(4), body(6)])
"""
obs = np.empty(61, dtype=np.float32)
obs[0:3] = np.asarray(gyro, dtype=np.float32)
obs[3:6] = np.asarray(projected_gravity, dtype=np.float32)
obs[6:20] = np.asarray(joint_pos_rad, dtype=np.float32) - np.asarray(default_pose, dtype=np.float32)
obs[20:34] = np.asarray(joint_vel_rad_s, dtype=np.float32)
obs[34:48] = np.asarray(last_action, dtype=np.float32)
obs[48:61] = np.asarray(command_13, dtype=np.float32)
if not np.all(np.isfinite(obs)):
raise ValueError("Observation vector contains non-finite values")
return obs
def decode_action(
raw_action: np.ndarray,
default_pose: np.ndarray,
action_scale: float = 1.0,
) -> np.ndarray:
act = np.asarray(raw_action, dtype=np.float32)
if act.shape != (14,) or not np.all(np.isfinite(act)):
raise ValueError("raw_action must be a finite 14D vector")
return np.asarray(default_pose, dtype=np.float32) + np.float32(action_scale) * act
def script_command_at(script: dict[str, Any], t_s: float, cmd_size: int = 13) -> np.ndarray:
"""Linearly interpolate a custom move_script.json timeline at time t_s."""
keyframes = script.get("keyframes", [])
out = np.zeros(cmd_size, dtype=np.float32)
if not keyframes:
return out
duration_s = float(script.get("durationS") or script.get("duration_s") or 0.0)
loop = script.get("loop", True) is not False
tt = float(t_s)
if loop and duration_s > 0.0:
tt = ((tt % duration_s) + duration_s) % duration_s
def _pad(raw_cmd: list[float]) -> np.ndarray:
arr = np.zeros(cmd_size, dtype=np.float32)
n = min(cmd_size, len(raw_cmd))
if n > 0:
arr[:n] = np.asarray(raw_cmd[:n], dtype=np.float32)
return arr
if tt <= float(keyframes[0]["t"]):
return _pad(keyframes[0]["cmd"])
for i in range(len(keyframes) - 1):
a = keyframes[i]
b = keyframes[i + 1]
ta = float(a["t"])
tb = float(b["t"])
if ta <= tt <= tb:
u = (tt - ta) / max(tb - ta, 1e-6)
ca = _pad(a["cmd"])
cb = _pad(b["cmd"])
return (1.0 - u) * ca + u * cb
return _pad(keyframes[-1]["cmd"])
+123
View File
@@ -0,0 +1,123 @@
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
from typing import Any
import numpy as np
@dataclass
class LoadedPolicy:
key: str
action_scale: float
backend: str
_session: Any | None = None
_input_name: str = "obs"
_output_name: str = "actions"
def infer(self, obs_61: np.ndarray) -> np.ndarray:
if obs_61.shape != (61,):
raise ValueError(f"Expected 61D observation, got {obs_61.shape}")
if self.backend == "dummy":
# Deterministic zero-action dummy policy for PC dry-run without onnxruntime.
return np.zeros(14, dtype=np.float32)
inp = np.asarray(obs_61, dtype=np.float32).reshape(1, 61)
out = self._session.run([self._output_name], {self._input_name: inp})[0]
act = np.asarray(out, dtype=np.float32).reshape(14)
if not np.all(np.isfinite(act)):
raise ValueError(f"Policy '{self.key}' returned non-finite action")
return act
class PolicySet:
def __init__(self, policies: dict[str, LoadedPolicy], backend: str) -> None:
self.policies = policies
self.backend = backend
@classmethod
def resolve_backend(cls, requested: str) -> str:
if requested in ("onnx", "dummy"):
return requested
if requested != "auto":
raise ValueError(f"Unknown policy backend: {requested}")
try:
import onnxruntime # noqa: F401
return "onnx"
except ImportError as exc:
raise RuntimeError(
"onnxruntime is not installed. Run 'bash scripts/install_deps.sh' on UNO Q, "
"or pass '--policy-backend dummy' for pre-hardware UI/protocol dry-run."
) from exc
@classmethod
def load_from_manifest(
cls,
base_dir: str | Path,
manifest: dict[str, Any],
backend: str = "auto",
) -> "PolicySet":
resolved_backend = cls.resolve_backend(backend)
ort = None
if resolved_backend == "onnx":
try:
import onnxruntime as ort_mod
ort = ort_mod
except ImportError as exc:
raise RuntimeError("onnxruntime is required for backend='onnx'") from exc
root = Path(base_dir)
default_scale = float(manifest.get("action_scale", 1.0))
skills = manifest.get("skills", {})
loaded: dict[str, LoadedPolicy] = {}
for key, spec in skills.items():
rel_path = spec.get("path")
if not rel_path:
continue
scale = float(spec.get("action_scale", default_scale))
if resolved_backend == "dummy":
loaded[key] = LoadedPolicy(key=key, action_scale=scale, backend="dummy")
continue
model_path = (root / rel_path).resolve()
if not model_path.is_file():
raise FileNotFoundError(f"Missing ONNX policy for '{key}': {model_path}")
sess_opts = ort.SessionOptions()
sess_opts.intra_op_num_threads = 2
sess = ort.InferenceSession(
str(model_path),
sess_options=sess_opts,
providers=["CPUExecutionProvider"],
)
inps = sess.get_inputs()
outs = sess.get_outputs()
if len(inps) != 1 or len(outs) != 1:
raise ValueError(f"Policy '{key}' must have 1 input / 1 output")
# Smoke validation mirroring custom-policy.js
smoke_obs = np.zeros((1, 61), dtype=np.float32)
smoke_out = sess.run([outs[0].name], {inps[0].name: smoke_obs})[0]
if tuple(np.asarray(smoke_out).shape) != (1, 14):
raise ValueError(
f"Policy '{key}' output shape {np.asarray(smoke_out).shape} != (1, 14)"
)
if not np.all(np.isfinite(smoke_out)):
raise ValueError(f"Policy '{key}' smoke output is non-finite")
loaded[key] = LoadedPolicy(
key=key,
action_scale=scale,
backend="onnx",
_session=sess,
_input_name=inps[0].name,
_output_name=outs[0].name,
)
if "walk" not in loaded or "stand" not in loaded:
raise ValueError("PolicySet must load at least 'walk' and 'stand'")
return cls(policies=loaded, backend=resolved_backend)
+561
View File
@@ -0,0 +1,561 @@
from __future__ import annotations
import math
from dataclasses import dataclass
from enum import Enum
from typing import Any
import numpy as np
from .observations import script_command_at
class LifecycleState(str, Enum):
IDLE = "IDLE"
SOFT_START = "SOFT_START"
ACTIVE = "ACTIVE"
CALIBRATION = "CALIBRATION"
FALL_SAFE = "FALL_SAFE"
@dataclass
class StepDecision:
lifecycle: LifecycleState
skill_mode: str
policy_key: str | None
torque_enabled: bool
soft_start_alpha: float
command_13: np.ndarray
zero_last_action: bool
mode_label: str
recovery_state: str | None
loco: str
head_mode: bool
class MicroduckStateMachine:
"""Hardware-safe 50Hz state machine mirroring app/src/robots/microduck/runtime.js."""
def __init__(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None:
self.manifest = manifest
self.custom_script = custom_script
self._load_params(manifest)
self.lifecycle: LifecycleState = LifecycleState.IDLE
self.loco: str = "legs" # "legs" | "rollers"
self.skill_mode: str = "walk" # "walk"|"sitstand"|"roll"|"kickL"|"kickR"|"crouch"|"groundpick"
self.sit_flag: int = 0
self.head_mode: bool = False
self.vel_cmd = np.zeros(3, dtype=np.float32) # [vx, 0, wz]
self.head_target = np.zeros(4, dtype=np.float32)
self.head_smooth = np.zeros(4, dtype=np.float32)
self.soft_start_q0 = np.zeros(14, dtype=np.float64)
self.soft_start_elapsed = 0.0
self.soft_start_duration = 1.5
self.sit_delay_rem: float | None = None
self.stand_delay_rem: float | None = None
self.kick_steps: int | None = None
self.post_kick_lock: int = 0
self.crouch_phase: float | None = None
self.pick_phase: float | None = None
self.roll_steps: int | None = None
self.roll_tipped: bool = False
self.script_t: float | None = None
self.fall_debounce: int = 0
self.recovery_state: str | None = None # None | "fallen" | "recovering"
self.recovery_steps: int = 0
self.recovery_upright_steps: int = 0
def _load_params(self, manifest: dict[str, Any]) -> None:
safety = manifest.get("safety", {})
head = manifest.get("head", {})
legacy = manifest.get("legacy", {})
variants = manifest.get("variants", {})
self.tilt_z = float(safety.get("tilt_z", -0.5))
self.upright_z = float(safety.get("upright_z", -0.85))
self.fall_debounce_steps = int(safety.get("fall_debounce_steps", 10))
self.fall_settle_steps = int(safety.get("fall_settle_steps", 15))
self.recover_upright_steps_req = int(safety.get("recover_upright_steps", 50))
self.recover_giveup_steps = int(safety.get("recover_giveup_steps", 300))
self.kick_total_steps = int(safety.get("kick_steps", 25))
self.post_kick_lock_total = int(safety.get("post_kick_lock_steps", 20))
self.roll_max_steps = int(safety.get("roll_max_steps", 150))
self.sit_delay_s = float(safety.get("sit_delay_s", 0.8))
self.stand_delay_s = float(safety.get("stand_delay_s", 2.0))
self.head_max = float(head.get("max_rad", 2.5))
self.head_alpha = float(head.get("alpha", 0.2))
self.head_signs = np.asarray(head.get("signs", [1, -1, 1, -1]), dtype=np.float32)
self.crouch_period_s = float(legacy.get("crouch_period", 5.0))
self.crouch_end_phase = float(legacy.get("crouch_end", 0.7))
self.groundpick_period_s = float(legacy.get("groundpick_period", 4.0))
self.groundpick_end_phase = float(legacy.get("groundpick_end", 0.7))
self.legs_limits = list(variants.get("legs", {}).get("limits", [0.25, -0.2, 1.0]))
self.rollers_limits = list(variants.get("rollers", {}).get("limits", [0.6, -0.5, 0.3]))
self.rollers_enabled = bool(variants.get("rollers", {}).get("enabled", False))
custom_move = manifest.get("custom_move")
self.custom_slot = custom_move.get("slot") if isinstance(custom_move, dict) else None
roll_skill = manifest.get("skills", {}).get("roll", {})
ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02))
if self.custom_slot == "trick" and roll_skill.get("duration_s"):
self.roll_max_steps = max(10, round(float(roll_skill["duration_s"]) / ctrl_dt))
def update_manifest(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None:
self.manifest = manifest
self.custom_script = custom_script
self._load_params(manifest)
if not self.rollers_enabled and self.loco == "rollers":
self.loco = "legs"
def _clear_skill_runs(self) -> None:
self.sit_delay_rem = None
self.stand_delay_rem = None
self.kick_steps = None
self.post_kick_lock = 0
self.crouch_phase = None
self.pick_phase = None
self.roll_steps = None
self.roll_tipped = False
self.script_t = None
self.fall_debounce = 0
self.recovery_state = None
self.recovery_steps = 0
self.recovery_upright_steps = 0
def emergency_stop(self) -> None:
self._clear_skill_runs()
self.skill_mode = "walk"
self.sit_flag = 0
self.head_mode = False
self.vel_cmd.fill(0)
self.head_target.fill(0)
self.head_smooth.fill(0)
self.lifecycle = LifecycleState.IDLE
def enter_calibration(self) -> None:
self.emergency_stop()
self.lifecycle = LifecycleState.CALIBRATION
def exit_calibration(self) -> None:
if self.lifecycle == LifecycleState.CALIBRATION:
self.lifecycle = LifecycleState.IDLE
def request_stand(self, current_q_rad: np.ndarray, soft_start_s: float = 1.5) -> None:
self._clear_skill_runs()
self.skill_mode = "walk"
self.sit_flag = 0
self.head_mode = False
self.vel_cmd.fill(0)
self.head_target.fill(0)
self.head_smooth.fill(0)
self.soft_start_q0 = np.asarray(current_q_rad, dtype=np.float64).copy()
self.soft_start_elapsed = 0.0
self.soft_start_duration = max(0.02, float(soft_start_s))
self.lifecycle = LifecycleState.SOFT_START
def set_velocity_cmd(self, norm_x: float, norm_wz: float, speed_scale: float = 1.0) -> None:
lim_f, lim_b, lim_a = self.rollers_limits if self.loco == "rollers" else self.legs_limits
nx = max(-1.0, min(1.0, float(norm_x)))
nwz = max(-1.0, min(1.0, float(norm_wz)))
scale = max(0.05, min(1.0, float(speed_scale)))
vx = (nx * lim_f if nx >= 0.0 else nx * (-lim_b)) * scale
wz = nwz * lim_a * scale
self.vel_cmd[0] = vx
self.vel_cmd[1] = 0.0
self.vel_cmd[2] = wz
def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None:
raw = np.array([neck_pitch, head_pitch, head_yaw, head_roll], dtype=np.float32)
raw = np.clip(raw, -1.0, 1.0)
self.head_target = self.head_signs * raw * self.head_max
def _is_one_shot_busy(self) -> bool:
return (
(self.skill_mode == "roll" and self.roll_steps is not None)
or (self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None)
or (self.skill_mode == "crouch" and self.crouch_phase is not None)
or (self.skill_mode == "groundpick" and self.pick_phase is not None)
)
def trigger_action(self, action: str) -> bool:
if self.lifecycle != LifecycleState.ACTIVE or self.recovery_state is not None:
return False
if action == "headToggle":
if self.head_mode:
self.head_mode = False
return True
if (
self.skill_mode in ("walk", "sitstand")
and self.post_kick_lock == 0
and self.stand_delay_rem is None
):
self.head_mode = True
return True
return False
if action == "locoToggle":
if not self.rollers_enabled:
return False
if self._is_one_shot_busy() or self.stand_delay_rem is not None:
return False
self._clear_skill_runs()
self.skill_mode = "walk"
self.sit_flag = 0
self.head_mode = False
self.loco = "rollers" if self.loco == "legs" else "legs"
return True
if action == "sitToggle":
if self.loco != "legs":
return self.trigger_action("crouch")
if self.custom_slot == "trick":
return self.trigger_action("roll")
if self.custom_slot == "script" and self.custom_script is not None:
return self.trigger_action("scriptToggle")
if self._is_one_shot_busy():
return False
self.head_mode = False
if self.skill_mode == "sitstand" and self.sit_flag == 1:
self.sit_flag = 0
self.sit_delay_rem = None
self.stand_delay_rem = self.stand_delay_s
return True
if self.skill_mode == "walk" and self.stand_delay_rem is None:
self.skill_mode = "sitstand"
self.sit_flag = 0
self.sit_delay_rem = self.sit_delay_s
return True
return False
if action == "scriptToggle":
if self.custom_script is None or self.loco != "legs":
return False
if self.script_t is not None:
self.script_t = None
return True
if self.skill_mode == "walk" and self.stand_delay_rem is None:
self.head_mode = False
self.script_t = 0.0
return True
return False
if action in ("kickL", "kickR"):
if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None:
return False
self.head_mode = False
self.sit_delay_rem = None
self.skill_mode = action
self.sit_flag = 0
self.kick_steps = 0
return True
if action == "groundPick":
if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None:
return False
self.head_mode = False
self.sit_delay_rem = None
self.skill_mode = "groundpick"
self.sit_flag = 0
self.pick_phase = 0.0
return True
if action == "roll":
if self.loco == "rollers":
return self.trigger_action("crouch")
if self.skill_mode != "walk" or self.stand_delay_rem is not None:
return False
self.head_mode = False
self.sit_delay_rem = None
self.skill_mode = "roll"
self.sit_flag = 0
self.roll_steps = 0
self.roll_tipped = False
return True
if action == "crouch":
if self.loco != "rollers" or self.skill_mode != "walk":
return False
self.head_mode = False
self.skill_mode = "crouch"
self.crouch_phase = 0.0
return True
return False
def _active_policy_key(self) -> str | None:
if self.recovery_state == "fallen":
return None
if self.recovery_state == "recovering":
return "stand"
if self.loco == "rollers" and self.skill_mode == "walk":
return "drive"
return self.skill_mode
def _mode_label(self) -> str:
if self.lifecycle == LifecycleState.IDLE:
return "Idle (Torque Off)"
if self.lifecycle == LifecycleState.CALIBRATION:
return "Calibration"
if self.lifecycle == LifecycleState.FALL_SAFE:
return "Fall Safe"
if self.lifecycle == LifecycleState.SOFT_START:
return "Soft Start"
if self.recovery_state:
return "Recovery"
if self.skill_mode == "roll":
return "Trick" if self.custom_slot == "trick" else "Roll"
if self.script_t is not None:
return "Script"
if self.skill_mode == "crouch":
return "Crouch"
if self.skill_mode == "groundpick":
return "Pick"
if self.skill_mode in ("kickL", "kickR"):
return "Kick"
if self.head_mode:
return "Head"
if self.skill_mode == "sitstand" and self.sit_flag == 1:
return "Sit"
return "Drive" if self.loco == "rollers" else "Run"
def _build_command_13(self) -> np.ndarray:
cmd = np.zeros(13, dtype=np.float32)
if self.skill_mode == "sitstand":
cmd[0] = float(self.sit_flag)
elif self.skill_mode == "crouch" and self.crouch_phase is not None:
a = 2.0 * math.pi * self.crouch_phase
cmd[0] = math.cos(a)
cmd[1] = math.sin(a)
elif self.skill_mode == "groundpick" and self.pick_phase is not None:
a = 2.0 * math.pi * self.pick_phase
cmd[0] = math.cos(a)
cmd[1] = math.sin(a)
elif not (
self.head_mode
or self.skill_mode == "roll"
or self.skill_mode in ("kickL", "kickR")
or self.post_kick_lock > 0
or self.recovery_state is not None
):
cmd[0:3] = self.vel_cmd
for h in range(4):
self.head_smooth[h] += self.head_alpha * (self.head_target[h] - self.head_smooth[h])
gp_zero = self.skill_mode == "groundpick" or self.recovery_state is not None
if not gp_zero:
cmd[3:7] = self.head_smooth
if (
self.script_t is not None
and self.skill_mode == "walk"
and self.recovery_state is None
and self.custom_script is not None
):
cmd = script_command_at(self.custom_script, self.script_t, 13)
return cmd
def step(
self,
dt: float,
proj_grav_z: float,
auto_stand_on_fall: bool = False,
) -> StepDecision:
zero_last_action = False
if self.lifecycle in (LifecycleState.IDLE, LifecycleState.CALIBRATION, LifecycleState.FALL_SAFE):
return StepDecision(
lifecycle=self.lifecycle,
skill_mode=self.skill_mode,
policy_key=None,
torque_enabled=False,
soft_start_alpha=0.0,
command_13=np.zeros(13, dtype=np.float32),
zero_last_action=False,
mode_label=self._mode_label(),
recovery_state=self.recovery_state,
loco=self.loco,
head_mode=self.head_mode,
)
if self.lifecycle == LifecycleState.SOFT_START:
self.soft_start_elapsed += dt
u = min(1.0, self.soft_start_elapsed / self.soft_start_duration)
alpha = 0.5 * (1.0 - math.cos(math.pi * u))
if u < 1.0 - 1e-6:
return StepDecision(
lifecycle=self.lifecycle,
skill_mode="soft_start",
policy_key=None,
torque_enabled=True,
soft_start_alpha=alpha,
command_13=np.zeros(13, dtype=np.float32),
zero_last_action=True,
mode_label=self._mode_label(),
recovery_state=None,
loco=self.loco,
head_mode=False,
)
self.lifecycle = LifecycleState.ACTIVE
self.skill_mode = "walk"
zero_last_action = True
# Active lifecycle: advance timers before fall/recovery and command build
if self.sit_delay_rem is not None:
self.sit_delay_rem -= dt
if self.sit_delay_rem <= 0.0:
self.sit_delay_rem = None
if self.skill_mode == "sitstand":
self.sit_flag = 1
if self.stand_delay_rem is not None:
self.stand_delay_rem -= dt
if self.stand_delay_rem <= 0.0:
self.stand_delay_rem = None
self.skill_mode = "walk"
zero_last_action = True
# Fall detection & recovery state machine
fallen_now = (not math.isfinite(proj_grav_z)) or (proj_grav_z > self.tilt_z)
if self.recovery_state == "fallen":
self.recovery_steps += 1
if self.recovery_steps >= self.fall_settle_steps:
self.recovery_state = "recovering"
self.recovery_steps = 0
self.recovery_upright_steps = 0
zero_last_action = True
elif self.recovery_state == "recovering":
self.recovery_steps += 1
if proj_grav_z < self.upright_z:
self.recovery_upright_steps += 1
else:
self.recovery_upright_steps = 0
if self.recovery_upright_steps >= self.recover_upright_steps_req:
self.recovery_state = None
self.recovery_steps = 0
self.recovery_upright_steps = 0
self.skill_mode = "walk"
zero_last_action = True
elif self.recovery_steps >= self.recover_giveup_steps:
self._clear_skill_runs()
self.lifecycle = LifecycleState.FALL_SAFE
return StepDecision(
lifecycle=self.lifecycle,
skill_mode="walk",
policy_key=None,
torque_enabled=False,
soft_start_alpha=0.0,
command_13=np.zeros(13, dtype=np.float32),
zero_last_action=True,
mode_label=self._mode_label(),
recovery_state=None,
loco=self.loco,
head_mode=False,
)
elif fallen_now:
recoverable = (
auto_stand_on_fall
and self.loco == "legs"
and self.skill_mode == "walk"
and self.post_kick_lock == 0
and self.stand_delay_rem is None
)
self.fall_debounce += 1
if self.fall_debounce >= self.fall_debounce_steps:
self.fall_debounce = 0
self.head_mode = False
if recoverable:
self.recovery_state = "fallen"
self.recovery_steps = 0
else:
self._clear_skill_runs()
self.lifecycle = LifecycleState.FALL_SAFE
return StepDecision(
lifecycle=self.lifecycle,
skill_mode="walk",
policy_key=None,
torque_enabled=False,
soft_start_alpha=0.0,
command_13=np.zeros(13, dtype=np.float32),
zero_last_action=True,
mode_label=self._mode_label(),
recovery_state=None,
loco=self.loco,
head_mode=False,
)
else:
self.fall_debounce = 0
policy_key = self._active_policy_key()
command_13 = self._build_command_13()
# Advance step-counted skills AFTER choosing this tick's policy/command
if self.post_kick_lock > 0 and self.skill_mode == "walk":
self.post_kick_lock -= 1
if self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None:
self.kick_steps += 1
if self.kick_steps >= self.kick_total_steps:
self.kick_steps = None
self.skill_mode = "walk"
self.post_kick_lock = self.post_kick_lock_total
if self.skill_mode == "crouch" and self.crouch_phase is not None:
self.crouch_phase += dt / self.crouch_period_s
if self.crouch_phase >= self.crouch_end_phase:
self.crouch_phase = None
self.skill_mode = "walk"
if self.skill_mode == "groundpick" and self.pick_phase is not None:
self.pick_phase += dt / self.groundpick_period_s
if self.pick_phase >= self.groundpick_end_phase:
self.pick_phase = None
self.skill_mode = "walk"
if self.skill_mode == "roll" and self.roll_steps is not None:
self.roll_steps += 1
if proj_grav_z > -0.3:
self.roll_tipped = True
upright = proj_grav_z < self.upright_z
done = self.roll_tipped and upright and self.roll_steps >= 40
expired = self.roll_steps >= self.roll_max_steps
if done or expired:
self.roll_steps = None
self.roll_tipped = False
self.skill_mode = "walk"
zero_last_action = True
if not upright:
self.lifecycle = LifecycleState.FALL_SAFE
if self.script_t is not None:
self.script_t += dt
if self.custom_script is None:
self.script_t = None
else:
dur = float(self.custom_script.get("durationS") or self.custom_script.get("duration_s") or 0.0)
loop = self.custom_script.get("loop", True) is not False
if not loop and self.script_t >= dur:
self.script_t = None
return StepDecision(
lifecycle=self.lifecycle,
skill_mode=self.skill_mode,
policy_key=policy_key,
torque_enabled=self.lifecycle == LifecycleState.ACTIVE,
soft_start_alpha=1.0,
command_13=command_13,
zero_last_action=zero_last_action,
mode_label=self._mode_label(),
recovery_state=self.recovery_state,
loco=self.loco,
head_mode=self.head_mode,
)
+1
View File
@@ -0,0 +1 @@
"""Hardware abstraction layers for Microduck on Arduino UNO Q."""
+57
View File
@@ -0,0 +1,57 @@
from __future__ import annotations
from abc import ABC, abstractmethod
from dataclasses import dataclass, field
from typing import Any
@dataclass
class HardwareFrame:
gyro_raw: list[float]
proj_gravity_raw: list[float]
ticks: list[int]
raw_speeds: list[int]
voltage_v: float = 7.4
max_temp_c: float = 35.0
imu_ok: bool = True
servo_err_mask: int = 0
timestamp_s: float = 0.0
extra: dict[str, Any] = field(default_factory=dict)
class HardwareBridge(ABC):
name: str = "abstract"
@abstractmethod
def connect(self) -> None:
"""Initialize connection to the MCU / servo bus."""
@abstractmethod
def close(self) -> None:
"""Release hardware resources safely."""
@abstractmethod
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
"""Push servo IDs, I2C address, watchdog, and IMU bias config to HAL."""
@abstractmethod
def sync_step(
self,
enable_torque: bool,
target_ticks: list[int],
speed_limit: int = 0,
acc: int = 50,
) -> HardwareFrame:
"""Send target joint ticks and read fresh IMU + servo feedback."""
@abstractmethod
def set_torque(self, enable: bool) -> None:
"""Immediately enable or disable torque on all configured servos."""
@abstractmethod
def ping_servos(self, max_id: int = 20) -> list[int]:
"""Scan and return responding servo IDs."""
@abstractmethod
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
"""Collect stationary gyro readings (rad/s in sensor frame) and return mean bias."""
+82
View File
@@ -0,0 +1,82 @@
from __future__ import annotations
import math
import time
from typing import Any
from .base import HardwareBridge, HardwareFrame
class MockHal(HardwareBridge):
"""PC-runnable hardware simulator for pre-hardware verification."""
name = "mock"
def __init__(self, initial_tick: int = 2048) -> None:
self.connected = False
self.torque_enabled = False
self.servo_ids = list(range(1, 15))
self.ticks = [int(initial_tick)] * 14
self.raw_speeds = [0] * 14
self._gyro = [0.0, 0.0, 0.0]
self._gravity = [0.0, 0.0, -1.0]
self._alpha = 0.45
def connect(self) -> None:
self.connected = True
def close(self) -> None:
self.torque_enabled = False
self.connected = False
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
joints = profile.get("joints", {})
self.servo_ids = [int(joints[n]["id"]) for n in joint_names]
def set_mock_gravity(self, grav: list[float]) -> None:
gx, gy, gz = float(grav[0]), float(grav[1]), float(grav[2])
norm = math.sqrt(gx * gx + gy * gy + gz * gz) or 1.0
self._gravity = [gx / norm, gy / norm, gz / norm]
def set_mock_gyro(self, gyro: list[float]) -> None:
self._gyro = [float(gyro[0]), float(gyro[1]), float(gyro[2])]
def sync_step(
self,
enable_torque: bool,
target_ticks: list[int],
speed_limit: int = 0,
acc: int = 50,
) -> HardwareFrame:
self.torque_enabled = bool(enable_torque)
if self.torque_enabled:
for i in range(14):
prev = self.ticks[i]
nxt = int(round(prev + self._alpha * (int(target_ticks[i]) - prev)))
delta = nxt - prev
self.ticks[i] = max(0, min(4095, nxt))
mag = min(0x7FFF, abs(delta) * 50)
self.raw_speeds[i] = (mag | 0x8000) if delta < 0 else mag
else:
self.raw_speeds = [0] * 14
return HardwareFrame(
gyro_raw=list(self._gyro),
proj_gravity_raw=list(self._gravity),
ticks=list(self.ticks),
raw_speeds=list(self.raw_speeds),
voltage_v=7.8,
max_temp_c=34.0,
imu_ok=True,
servo_err_mask=0,
timestamp_s=time.monotonic(),
)
def set_torque(self, enable: bool) -> None:
self.torque_enabled = bool(enable)
def ping_servos(self, max_id: int = 20) -> list[int]:
return [sid for sid in self.servo_ids if 0 <= sid <= max_id]
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
return list(self._gyro)
+109
View File
@@ -0,0 +1,109 @@
from __future__ import annotations
import time
from typing import Any
from .base import HardwareBridge, HardwareFrame
class RouterBridgeHal(HardwareBridge):
"""Arduino UNO Q HAL communicating with STM32U585 over Arduino_RouterBridge."""
name = "router"
def __init__(self) -> None:
self._bridge = None
self._servo_ids = list(range(1, 15))
def connect(self) -> None:
try:
from arduino.app_utils import Bridge # type: ignore
except ImportError as exc:
raise RuntimeError(
"arduino.app_utils.Bridge is not available. Run on Arduino UNO Q via App Lab / "
"arduino-app-cli, or pass '--hal mock' for PC pre-hardware debugging."
) from exc
self._bridge = Bridge
def close(self) -> None:
if self._bridge is not None:
try:
self.set_torque(False)
except Exception:
pass
self._bridge = None
def _require_bridge(self) -> Any:
if self._bridge is None:
raise RuntimeError("RouterBridgeHal is not connected")
return self._bridge
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
bridge = self._require_bridge()
joints = profile["joints"]
servo = profile["servo"]
imu = profile["imu"]
safety = profile.get("safety", {})
self._servo_ids = [int(joints[name]["id"]) for name in joint_names]
addr_raw = imu.get("i2c_addr", "0x6A")
i2c_addr = int(addr_raw, 16) if isinstance(addr_raw, str) else int(addr_raw)
bias = [float(x) for x in imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0])]
payload: list[float] = [
float(servo.get("baudrate", 1000000)),
float(i2c_addr),
float(safety.get("watchdog_ms", 150)),
bias[0],
bias[1],
bias[2],
*[float(sid) for sid in self._servo_ids],
]
bridge.call("hw_configure", payload)
def sync_step(
self,
enable_torque: bool,
target_ticks: list[int],
speed_limit: int = 0,
acc: int = 50,
) -> HardwareFrame:
bridge = self._require_bridge()
if len(target_ticks) != 14:
raise ValueError("Expected 14 target_ticks")
cmd_frame = [
1 if enable_torque else 0,
int(speed_limit),
int(acc),
*[int(t) for t in target_ticks],
]
res = bridge.call("hw_sync_step", cmd_frame)
if not isinstance(res, (list, tuple)) or len(res) < 38:
raise RuntimeError(f"Invalid hw_sync_step response from MCU: {res!r}")
return HardwareFrame(
gyro_raw=[float(res[0]), float(res[1]), float(res[2])],
proj_gravity_raw=[float(res[3]), float(res[4]), float(res[5])],
ticks=[int(round(res[6 + i])) for i in range(14)],
raw_speeds=[int(round(res[20 + i])) for i in range(14)],
voltage_v=float(res[34]),
max_temp_c=float(res[35]),
imu_ok=bool(int(round(res[36]))),
servo_err_mask=int(round(res[37])),
timestamp_s=time.monotonic(),
)
def set_torque(self, enable: bool) -> None:
bridge = self._require_bridge()
bridge.call("hw_set_torque", 1 if enable else 0)
def ping_servos(self, max_id: int = 20) -> list[int]:
bridge = self._require_bridge()
res = bridge.call("hw_ping_servos", int(max_id))
return [int(x) for x in res] if isinstance(res, (list, tuple)) else []
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
bridge = self._require_bridge()
res = bridge.call("hw_calibrate_imu", int(samples))
if not isinstance(res, (list, tuple)) or len(res) != 3:
raise RuntimeError(f"Invalid hw_calibrate_imu response: {res!r}")
return [float(res[0]), float(res[1]), float(res[2])]
+125
View File
@@ -0,0 +1,125 @@
from __future__ import annotations
import argparse
import os
import signal
import sys
from pathlib import Path
PYTHON_ROOT = Path(__file__).resolve().parent
UNOQ_ROOT = PYTHON_ROOT.parent
if str(PYTHON_ROOT) not in sys.path:
sys.path.insert(0, str(PYTHON_ROOT))
from core.config import load_hardware_profile, load_robot_manifest # noqa: E402
from core.controller import MicroduckController # noqa: E402
from core.deploy_service import DeployService # noqa: E402
from core.policy_runner import PolicySet # noqa: E402
from hal.base import HardwareBridge # noqa: E402
from hal.mock_hal import MockHal # noqa: E402
from hal.router_hal import RouterBridgeHal # noqa: E402
from web.fallback_server import FallbackWebServer # noqa: E402
def _select_hal(hal_mode: str) -> HardwareBridge:
if hal_mode == "mock":
return MockHal()
if hal_mode == "router":
return RouterBridgeHal()
# auto: prefer RouterBridge when running on UNO Q App Lab, otherwise ask explicitly for --hal mock
try:
from arduino.app_utils import Bridge # type: ignore # noqa: F401
return RouterBridgeHal()
except ImportError as exc:
raise RuntimeError(
"arduino.app_utils is not available in this Python environment. "
"Launch inside UNO Q App Lab, or pass '--hal mock' for PC pre-hardware testing."
) from exc
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description="Microduck UNO Q 50Hz Controller & Deploy Server")
parser.add_argument("--base-dir", type=Path, default=UNOQ_ROOT, help="UNO Q config/policies directory")
parser.add_argument(
"--hal",
choices=("auto", "router", "mock"),
default=os.environ.get("MICRODUCK_HAL", "auto"),
help="Hardware backend: 'router' (UNO Q STM32), 'mock' (PC simulator), or 'auto'",
)
parser.add_argument(
"--policy-backend",
choices=("auto", "onnx", "dummy"),
default=os.environ.get("MICRODUCK_POLICY_BACKEND", "auto"),
help="Inference backend: 'onnx' / 'auto', or explicit 'dummy' dry-run",
)
parser.add_argument("--host", default="0.0.0.0")
parser.add_argument("--port", type=int, default=8000)
args = parser.parse_args(argv)
base_dir = Path(args.base_dir).resolve()
manifest = load_robot_manifest(base_dir / "config" / "robot_manifest.json")
profile = load_hardware_profile(base_dir / "config" / "hardware_profile.json", joint_names=manifest["joint_names"])
custom_script = None
script_path = base_dir / "config" / "custom_move_script.json"
if script_path.is_file():
import json
custom_script = json.loads(script_path.read_text(encoding="utf-8"))
hal = _select_hal(args.hal)
boot_error: str | None = None
try:
policies = PolicySet.load_from_manifest(base_dir, manifest, backend=args.policy_backend)
except FileNotFoundError as exc:
# Allow web server & /api/deploy to boot on a fresh board before first push from website
boot_error = f"Awaiting first policy deploy: {exc}"
policies = PolicySet({}, backend="awaiting_deploy")
controller = MicroduckController(
base_dir=base_dir,
manifest=manifest,
profile=profile,
hal=hal,
policies=policies,
custom_script=custom_script,
)
if boot_error:
controller._last_error = boot_error
deploy_backend = args.policy_backend
deploy_service = DeployService(base_dir=base_dir, controller=controller, policy_backend=deploy_backend)
controller.start()
def _shutdown(*_args: object) -> None:
raise SystemExit(0)
signal.signal(signal.SIGTERM, _shutdown)
srv: FallbackWebServer | None = None
try:
try:
import uvicorn # type: ignore
from web.server import create_fastapi_app
app = create_fastapi_app(controller, deploy_service)
uvicorn.run(app, host=args.host, port=args.port, log_level="info")
except ImportError:
srv = FallbackWebServer(args.host, args.port, controller, deploy_service)
print(f"[microduck-unoq] FastAPI/uvicorn not installed; serving stdlib HTTP/WS on {args.host}:{srv.port}")
srv.serve_forever()
except (KeyboardInterrupt, SystemExit):
pass
finally:
if srv is not None:
srv.server_close()
controller.stop()
return 0
if __name__ == "__main__":
raise SystemExit(main())
+5
View File
@@ -0,0 +1,5 @@
numpy>=1.26.0
onnxruntime>=1.18.0
fastapi>=0.115.0
uvicorn[standard]>=0.30.0
python-multipart>=0.0.9
@@ -0,0 +1,134 @@
import sys
from pathlib import Path
import numpy as np
import pytest
PYTHON_ROOT = Path(__file__).resolve().parents[1]
UNOQ_ROOT = PYTHON_ROOT.parent
if str(PYTHON_ROOT) not in sys.path:
sys.path.insert(0, str(PYTHON_ROOT))
from core.config import load_hardware_profile, load_robot_manifest # noqa: E402
from core.controller import MicroduckController # noqa: E402
from core.policy_runner import PolicySet # noqa: E402
from core.state_machine import LifecycleState # noqa: E402
from hal.mock_hal import MockHal # noqa: E402
def _make_controller(tmp_path: Path, auto_stand_on_fall: bool = False):
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json")
profile["safety"]["auto_stand_on_fall"] = auto_stand_on_fall
profile["safety"]["soft_start_s"] = 0.04 # 2 steps at 50Hz for fast test
hal = MockHal()
policies = PolicySet.load_from_manifest(UNOQ_ROOT, manifest, backend="dummy")
ctrl = MicroduckController(
base_dir=UNOQ_ROOT,
manifest=manifest,
profile=profile,
hal=hal,
policies=policies,
profile_path=tmp_path / "hardware_profile.json",
)
return ctrl, hal
def test_controller_stand_walk_estop_and_calibration_with_mock_hal(tmp_path: Path):
ctrl, hal = _make_controller(tmp_path, auto_stand_on_fall=False)
ctrl.connect()
# Initially idle, torque off
st = ctrl.step_once(dt=0.02)
assert st["lifecycle"] == LifecycleState.IDLE.value
assert st["torque_enabled"] is False
# Prepare stand -> soft start -> active walk
ctrl.request_stand()
ctrl.step_once(dt=0.02)
assert ctrl.state_machine.lifecycle == LifecycleState.SOFT_START
st = ctrl.step_once(dt=0.02)
assert st["lifecycle"] == LifecycleState.ACTIVE
assert st["torque_enabled"] is True
# Remote control timeout > 0.5s zeroes velocity command
ctrl.set_remote_input(0.8, 0.4, now_s=10.0)
ctrl.step_once(dt=0.02, now_s=10.1)
assert ctrl.state_machine.vel_cmd[0] > 0.0
ctrl.step_once(dt=0.02, now_s=10.7)
assert np.allclose(ctrl.state_machine.vel_cmd, 0.0)
# E-STOP disables torque immediately
ctrl.emergency_stop()
st = ctrl.step_once(dt=0.02)
assert st["lifecycle"] == LifecycleState.IDLE.value
assert st["torque_enabled"] is False
assert hal.torque_enabled is False
def test_controller_fall_protection_and_auto_stand_recovery(tmp_path: Path):
# Case 1: default auto_stand_on_fall=False -> FALL_SAFE torque off
ctrl, hal = _make_controller(tmp_path / "c1", auto_stand_on_fall=False)
ctrl.connect()
ctrl.request_stand()
ctrl.step_once(dt=0.02)
ctrl.step_once(dt=0.02)
assert ctrl.state_machine.lifecycle == LifecycleState.ACTIVE
# sync_step samples the updated mock gravity on the 1st tick, then 10 debounce ticks follow
hal.set_mock_gravity([0.9, 0.0, -0.1])
ctrl.step_once(dt=0.02)
for _ in range(10):
st = ctrl.step_once(dt=0.02)
assert st["lifecycle"] == LifecycleState.FALL_SAFE.value
assert st["torque_enabled"] is False
# Case 2: auto_stand_on_fall=True -> enters recovery using stand policy
ctrl2, hal2 = _make_controller(tmp_path / "c2", auto_stand_on_fall=True)
ctrl2.connect()
ctrl2.request_stand()
ctrl2.step_once(dt=0.02)
ctrl2.step_once(dt=0.02)
hal2.set_mock_gravity([0.9, 0.0, -0.1])
ctrl2.step_once(dt=0.02)
for _ in range(10):
ctrl2.step_once(dt=0.02)
assert ctrl2.state_machine.recovery_state == "fallen"
for _ in range(15):
st = ctrl2.step_once(dt=0.02)
assert st["recovery_state"] == "recovering"
assert st["active_policy"] == "stand"
# Upright restored -> 1 step to sample upright + 50 steps to return to walk
hal2.set_mock_gravity([0.0, 0.0, -1.0])
ctrl2.step_once(dt=0.02)
for _ in range(50):
st = ctrl2.step_once(dt=0.02)
assert st["recovery_state"] is None
assert st["active_policy"] == "walk"
def test_calibration_zero_capture_and_imu_bias(tmp_path: Path):
ctrl, hal = _make_controller(tmp_path)
ctrl.connect()
# Simulate joint 0 sitting at tick 2100 instead of 2048
hal.ticks[0] = 2100
ctrl.step_once(dt=0.02)
updated = ctrl.capture_joint_zero("left_hip_yaw")
assert updated["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 2100 - 2048
hal.set_mock_gyro([0.03, -0.02, 0.01])
imu_res = ctrl.calibrate_imu_zero(samples=10)
assert np.allclose(imu_res["gyro_bias_rad_s"], [0.03, -0.02, 0.01], atol=1e-5)
# Ping servos
ids = ctrl.ping_servos()
assert len(ids) == 14
def test_policy_set_onnx_requires_explicit_dummy_when_missing():
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
with pytest.raises((FileNotFoundError, RuntimeError)):
PolicySet.load_from_manifest(UNOQ_ROOT, manifest, backend="onnx")
+216
View File
@@ -0,0 +1,216 @@
import json
import math
import sys
from pathlib import Path
import numpy as np
import pytest
PYTHON_ROOT = Path(__file__).resolve().parents[1]
UNOQ_ROOT = PYTHON_ROOT.parent
if str(PYTHON_ROOT) not in sys.path:
sys.path.insert(0, str(PYTHON_ROOT))
from core.config import ( # noqa: E402
apply_imu_calibration,
load_hardware_profile,
load_robot_manifest,
rad_to_ticks,
ticks_to_rad,
)
from core.observations import ( # noqa: E402
build_observation,
decode_action,
script_command_at,
)
from core.state_machine import LifecycleState, MicroduckStateMachine # noqa: E402
def test_default_manifest_and_profile_match_microduck_contract():
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json")
assert manifest["obs_size"] == 61
assert manifest["action_size"] == 14
assert manifest["command_size"] == 13
assert len(manifest["joint_names"]) == 14
assert len(manifest["default_pose"]) == 14
assert set(manifest["joint_names"]) == set(profile["joints"].keys())
assert manifest["skills"]["walk"]["path"] == "policies/walk.onnx"
assert manifest["skills"]["stand"]["path"] == "policies/stand.onnx"
def test_rad_to_ticks_and_back_with_sign_and_offset():
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json")
names = manifest["joint_names"]
# Customize two joints with inverted direction and mechanical offset
profile["joints"]["left_hip_pitch"]["sign"] = -1
profile["joints"]["left_hip_pitch"]["zero_offset_ticks"] = 35
profile["joints"]["right_knee"]["sign"] = 1
profile["joints"]["right_knee"]["zero_offset_ticks"] = -20
q_in = np.array(manifest["default_pose"], dtype=np.float64)
ticks = rad_to_ticks(q_in, names, profile)
assert len(ticks) == 14
assert all(0 <= t <= 4095 for t in ticks)
q_out = ticks_to_rad(ticks, names, profile)
# 12-bit quantization resolution ~ 2*pi/4096 = 0.001534 rad -> max error <= 0.001 rad
assert np.allclose(q_out, q_in, atol=1e-3)
def test_rad_to_ticks_clamps_to_joint_mechanical_limits():
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json")
names = manifest["joint_names"]
q_extreme = np.full(14, 10.0, dtype=np.float64)
ticks = rad_to_ticks(q_extreme, names, profile)
q_clamped = ticks_to_rad(ticks, names, profile)
for i, name in enumerate(names):
assert q_clamped[i] <= profile["joints"][name]["max_rad"] + 2e-3
def test_imu_calibration_axis_remap_and_gyro_bias():
profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json")
profile["imu"]["axis_map"] = [
[0, 1, 0],
[-1, 0, 0],
[0, 0, 1],
]
profile["imu"]["gyro_bias_rad_s"] = [0.1, -0.05, 0.02]
gyro, grav = apply_imu_calibration(
np.array([0.2, 0.35, -0.08]),
np.array([0.0, 0.0, -1.0]),
profile,
)
# First subtract bias [0.1, 0.4, -0.1], then rotate via axis_map -> [0.4, -0.1, -0.1]
assert np.allclose(gyro, [0.4, -0.1, -0.1], atol=1e-6)
assert np.allclose(grav, [0.0, 0.0, -1.0], atol=1e-6)
def test_observation_layout_and_action_decoding():
default_pose = np.arange(14, dtype=np.float32) * 0.01
gyro = np.array([0.1, 0.2, 0.3], dtype=np.float32)
grav = np.array([0.0, 0.0, -1.0], dtype=np.float32)
q = default_pose + 0.05
qd = np.full(14, 0.25, dtype=np.float32)
last_act = np.full(14, -0.5, dtype=np.float32)
cmd = np.arange(13, dtype=np.float32) * 0.1
obs = build_observation(gyro, grav, q, qd, last_act, cmd, default_pose)
assert obs.shape == (61,)
assert obs.dtype == np.float32
assert np.allclose(obs[0:3], gyro)
assert np.allclose(obs[3:6], grav)
assert np.allclose(obs[6:20], 0.05, atol=1e-6)
assert np.allclose(obs[20:34], 0.25)
assert np.allclose(obs[34:48], -0.5)
assert np.allclose(obs[48:61], cmd)
targets = decode_action(last_act, default_pose, action_scale=1.0)
assert np.allclose(targets, default_pose - 0.5)
with pytest.raises(ValueError):
build_observation(np.array([np.nan, 0.0, 0.0]), grav, q, qd, last_act, cmd, default_pose)
def test_script_command_interpolation_matches_js():
script = {
"duration_s": 2.0,
"loop": True,
"keyframes": [
{"t": 0.0, "cmd": [0.0, 0.0, 0.0]},
{"t": 1.0, "cmd": [0.2, 0.0, -0.4]},
],
}
mid = script_command_at(script, 0.5, 13)
assert mid.shape == (13,)
assert np.allclose(mid[:3], [0.1, 0.0, -0.2])
assert np.allclose(mid[3:], 0.0)
def test_state_machine_lifecycle_soft_start_skills_and_fall():
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
sm = MicroduckStateMachine(manifest)
assert sm.lifecycle == LifecycleState.IDLE
# Soft start transitions to ACTIVE walk after 1.5s (75 steps at 50Hz)
sm.request_stand(np.zeros(14, dtype=np.float64), soft_start_s=1.5)
assert sm.lifecycle == LifecycleState.SOFT_START
for _ in range(74):
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.torque_enabled is True
assert dec.policy_key is None
assert 0.0 < dec.soft_start_alpha <= 1.0
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert sm.lifecycle == LifecycleState.ACTIVE
assert dec.policy_key == "walk"
# Kick runs 25 steps then locks velocity for 20 steps
sm.set_velocity_cmd(0.2, 0.5, speed_scale=1.0)
assert sm.trigger_action("kickL") is True
for i in range(25):
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.policy_key == "kickL"
assert np.allclose(dec.command_13[:3], [0.0, 0.0, 0.0])
# Now back in walk, but velocity locked for 20 steps
for i in range(20):
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.policy_key == "walk"
assert np.allclose(dec.command_13[:3], [0.0, 0.0, 0.0])
# Lock expired: normalized [0.2, 0.5] scales by legs limits [0.25, -0.2, 1.0]
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.command_13[0] == pytest.approx(0.05)
assert dec.command_13[2] == pytest.approx(0.5)
sm.set_velocity_cmd(-1.0, 0.0, speed_scale=0.5)
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.command_13[0] == pytest.approx(-0.1)
# Ground pick encodes cos/sin phase and zeroes head offsets
sm.trigger_action("headToggle")
sm.set_head_target(1.0, 0.0, 0.0, 0.0)
sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert sm.trigger_action("groundPick") is True
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.policy_key == "groundpick"
assert np.allclose(dec.command_13[3:7], 0.0)
assert math.isclose(dec.command_13[0] ** 2 + dec.command_13[1] ** 2, 1.0, rel_tol=1e-5)
# Clear groundpick to finish
for _ in range(145):
sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert sm.skill_mode == "walk"
# Fall detection: 10 steps of gz > -0.5 triggers FALL_SAFE when auto_stand_on_fall=False
for _ in range(9):
dec = sm.step(0.02, proj_grav_z=-0.2, auto_stand_on_fall=False)
assert sm.lifecycle == LifecycleState.ACTIVE
dec = sm.step(0.02, proj_grav_z=-0.2, auto_stand_on_fall=False)
assert sm.lifecycle == LifecycleState.FALL_SAFE
assert dec.torque_enabled is False
# Re-stand and test auto_stand_on_fall=True
sm.request_stand(np.zeros(14, dtype=np.float64), soft_start_s=0.02)
sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=True)
assert sm.lifecycle == LifecycleState.ACTIVE
for _ in range(10):
dec = sm.step(0.02, proj_grav_z=-0.1, auto_stand_on_fall=True)
assert sm.recovery_state == "fallen"
for _ in range(15):
dec = sm.step(0.02, proj_grav_z=-0.1, auto_stand_on_fall=True)
assert sm.recovery_state == "recovering"
assert dec.policy_key == "stand"
for _ in range(50):
dec = sm.step(0.02, proj_grav_z=-0.95, auto_stand_on_fall=True)
assert sm.recovery_state is None
assert sm.skill_mode == "walk"
+197
View File
@@ -0,0 +1,197 @@
import io
import json
import shutil
import sys
import threading
import urllib.error
import urllib.request
import zipfile
from pathlib import Path
import pytest
PYTHON_ROOT = Path(__file__).resolve().parents[1]
UNOQ_ROOT = PYTHON_ROOT.parent
if str(PYTHON_ROOT) not in sys.path:
sys.path.insert(0, str(PYTHON_ROOT))
from core.config import load_hardware_profile, load_robot_manifest # noqa: E402
from core.controller import MicroduckController # noqa: E402
from core.deploy_service import DeployService # noqa: E402
from core.policy_runner import PolicySet # noqa: E402
from hal.mock_hal import MockHal # noqa: E402
from web.fallback_server import FallbackWebServer # noqa: E402
def _make_runtime_dir(tmp_path: Path) -> tuple[Path, MicroduckController]:
base = tmp_path / "unoq"
(base / "config").mkdir(parents=True)
(base / "policies").mkdir(parents=True)
shutil.copy2(UNOQ_ROOT / "config" / "robot_manifest.json", base / "config" / "robot_manifest.json")
shutil.copy2(UNOQ_ROOT / "config" / "hardware_profile.json", base / "config" / "hardware_profile.json")
manifest = load_robot_manifest(base / "config" / "robot_manifest.json")
profile = load_hardware_profile(base / "config" / "hardware_profile.json")
profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] = 77
from core.config import save_hardware_profile
save_hardware_profile(base / "config" / "hardware_profile.json", profile, manifest["joint_names"])
for skill in manifest["skills"].values():
(base / skill["path"]).write_bytes(b"MOCK_ONNX_V1")
hal = MockHal()
policies = PolicySet.load_from_manifest(base, manifest, backend="dummy")
ctrl = MicroduckController(base_dir=base, manifest=manifest, profile=profile, hal=hal, policies=policies)
ctrl.connect()
return base, ctrl
def _build_zip_bytes(
manifest: dict,
files: dict[str, bytes] | None = None,
profile: dict | None = None,
custom_script: dict | None = None,
prefix: str = "",
) -> bytes:
buf = io.BytesIO()
with zipfile.ZipFile(buf, "w", compression=zipfile.ZIP_DEFLATED) as zf:
zf.writestr(f"{prefix}config/robot_manifest.json", json.dumps(manifest))
if profile is not None:
zf.writestr(f"{prefix}config/hardware_profile.json", json.dumps(profile))
if custom_script is not None:
zf.writestr(f"{prefix}config/custom_move_script.json", json.dumps(custom_script))
if files is not None:
for rel, data in files.items():
zf.writestr(f"{prefix}{rel}", data)
else:
for skill in manifest.get("skills", {}).values():
zf.writestr(f"{prefix}{skill['path']}", b"MOCK_ONNX_V2")
return buf.getvalue()
def test_deploy_service_applies_valid_zip_and_preserves_profile_by_default(tmp_path: Path):
base, ctrl = _make_runtime_dir(tmp_path)
deploy = DeployService(base_dir=base, controller=ctrl, policy_backend="dummy")
manifest = load_robot_manifest(base / "config" / "robot_manifest.json")
manifest["variants"]["rollers"]["enabled"] = True
manifest["skills"]["drive"] = {
"path": "policies/drive.onnx",
"kind": "perpetual",
"slot": "drive",
"action_scale": 1.0,
}
incoming_profile = load_hardware_profile(base / "config" / "hardware_profile.json")
incoming_profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] = 0
custom_script = {
"name": "wave",
"durationS": 1.0,
"loop": True,
"keyframes": [{"t": 0.0, "cmd": [0.1] + [0.0] * 12}],
}
manifest["custom_move"] = {"name": "wave", "slot": "script", "type": "script"}
zip_bytes = _build_zip_bytes(manifest, profile=incoming_profile, custom_script=custom_script, prefix="microduck_unoq/")
result = deploy.apply_zip_bytes(zip_bytes, overwrite_profile=False)
assert result["ok"] is True
assert "drive" in result["loaded_skills"]
# On-robot calibration must be preserved unless overwrite_profile=True
assert ctrl.profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 77
assert ctrl.state_machine.rollers_enabled is True
assert ctrl.state_machine.custom_script["name"] == "wave"
assert (base / "policies" / "drive.onnx").read_bytes() == b"MOCK_ONNX_V2"
# Now deploy with overwrite_profile=True
result2 = deploy.apply_zip_bytes(zip_bytes, overwrite_profile=True)
assert result2["ok"] is True
assert ctrl.profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 0
def test_deploy_service_rejects_traversal_and_invalid_manifest(tmp_path: Path):
base, ctrl = _make_runtime_dir(tmp_path)
deploy = DeployService(base_dir=base, controller=ctrl, policy_backend="dummy")
manifest = load_robot_manifest(base / "config" / "robot_manifest.json")
# 1) Path traversal inside zip must be rejected without touching files
bad_buf = io.BytesIO()
with zipfile.ZipFile(bad_buf, "w") as zf:
zf.writestr("../evil.txt", b"pwn")
zf.writestr("config/robot_manifest.json", json.dumps(manifest))
with pytest.raises(ValueError):
deploy.apply_zip_bytes(bad_buf.getvalue())
assert not (base.parent / "evil.txt").exists()
# 2) Wrong obs_size must be rejected
bad_manifest = dict(manifest)
bad_manifest["obs_size"] = 99
with pytest.raises(ValueError):
deploy.apply_zip_bytes(_build_zip_bytes(bad_manifest))
# 3) Missing skill onnx file referenced in manifest must be rejected
partial_files = {"policies/stand.onnx": b"MOCK_ONNX"} # walk.onnx missing
with pytest.raises(ValueError):
deploy.apply_zip_bytes(_build_zip_bytes(manifest, files=partial_files))
def test_fallback_http_server_status_deploy_control_and_mobile_ui(tmp_path: Path):
base, ctrl = _make_runtime_dir(tmp_path)
deploy = DeployService(base_dir=base, controller=ctrl, policy_backend="dummy")
server = FallbackWebServer(host="127.0.0.1", port=0, controller=ctrl, deploy_service=deploy)
port = server.port
thread = threading.Thread(target=server.serve_forever, daemon=True)
thread.start()
try:
# 1) Mobile index page
with urllib.request.urlopen(f"http://127.0.0.1:{port}/", timeout=3) as resp:
html = resp.read().decode("utf-8")
assert resp.status == 200
assert "Microduck" in html
assert "E-STOP" in html
# 2) Initial status
with urllib.request.urlopen(f"http://127.0.0.1:{port}/api/status", timeout=3) as resp:
status = json.loads(resp.read().decode("utf-8"))
assert status["lifecycle"] == "IDLE"
assert "walk" in status["loaded_skills"]
# 3) HTTP push hot deploy (raw application/zip)
manifest = load_robot_manifest(base / "config" / "robot_manifest.json")
manifest["variants"]["rollers"]["enabled"] = True
manifest["skills"]["drive"] = {
"path": "policies/drive.onnx",
"kind": "perpetual",
"slot": "drive",
"action_scale": 1.0,
}
zip_bytes = _build_zip_bytes(manifest)
req = urllib.request.Request(
f"http://127.0.0.1:{port}/api/deploy?overwrite_profile=0",
data=zip_bytes,
method="POST",
headers={"Content-Type": "application/zip"},
)
with urllib.request.urlopen(req, timeout=5) as resp:
deployed = json.loads(resp.read().decode("utf-8"))
assert resp.status == 200
assert deployed["ok"] is True
assert "drive" in deployed["loaded_skills"]
# 4) Stand + step + E-STOP over HTTP
req_stand = urllib.request.Request(f"http://127.0.0.1:{port}/api/stand", data=b"{}", method="POST")
with urllib.request.urlopen(req_stand, timeout=3) as resp:
st = json.loads(resp.read().decode("utf-8"))
assert st["lifecycle"] == "SOFT_START"
ctrl.step_once(dt=0.02)
req_estop = urllib.request.Request(f"http://127.0.0.1:{port}/api/estop", data=b"{}", method="POST")
with urllib.request.urlopen(req_estop, timeout=3) as resp:
st2 = json.loads(resp.read().decode("utf-8"))
assert st2["lifecycle"] == "IDLE"
assert ctrl.profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 77
finally:
server.shutdown()
server.server_close()
+1
View File
@@ -0,0 +1 @@
"""Web HTTP/WebSocket control and deployment package for Microduck UNO Q."""
+184
View File
@@ -0,0 +1,184 @@
from __future__ import annotations
import copy
import json
from email.parser import BytesParser
from email.policy import default as email_default_policy
from pathlib import Path
from typing import Any
from core.controller import MicroduckController
from core.deploy_service import DeployService
STATIC_DIR = Path(__file__).resolve().parent / "static"
def load_index_html() -> bytes:
path = STATIC_DIR / "index.html"
if not path.is_file():
return b"<html><body><h1>Microduck controller UI missing</h1></body></html>"
return path.read_bytes()
def parse_bool_flag(raw: str | None) -> bool:
if raw is None:
return False
return raw.strip().lower() in ("1", "true", "yes", "on")
def extract_deploy_payload(
content_type: str,
body: bytes,
query_overwrite: str | None = None,
) -> tuple[bytes, bool]:
overwrite = parse_bool_flag(query_overwrite)
ctype = (content_type or "").lower()
if "multipart/form-data" in ctype:
raw_msg = f"Content-Type: {content_type}\r\nMIME-Version: 1.0\r\n\r\n".encode("latin1") + body
msg = BytesParser(policy=email_default_policy).parsebytes(raw_msg)
if not msg.is_multipart():
raise ValueError("Malformed multipart/form-data upload")
zip_bytes: bytes | None = None
for part in msg.iter_parts():
field = part.get_param("name", header="content-disposition")
payload = part.get_payload(decode=True) or b""
if field in ("bundle", "file", "zip") and payload:
zip_bytes = payload
elif field == "overwrite_profile":
overwrite = parse_bool_flag(payload.decode("utf-8", errors="ignore"))
if not zip_bytes:
raise ValueError("Multipart upload missing 'bundle' zip field")
return zip_bytes, overwrite
return body, overwrite
def build_status_payload(controller: MicroduckController, deploy_service: DeployService) -> dict[str, Any]:
st = controller.get_status()
st["last_deploy"] = copy.deepcopy(deploy_service.last_deploy) if deploy_service.last_deploy else None
return st
def handle_ws_message(controller: MicroduckController, msg: dict[str, Any]) -> dict[str, Any] | None:
kind = msg.get("type")
if kind == "input":
controller.set_remote_input(
norm_x=float(msg.get("vx", 0.0)),
norm_wz=float(msg.get("wz", 0.0)),
speed_scale=float(msg["speed_scale"]) if "speed_scale" in msg else None,
)
return None
if kind == "head":
controller.set_head_target(
float(msg.get("neck_pitch", 0.0)),
float(msg.get("head_pitch", 0.0)),
float(msg.get("head_yaw", 0.0)),
float(msg.get("head_roll", 0.0)),
)
return None
if kind == "stand":
try:
controller.request_stand()
return {"type": "ack", "event": "stand"}
except RuntimeError as exc:
return {"type": "error", "error": str(exc)}
if kind == "estop":
controller.emergency_stop()
return {"type": "ack", "event": "estop"}
if kind == "action":
action = str(msg.get("action", ""))
ok = controller.trigger_action(action)
return {"type": "ack", "event": "action", "action": action, "ok": ok}
if kind == "settings":
if "auto_stand_on_fall" in msg:
controller.set_auto_stand_on_fall(bool(msg["auto_stand_on_fall"]))
if "speed_scale" in msg:
controller.set_speed_scale(float(msg["speed_scale"]))
return {"type": "ack", "event": "settings"}
if kind == "ping":
return {"type": "pong", "t": msg.get("t")}
return None
def dispatch_api_get(
path: str,
controller: MicroduckController,
deploy_service: DeployService,
) -> tuple[int, dict[str, Any]]:
if path == "/api/status":
return 200, build_status_payload(controller, deploy_service)
if path == "/api/profile":
return 200, {"ok": True, "profile": copy.deepcopy(controller.profile)}
return 404, {"ok": False, "error": f"Unknown GET endpoint: {path}"}
def dispatch_api_post(
path: str,
body_bytes: bytes,
content_type: str,
query_params: dict[str, list[str]],
controller: MicroduckController,
deploy_service: DeployService,
) -> tuple[int, dict[str, Any]]:
if path == "/api/deploy":
try:
q_ow = query_params.get("overwrite_profile", [None])[0]
raw_zip, overwrite = extract_deploy_payload(content_type, body_bytes, q_ow)
res = dict(deploy_service.apply_zip_bytes(raw_zip, overwrite_profile=overwrite))
res["status"] = build_status_payload(controller, deploy_service)
return 200, res
except ValueError as exc:
return 400, {"ok": False, "error": str(exc)}
except Exception as exc:
return 500, {"ok": False, "error": str(exc)}
try:
payload = json.loads(body_bytes.decode("utf-8")) if body_bytes.strip() else {}
except Exception:
return 400, {"ok": False, "error": "Invalid JSON body"}
try:
if path == "/api/stand":
controller.request_stand()
return 200, build_status_payload(controller, deploy_service)
if path == "/api/estop":
controller.emergency_stop()
return 200, build_status_payload(controller, deploy_service)
if path == "/api/action":
action = str(payload.get("action", ""))
ok = controller.trigger_action(action)
return 200, {"ok": ok, "status": build_status_payload(controller, deploy_service)}
if path == "/api/settings":
if "auto_stand_on_fall" in payload:
controller.set_auto_stand_on_fall(bool(payload["auto_stand_on_fall"]))
if "speed_scale" in payload:
controller.set_speed_scale(float(payload["speed_scale"]))
return 200, build_status_payload(controller, deploy_service)
if path == "/api/profile":
profile = controller.update_profile(payload)
return 200, {"ok": True, "profile": profile}
if path == "/api/calibrate/ping":
max_id = int(payload.get("max_id", 20))
return 200, {"ok": True, "ids": controller.ping_servos(max_id=max_id)}
if path == "/api/calibrate/joint_zero":
joint = str(payload.get("joint", ""))
profile = controller.capture_joint_zero(joint)
return 200, {"ok": True, "profile": profile}
if path == "/api/calibrate/test_joint":
joint = str(payload.get("joint", ""))
rad = float(payload.get("rad", 0.0))
ticks = controller.test_single_joint(joint, rad)
return 200, {"ok": True, "ticks": ticks}
if path == "/api/calibrate/exit":
controller.exit_calibration()
return 200, build_status_payload(controller, deploy_service)
if path == "/api/calibrate/imu_zero":
samples = int(payload.get("samples", 200))
imu = controller.calibrate_imu_zero(samples=samples)
return 200, {"ok": True, "imu": imu}
except (ValueError, RuntimeError) as exc:
return 400, {"ok": False, "error": str(exc)}
except Exception as exc:
return 500, {"ok": False, "error": str(exc)}
return 404, {"ok": False, "error": f"Unknown POST endpoint: {path}"}
+227
View File
@@ -0,0 +1,227 @@
from __future__ import annotations
import base64
import hashlib
import json
import select
import socket
import struct
import threading
import time
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
from typing import Any
from urllib.parse import parse_qs, urlparse
from core.controller import MicroduckController
from core.deploy_service import DeployService
from .api import (
build_status_payload,
dispatch_api_get,
dispatch_api_post,
handle_ws_message,
load_index_html,
)
WS_GUID = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11"
def _ws_encode_frame(opcode: int, payload: bytes) -> bytes:
header = bytearray([0x80 | (opcode & 0x0F)])
n = len(payload)
if n < 126:
header.append(n)
elif n < 65536:
header.append(126)
header.extend(struct.pack("!H", n))
else:
header.append(127)
header.extend(struct.pack("!Q", n))
return bytes(header) + payload
def _recv_exact(sock: socket.socket, n: int) -> bytes | None:
chunks = bytearray()
while len(chunks) < n:
chunk = sock.recv(n - len(chunks))
if not chunk:
return None
chunks.extend(chunk)
return bytes(chunks)
def _ws_read_frame(sock: socket.socket) -> tuple[int, bytes] | None:
head = _recv_exact(sock, 2)
if head is None:
return None
b1, b2 = head[0], head[1]
opcode = b1 & 0x0F
masked = bool(b2 & 0x80)
length = b2 & 0x7F
if length == 126:
ext = _recv_exact(sock, 2)
if ext is None:
return None
length = struct.unpack("!H", ext)[0]
elif length == 127:
ext = _recv_exact(sock, 8)
if ext is None:
return None
length = struct.unpack("!Q", ext)[0]
if length > 1024 * 1024:
return None
mask = _recv_exact(sock, 4) if masked else b""
if masked and mask is None:
return None
payload = _recv_exact(sock, length) if length > 0 else b""
if payload is None:
return None
if masked:
payload = bytes(b ^ mask[i % 4] for i, b in enumerate(payload))
return opcode, payload
class FallbackWebServer(ThreadingHTTPServer):
"""Zero-dependency HTTP + RFC6455 WebSocket server (runs on stock Python 3.12)."""
allow_reuse_address = True
daemon_threads = True
def __init__(
self,
host: str,
port: int,
controller: MicroduckController,
deploy_service: DeployService,
) -> None:
self.controller = controller
self.deploy_service = deploy_service
super().__init__((host, port), _RequestHandler)
self.port = int(self.server_address[1])
class _RequestHandler(BaseHTTPRequestHandler):
protocol_version = "HTTP/1.1"
def log_message(self, format: str, *args: Any) -> None: # noqa: A002
return
def _send_cors_headers(self) -> None:
self.send_header("Access-Control-Allow-Origin", "*")
self.send_header("Access-Control-Allow-Methods", "GET, POST, OPTIONS")
self.send_header("Access-Control-Allow-Headers", "Content-Type")
self.send_header("Access-Control-Allow-Private-Network", "true")
def _send_json(self, status_code: int, payload: dict[str, Any]) -> None:
raw = json.dumps(payload).encode("utf-8")
self.send_response(status_code)
self._send_cors_headers()
self.send_header("Content-Type", "application/json; charset=utf-8")
self.send_header("Content-Length", str(len(raw)))
self.end_headers()
self.wfile.write(raw)
def _send_bytes(self, status_code: int, content_type: str, data: bytes) -> None:
self.send_response(status_code)
self._send_cors_headers()
self.send_header("Content-Type", content_type)
self.send_header("Content-Length", str(len(data)))
self.end_headers()
self.wfile.write(data)
def do_OPTIONS(self) -> None: # noqa: N802
self.send_response(204)
self._send_cors_headers()
self.send_header("Content-Length", "0")
self.end_headers()
def do_GET(self) -> None: # noqa: N802
parsed = urlparse(self.path)
if parsed.path == "/ws/control" and self.headers.get("Upgrade", "").lower() == "websocket":
self._handle_websocket()
return
if parsed.path in ("/", "/index.html"):
self._send_bytes(200, "text/html; charset=utf-8", load_index_html())
return
srv: FallbackWebServer = self.server # type: ignore[assignment]
code, payload = dispatch_api_get(parsed.path, srv.controller, srv.deploy_service)
self._send_json(code, payload)
def do_POST(self) -> None: # noqa: N802
parsed = urlparse(self.path)
length = int(self.headers.get("Content-Length", "0") or "0")
if length < 0 or length > 160 * 1024 * 1024:
self._send_json(413, {"ok": False, "error": "Payload too large"})
return
body = self.rfile.read(length) if length > 0 else b""
content_type = self.headers.get("Content-Type", "")
query = parse_qs(parsed.query)
srv: FallbackWebServer = self.server # type: ignore[assignment]
code, payload = dispatch_api_post(
parsed.path,
body,
content_type,
query,
srv.controller,
srv.deploy_service,
)
self._send_json(code, payload)
def _handle_websocket(self) -> None:
key = self.headers.get("Sec-WebSocket-Key")
if not key:
self.send_error(400, "Missing Sec-WebSocket-Key")
return
accept = base64.b64encode(hashlib.sha1((key.strip() + WS_GUID).encode("ascii")).digest()).decode("ascii")
self.send_response(101, "Switching Protocols")
self.send_header("Upgrade", "websocket")
self.send_header("Connection", "Upgrade")
self.send_header("Sec-WebSocket-Accept", accept)
self.end_headers()
srv: FallbackWebServer = self.server # type: ignore[assignment]
sock: socket.socket = self.connection
sock.settimeout(5.0)
write_lock = threading.Lock()
last_telemetry = 0.0
try:
while True:
now = time.monotonic()
if now - last_telemetry >= 0.1:
last_telemetry = now
msg = {"type": "telemetry", **build_status_payload(srv.controller, srv.deploy_service)}
frame = _ws_encode_frame(0x1, json.dumps(msg).encode("utf-8"))
with write_lock:
sock.sendall(frame)
readable, _, _ = select.select([sock], [], [], 0.05)
if not readable:
continue
parsed = _ws_read_frame(sock)
if parsed is None:
break
opcode, payload = parsed
if opcode == 0x8: # close
with write_lock:
sock.sendall(_ws_encode_frame(0x8, b""))
break
if opcode == 0x9: # ping -> pong
with write_lock:
sock.sendall(_ws_encode_frame(0xA, payload))
continue
if opcode == 0x1: # text
try:
data = json.loads(payload.decode("utf-8"))
except Exception:
continue
ack = handle_ws_message(srv.controller, data)
if ack is not None:
with write_lock:
sock.sendall(_ws_encode_frame(0x1, json.dumps(ack).encode("utf-8")))
except (BrokenPipeError, ConnectionResetError, TimeoutError, OSError):
pass
finally:
self.close_connection = True
+92
View File
@@ -0,0 +1,92 @@
from __future__ import annotations
import asyncio
import json
from typing import Any
from urllib.parse import parse_qs
from core.controller import MicroduckController
from core.deploy_service import DeployService
from .api import (
build_status_payload,
dispatch_api_get,
dispatch_api_post,
handle_ws_message,
load_index_html,
)
def create_fastapi_app(controller: MicroduckController, deploy_service: DeployService) -> Any:
"""Create FastAPI app backed by the shared web.api dispatcher."""
from fastapi import FastAPI, Request, WebSocket, WebSocketDisconnect
from fastapi.middleware.cors import CORSMiddleware
from fastapi.responses import HTMLResponse, JSONResponse
app = FastAPI(title="Microduck UNO Q Controller", version="0.1.0")
app.add_middleware(
CORSMiddleware,
allow_origins=["*"],
allow_methods=["*"],
allow_headers=["*"],
)
@app.middleware("http")
async def add_pna_header(request: Request, call_next: Any) -> Any:
resp = await call_next(request)
resp.headers["Access-Control-Allow-Private-Network"] = "true"
return resp
@app.get("/", response_class=HTMLResponse)
async def index() -> HTMLResponse:
return HTMLResponse(content=load_index_html().decode("utf-8"))
@app.get("/api/{subpath:path}")
async def api_get(subpath: str) -> JSONResponse:
code, payload = dispatch_api_get(f"/api/{subpath}", controller, deploy_service)
return JSONResponse(status_code=code, content=payload)
@app.post("/api/{subpath:path}")
async def api_post(subpath: str, request: Request) -> JSONResponse:
body = await request.body()
content_type = request.headers.get("content-type", "")
query = parse_qs(request.url.query)
code, payload = dispatch_api_post(
f"/api/{subpath}",
body,
content_type,
query,
controller,
deploy_service,
)
return JSONResponse(status_code=code, content=payload)
@app.websocket("/ws/control")
async def ws_control(websocket: WebSocket) -> None:
await websocket.accept()
stop_event = asyncio.Event()
async def telemetry_loop() -> None:
while not stop_event.is_set():
msg = {"type": "telemetry", **build_status_payload(controller, deploy_service)}
await websocket.send_text(json.dumps(msg))
await asyncio.sleep(0.1)
sender = asyncio.create_task(telemetry_loop())
try:
while True:
raw = await websocket.receive_text()
try:
data = json.loads(raw)
except Exception:
continue
ack = handle_ws_message(controller, data)
if ack is not None:
await websocket.send_text(json.dumps(ack))
except WebSocketDisconnect:
pass
finally:
stop_event.set()
sender.cancel()
return app
+619
View File
@@ -0,0 +1,619 @@
<!doctype html>
<html lang="en">
<head>
<meta charset="utf-8" />
<meta name="viewport" content="width=device-width,initial-scale=1,maximum-scale=1,user-scalable=no" />
<title>Microduck UNO Q Controller</title>
<style>
:root {
--bg: #0f1117;
--panel: #191d27;
--ink: #f6f4ed;
--muted: #98a2b3;
--accent: #ff7a2f;
--accent-dark: #c95212;
--danger: #e53935;
--ok: #2ecc71;
--warn: #f39c12;
--border: #2a3142;
--mono: ui-monospace, SFMono-Regular, Menlo, Monaco, Consolas, monospace;
}
* { box-sizing: border-box; -webkit-tap-highlight-color: transparent; }
body {
margin: 0;
background: var(--bg);
color: var(--ink);
font-family: var(--mono);
overflow-x: hidden;
min-height: 100vh;
}
.topbar {
display: flex;
flex-wrap: wrap;
align-items: center;
justify-content: space-between;
gap: 8px;
padding: 10px 14px;
background: var(--panel);
border-bottom: 2px solid var(--border);
}
.brand { font-weight: 700; font-size: 15px; letter-spacing: 0.04em; color: var(--accent); }
.badges { display: flex; flex-wrap: wrap; gap: 6px; align-items: center; font-size: 12px; }
.badge {
padding: 3px 8px;
border-radius: 999px;
border: 1px solid var(--border);
background: #12151d;
}
.badge.ok { border-color: var(--ok); color: var(--ok); }
.badge.warn { border-color: var(--warn); color: var(--warn); }
.badge.danger { border-color: var(--danger); color: var(--danger); }
.tabs { display: flex; gap: 6px; }
.tab-btn {
padding: 6px 12px;
background: #12151d;
color: var(--muted);
border: 1px solid var(--border);
border-radius: 6px;
font-family: inherit;
font-size: 12px;
font-weight: 700;
cursor: pointer;
}
.tab-btn.active { background: var(--accent); color: #08080c; border-color: var(--accent); }
.safety-bar {
display: grid;
grid-template-columns: 1.3fr 1.2fr 1fr 1fr;
gap: 8px;
padding: 10px 14px;
background: #141821;
border-bottom: 1px solid var(--border);
align-items: center;
}
@media (max-width: 720px) {
.safety-bar { grid-template-columns: 1fr 1fr; }
}
button.btn {
font-family: inherit;
font-weight: 700;
font-size: 13px;
border-radius: 8px;
border: 1px solid var(--border);
padding: 10px 12px;
background: var(--panel);
color: var(--ink);
cursor: pointer;
}
button.btn:active { transform: translateY(1px); }
button.btn:disabled { opacity: 0.4; cursor: not-allowed; }
button.btn-estop {
background: var(--danger);
border-color: #ff6b6b;
color: #fff;
font-size: 15px;
letter-spacing: 0.06em;
}
button.btn-stand {
background: var(--accent);
border-color: #ffa573;
color: #08080c;
}
.toggle-box, .slider-box {
display: flex;
flex-direction: column;
gap: 4px;
font-size: 11px;
color: var(--muted);
background: var(--panel);
border: 1px solid var(--border);
border-radius: 8px;
padding: 6px 10px;
}
.toggle-row { display: flex; justify-content: space-between; align-items: center; gap: 8px; color: var(--ink); font-size: 12px; }
.main { padding: 12px 14px; }
.hidden { display: none !important; }
.control-grid {
display: grid;
grid-template-columns: minmax(240px, 1fr) minmax(240px, 1fr);
gap: 14px;
align-items: stretch;
}
@media (max-width: 700px) {
.control-grid { grid-template-columns: 1fr; }
}
.card {
background: var(--panel);
border: 1px solid var(--border);
border-radius: 10px;
padding: 12px;
}
.card-title { font-size: 12px; color: var(--muted); margin: 0 0 10px; text-transform: uppercase; letter-spacing: 0.06em; }
.stick-zone {
position: relative;
height: 250px;
border-radius: 10px;
background: radial-gradient(circle at center, #202636 0%, #11151f 75%);
border: 1px dashed #374151;
touch-action: none;
user-select: none;
overflow: hidden;
display: flex;
align-items: center;
justify-content: center;
}
.stick-hint { color: var(--muted); font-size: 12px; pointer-events: none; text-align: center; padding: 12px; }
.stick-base {
position: absolute;
width: 112px;
height: 112px;
border-radius: 50%;
border: 2px solid rgba(255, 122, 47, 0.75);
background: rgba(255, 122, 47, 0.08);
transform: translate(-50%, -50%);
pointer-events: none;
}
.stick-knob {
position: absolute;
width: 44px;
height: 44px;
border-radius: 50%;
background: var(--accent);
box-shadow: 0 0 16px rgba(255, 122, 47, 0.5);
transform: translate(-50%, -50%);
pointer-events: none;
}
.vel-readout { margin-top: 8px; font-size: 12px; color: var(--muted); display: flex; justify-content: space-between; }
.skills-grid {
display: grid;
grid-template-columns: repeat(2, minmax(0, 1fr));
gap: 8px;
}
.skill-btn.active { background: var(--accent); color: #08080c; border-color: #ffa573; }
.head-panel { margin-top: 10px; display: grid; grid-template-columns: 1fr 1fr; gap: 8px; }
.calib-actions { display: flex; flex-wrap: wrap; gap: 8px; margin-bottom: 10px; }
.joint-table { width: 100%; border-collapse: collapse; font-size: 12px; }
.joint-table th, .joint-table td { border-bottom: 1px solid var(--border); padding: 6px 4px; text-align: left; }
.joint-table input[type="number"] {
width: 64px;
background: #0f1117;
color: var(--ink);
border: 1px solid var(--border);
border-radius: 4px;
padding: 3px 5px;
font-family: inherit;
font-size: 12px;
}
.mini-btn {
padding: 4px 7px;
font-size: 11px;
border-radius: 5px;
border: 1px solid var(--border);
background: #12151d;
color: var(--ink);
font-family: inherit;
cursor: pointer;
}
.mini-btn:hover { border-color: var(--accent); }
.log-line { margin-top: 8px; font-size: 11px; color: var(--muted); word-break: break-word; }
</style>
</head>
<body>
<header class="topbar">
<div class="brand">MICRODUCK // UNO Q</div>
<div class="badges">
<span id="badge-life" class="badge warn">IDLE</span>
<span id="badge-mode" class="badge">Idle</span>
<span id="badge-ws" class="badge">WS ...</span>
<span id="badge-imu" class="badge">IMU --</span>
<span id="badge-batt" class="badge">--.-V</span>
<span id="badge-temp" class="badge">--C</span>
<span id="badge-hz" class="badge">--Hz</span>
</div>
<div class="tabs">
<button id="tab-ctrl" class="tab-btn active" type="button">Control</button>
<button id="tab-cal" class="tab-btn" type="button">Calibration</button>
</div>
</header>
<section class="safety-bar">
<button id="btn-estop" class="btn btn-estop" type="button">E-STOP (TORQUE OFF)</button>
<button id="btn-stand" class="btn btn-stand" type="button">Prepare Stand (1.5s)</button>
<label class="toggle-box">
<span>Recovery Policy</span>
<span class="toggle-row">
<span>Auto Stand on Fall</span>
<input id="chk-auto-stand" type="checkbox" />
</span>
</label>
<label class="slider-box">
<span>Speed Scale: <strong id="lbl-speed">50%</strong></span>
<input id="rng-speed" type="range" min="10" max="100" step="5" value="50" />
</label>
</section>
<main class="main">
<section id="view-control" class="control-grid">
<div class="card">
<h2 class="card-title">Virtual Joystick (Hold & Drag)</h2>
<div id="stick-zone" class="stick-zone">
<div id="stick-hint" class="stick-hint">Touch or drag anywhere here to walk / turn</div>
<div id="stick-base" class="stick-base hidden"></div>
<div id="stick-knob" class="stick-knob hidden"></div>
</div>
<div class="vel-readout">
<span id="lbl-vx">vx norm: 0.00</span>
<span id="lbl-wz">wz norm: 0.00</span>
</div>
<div id="head-panel" class="head-panel hidden">
<label class="slider-box">Neck Pitch <input id="hd-0" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Pitch <input id="hd-1" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Yaw <input id="hd-2" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Roll <input id="hd-3" type="range" min="-1" max="1" step="0.05" value="0" /></label>
</div>
</div>
<div class="card">
<h2 class="card-title">Skills & Modes</h2>
<div class="skills-grid">
<button class="btn skill-btn" data-action="sitStand" type="button">Sit / Stand</button>
<button class="btn skill-btn" data-action="head" id="btn-head" type="button">Head Mode</button>
<button class="btn skill-btn" data-action="kickL" type="button">Kick Left</button>
<button class="btn skill-btn" data-action="kickR" type="button">Kick Right</button>
<button class="btn skill-btn" data-action="groundPick" type="button">Ground Pick</button>
<button class="btn skill-btn" data-action="roll" id="btn-roll" type="button">Roll / Trick</button>
<button class="btn skill-btn" data-action="rollers" id="btn-rollers" type="button">Feet / Rollers</button>
<button class="btn skill-btn hidden" data-action="script" id="btn-script" type="button">Custom Script</button>
</div>
<div id="ctrl-log" class="log-line">Ready. Tap "Prepare Stand" when the robot is upright on the floor.</div>
</div>
</section>
<section id="view-calib" class="card hidden">
<h2 class="card-title">Hardware Profile & Servo Calibration (IDLE Required for Joint Test)</h2>
<div class="calib-actions">
<button id="btn-ping" class="btn" type="button">Ping Servos (1-20)</button>
<button id="btn-imu-zero" class="btn" type="button">IMU Bias Zero (Keep Still)</button>
<button id="btn-exit-cal" class="btn" type="button">Exit Calibration (Torque Off)</button>
<button id="btn-save-prof" class="btn btn-stand" type="button">Save Profile to Robot</button>
</div>
<div id="calib-status" class="log-line">No ping scan yet.</div>
<div style="overflow-x:auto; margin-top:10px;">
<table class="joint-table">
<thead>
<tr>
<th>Joint</th>
<th>ID</th>
<th>Sign</th>
<th>Offset</th>
<th>Live Tick</th>
<th>Live Rad</th>
<th>Actions</th>
</tr>
</thead>
<tbody id="joint-tbody"></tbody>
</table>
</div>
</section>
</main>
<script>
(() => {
const $ = (id) => document.getElementById(id);
let ws = null;
let reconnectTimer = null;
let profile = null;
let latestStatus = null;
const stickState = { active: false, pointerId: null, originX: 0, originY: 0, nx: 0, nwz: 0 };
const MAX_RADIUS = 56;
function log(msg) {
$("ctrl-log").textContent = msg;
}
async function apiPost(path, body) {
const res = await fetch(path, {
method: "POST",
headers: { "Content-Type": "application/json" },
body: JSON.stringify(body || {}),
});
const data = await res.json();
if (!res.ok || data.ok === false) {
throw new Error(data.error || `HTTP ${res.status}`);
}
return data;
}
function setTab(tab) {
const isCtrl = tab === "ctrl";
$("view-control").classList.toggle("hidden", !isCtrl);
$("view-calib").classList.toggle("hidden", isCtrl);
$("tab-ctrl").classList.toggle("active", isCtrl);
$("tab-cal").classList.toggle("active", !isCtrl);
if (!isCtrl) loadProfile();
}
$("tab-ctrl").onclick = () => setTab("ctrl");
$("tab-cal").onclick = () => setTab("cal");
function sendWs(obj) {
if (ws && ws.readyState === WebSocket.OPEN) {
ws.send(JSON.stringify(obj));
return true;
}
return false;
}
function updateStatusUI(st) {
latestStatus = st;
const lifeEl = $("badge-life");
lifeEl.textContent = st.lifecycle || "UNKNOWN";
lifeEl.className = "badge " + (st.lifecycle === "ACTIVE" ? "ok" : st.lifecycle === "FALL_SAFE" ? "danger" : "warn");
$("badge-mode").textContent = st.mode_label || st.skill_mode || "-";
$("badge-imu").textContent = st.imu_ok ? "IMU OK" : "IMU ERR";
$("badge-imu").className = "badge " + (st.imu_ok ? "ok" : "danger");
$("badge-batt").textContent = `${Number(st.voltage_v || 0).toFixed(2)}V`;
$("badge-temp").textContent = `${Number(st.max_temp_c || 0).toFixed(0)}C`;
$("badge-hz").textContent = `${Number(st.ctrl_hz || 0).toFixed(0)}Hz`;
$("chk-auto-stand").checked = !!st.auto_stand_on_fall;
const spdPct = Math.round((st.speed_scale || 0.5) * 100);
if (document.activeElement !== $("rng-speed")) {
$("rng-speed").value = String(spdPct);
$("lbl-speed").textContent = `${spdPct}%`;
}
$("btn-head").classList.toggle("active", !!st.head_mode);
$("head-panel").classList.toggle("hidden", !st.head_mode);
$("btn-rollers").disabled = !st.rollers_enabled;
$("btn-rollers").classList.toggle("active", st.loco === "rollers");
const hasScript = !!(st.custom_move && st.custom_move.slot === "script");
$("btn-script").classList.toggle("hidden", !hasScript);
if (st.error) log(`Error: ${st.error}`);
if (st.ticks && st.joint_pos_rad && profile) {
Object.keys(profile.joints).forEach((name, idx) => {
const tCell = document.getElementById(`live-tick-${name}`);
const rCell = document.getElementById(`live-rad-${name}`);
if (tCell) tCell.textContent = String(st.ticks[idx] ?? "-");
if (rCell) rCell.textContent = Number(st.joint_pos_rad[idx] ?? 0).toFixed(3);
});
}
}
function connectWs() {
if (reconnectTimer) clearTimeout(reconnectTimer);
const proto = location.protocol === "https:" ? "wss:" : "ws:";
ws = new WebSocket(`${proto}//${location.host}/ws/control`);
ws.onopen = () => {
$("badge-ws").textContent = "WS LIVE";
$("badge-ws").className = "badge ok";
};
ws.onclose = () => {
$("badge-ws").textContent = "WS OFF";
$("badge-ws").className = "badge danger";
reconnectTimer = setTimeout(connectWs, 1000);
};
ws.onmessage = (evt) => {
try {
const msg = JSON.parse(evt.data);
if (msg.type === "telemetry") updateStatusUI(msg);
} catch (_) {}
};
}
// Safety & Skill controls
$("btn-estop").onclick = async () => {
sendWs({ type: "estop" });
try { updateStatusUI(await apiPost("/api/estop")); log("E-STOP activated (torque off)"); } catch (e) { log(e.message); }
};
$("btn-stand").onclick = async () => {
try { updateStatusUI(await apiPost("/api/stand")); log("Soft-start stand engaged..."); } catch (e) { log(e.message); }
};
$("chk-auto-stand").onchange = async (e) => {
try { updateStatusUI(await apiPost("/api/settings", { auto_stand_on_fall: e.target.checked })); } catch (err) { log(err.message); }
};
$("rng-speed").oninput = (e) => {
$("lbl-speed").textContent = `${e.target.value}%`;
};
$("rng-speed").onchange = async (e) => {
try { updateStatusUI(await apiPost("/api/settings", { speed_scale: Number(e.target.value) / 100 })); } catch (err) { log(err.message); }
};
document.querySelectorAll(".skill-btn").forEach((btn) => {
btn.onclick = async () => {
const action = btn.dataset.action;
try {
const res = await apiPost("/api/action", { action });
if (res.status) updateStatusUI(res.status);
log(res.ok ? `Triggered ${action}` : `Action '${action}' rejected in current state`);
} catch (e) { log(e.message); }
};
});
const sendHead = () => {
sendWs({
type: "head",
neck_pitch: Number($("hd-0").value),
head_pitch: Number($("hd-1").value),
head_yaw: Number($("hd-2").value),
head_roll: Number($("hd-3").value),
});
};
["hd-0", "hd-1", "hd-2", "hd-3"].forEach((id) => { $(id).oninput = sendHead; });
// Floating virtual joystick
const zone = $("stick-zone");
const base = $("stick-base");
const knob = $("stick-knob");
const hint = $("stick-hint");
function updateStickVisual(x, y) {
if (!stickState.active) {
base.classList.add("hidden");
knob.classList.add("hidden");
hint.classList.remove("hidden");
return;
}
hint.classList.add("hidden");
base.classList.remove("hidden");
knob.classList.remove("hidden");
base.style.left = `${stickState.originX}px`;
base.style.top = `${stickState.originY}px`;
knob.style.left = `${x}px`;
knob.style.top = `${y}px`;
}
function applyStickCoords(clientX, clientY) {
const rect = zone.getBoundingClientRect();
const px = clientX - rect.left;
const py = clientY - rect.top;
const dx = px - stickState.originX;
const dy = py - stickState.originY;
const dist = Math.hypot(dx, dy);
const clamped = dist > MAX_RADIUS ? MAX_RADIUS / dist : 1.0;
const cx = stickState.originX + dx * clamped;
const cy = stickState.originY + dy * clamped;
stickState.nx = -((cy - stickState.originY) / MAX_RADIUS);
stickState.nwz = -((cx - stickState.originX) / MAX_RADIUS);
$("lbl-vx").textContent = `vx norm: ${stickState.nx.toFixed(2)}`;
$("lbl-wz").textContent = `wz norm: ${stickState.nwz.toFixed(2)}`;
updateStickVisual(cx, cy);
}
function releaseStick() {
stickState.active = false;
stickState.pointerId = null;
stickState.nx = 0;
stickState.nwz = 0;
$("lbl-vx").textContent = "vx norm: 0.00";
$("lbl-wz").textContent = "wz norm: 0.00";
updateStickVisual(0, 0);
sendWs({ type: "input", vx: 0, wz: 0, speed_scale: Number($("rng-speed").value) / 100 });
}
zone.addEventListener("pointerdown", (e) => {
const rect = zone.getBoundingClientRect();
stickState.active = true;
stickState.pointerId = e.pointerId;
stickState.originX = e.clientX - rect.left;
stickState.originY = e.clientY - rect.top;
zone.setPointerCapture(e.pointerId);
applyStickCoords(e.clientX, e.clientY);
});
zone.addEventListener("pointermove", (e) => {
if (!stickState.active || e.pointerId !== stickState.pointerId) return;
applyStickCoords(e.clientX, e.clientY);
});
zone.addEventListener("pointerup", (e) => {
if (e.pointerId === stickState.pointerId) releaseStick();
});
zone.addEventListener("pointercancel", releaseStick);
setInterval(() => {
if (stickState.active) {
sendWs({
type: "input",
vx: stickState.nx,
wz: stickState.nwz,
speed_scale: Number($("rng-speed").value) / 100,
});
}
}, 50);
// Calibration helpers
function renderJointTable() {
const tbody = $("joint-tbody");
tbody.innerHTML = "";
if (!profile || !profile.joints) return;
Object.entries(profile.joints).forEach(([name, cfg]) => {
const tr = document.createElement("tr");
tr.innerHTML = `
<td>${name}</td>
<td><input type="number" min="0" max="253" value="${cfg.id}" data-j="${name}" data-f="id" /></td>
<td><button class="mini-btn" type="button" data-flip="${name}">${cfg.sign > 0 ? "+1" : "-1"}</button></td>
<td><input type="number" min="-4096" max="4096" value="${cfg.zero_offset_ticks}" data-j="${name}" data-f="zero_offset_ticks" /></td>
<td id="live-tick-${name}">-</td>
<td id="live-rad-${name}">0.000</td>
<td>
<button class="mini-btn" type="button" data-zero="${name}">Zero</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="-0.2">-0.2</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="0">0</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="0.2">+0.2</button>
</td>
`;
tbody.appendChild(tr);
});
tbody.querySelectorAll("input[data-j]").forEach((inp) => {
inp.onchange = () => {
profile.joints[inp.dataset.j][inp.dataset.f] = Number(inp.value);
};
});
tbody.querySelectorAll("button[data-flip]").forEach((btn) => {
btn.onclick = () => {
const j = profile.joints[btn.dataset.flip];
j.sign = j.sign > 0 ? -1 : 1;
btn.textContent = j.sign > 0 ? "+1" : "-1";
};
});
tbody.querySelectorAll("button[data-zero]").forEach((btn) => {
btn.onclick = async () => {
try {
const res = await apiPost("/api/calibrate/joint_zero", { joint: btn.dataset.zero });
profile = res.profile;
renderJointTable();
$("calib-status").textContent = `Captured current tick as zero for ${btn.dataset.zero}`;
} catch (e) { $("calib-status").textContent = `Error: ${e.message}`; }
};
});
tbody.querySelectorAll("button[data-test]").forEach((btn) => {
btn.onclick = async () => {
try {
await apiPost("/api/calibrate/test_joint", { joint: btn.dataset.test, rad: Number(btn.dataset.rad) });
$("calib-status").textContent = `Commanded ${btn.dataset.test} -> ${btn.dataset.rad} rad`;
} catch (e) { $("calib-status").textContent = `Error: ${e.message}`; }
};
});
}
async function loadProfile() {
const res = await fetch("/api/profile");
const data = await res.json();
profile = data.profile;
renderJointTable();
}
$("btn-ping").onclick = async () => {
try {
const res = await apiPost("/api/calibrate/ping", { max_id: 20 });
$("calib-status").textContent = `Responding IDs (${res.ids.length}): [${res.ids.join(", ")}]`;
} catch (e) { $("calib-status").textContent = `Ping error: ${e.message}`; }
};
$("btn-imu-zero").onclick = async () => {
try {
$("calib-status").textContent = "Sampling IMU bias (keep robot stationary)...";
const res = await apiPost("/api/calibrate/imu_zero", { samples: 200 });
if (profile) profile.imu = res.imu;
$("calib-status").textContent = `IMU gyro bias updated: [${res.imu.gyro_bias_rad_s.map((x) => x.toFixed(4)).join(", ")}]`;
} catch (e) { $("calib-status").textContent = `IMU zero error: ${e.message}`; }
};
$("btn-exit-cal").onclick = async () => {
try { updateStatusUI(await apiPost("/api/calibrate/exit")); $("calib-status").textContent = "Exited calibration (torque off)"; } catch (e) { $("calib-status").textContent = e.message; }
};
$("btn-save-prof").onclick = async () => {
if (!profile) return;
try {
const res = await apiPost("/api/profile", profile);
profile = res.profile;
renderJointTable();
$("calib-status").textContent = "Saved hardware_profile.json to UNO Q.";
} catch (e) { $("calib-status").textContent = `Save error: ${e.message}`; }
};
fetch("/api/status").then((r) => r.json()).then(updateStatusUI).catch(() => {});
connectWs();
})();
</script>
</body>
</html>