feat: add Microduck UNO Q deployment and hotspot skill control
This commit is contained in:
@@ -0,0 +1 @@
|
||||
"""Microduck Arduino UNO Q Python runtime package."""
|
||||
@@ -0,0 +1 @@
|
||||
"""Core configuration, observation pipeline, ONNX runner, and control loop."""
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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"])
|
||||
@@ -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)
|
||||
@@ -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,
|
||||
)
|
||||
@@ -0,0 +1 @@
|
||||
"""Hardware abstraction layers for Microduck on Arduino UNO Q."""
|
||||
@@ -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."""
|
||||
@@ -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)
|
||||
@@ -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])]
|
||||
@@ -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())
|
||||
@@ -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")
|
||||
@@ -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"
|
||||
@@ -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()
|
||||
@@ -0,0 +1 @@
|
||||
"""Web HTTP/WebSocket control and deployment package for Microduck UNO Q."""
|
||||
@@ -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}"}
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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>
|
||||
Reference in New Issue
Block a user