feat: add Microduck UNO Q deployment and hotspot skill control

This commit is contained in:
2026-11-19 13:49:08 +08:00
parent 2b0c395508
commit 164a397726
99 changed files with 12488 additions and 4 deletions
@@ -0,0 +1,134 @@
import sys
from pathlib import Path
import numpy as np
import pytest
PYTHON_ROOT = Path(__file__).resolve().parents[1]
UNOQ_ROOT = PYTHON_ROOT.parent
if str(PYTHON_ROOT) not in sys.path:
sys.path.insert(0, str(PYTHON_ROOT))
from core.config import load_hardware_profile, load_robot_manifest # noqa: E402
from core.controller import MicroduckController # noqa: E402
from core.policy_runner import PolicySet # noqa: E402
from core.state_machine import LifecycleState # noqa: E402
from hal.mock_hal import MockHal # noqa: E402
def _make_controller(tmp_path: Path, auto_stand_on_fall: bool = False):
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json")
profile["safety"]["auto_stand_on_fall"] = auto_stand_on_fall
profile["safety"]["soft_start_s"] = 0.04 # 2 steps at 50Hz for fast test
hal = MockHal()
policies = PolicySet.load_from_manifest(UNOQ_ROOT, manifest, backend="dummy")
ctrl = MicroduckController(
base_dir=UNOQ_ROOT,
manifest=manifest,
profile=profile,
hal=hal,
policies=policies,
profile_path=tmp_path / "hardware_profile.json",
)
return ctrl, hal
def test_controller_stand_walk_estop_and_calibration_with_mock_hal(tmp_path: Path):
ctrl, hal = _make_controller(tmp_path, auto_stand_on_fall=False)
ctrl.connect()
# Initially idle, torque off
st = ctrl.step_once(dt=0.02)
assert st["lifecycle"] == LifecycleState.IDLE.value
assert st["torque_enabled"] is False
# Prepare stand -> soft start -> active walk
ctrl.request_stand()
ctrl.step_once(dt=0.02)
assert ctrl.state_machine.lifecycle == LifecycleState.SOFT_START
st = ctrl.step_once(dt=0.02)
assert st["lifecycle"] == LifecycleState.ACTIVE
assert st["torque_enabled"] is True
# Remote control timeout > 0.5s zeroes velocity command
ctrl.set_remote_input(0.8, 0.4, now_s=10.0)
ctrl.step_once(dt=0.02, now_s=10.1)
assert ctrl.state_machine.vel_cmd[0] > 0.0
ctrl.step_once(dt=0.02, now_s=10.7)
assert np.allclose(ctrl.state_machine.vel_cmd, 0.0)
# E-STOP disables torque immediately
ctrl.emergency_stop()
st = ctrl.step_once(dt=0.02)
assert st["lifecycle"] == LifecycleState.IDLE.value
assert st["torque_enabled"] is False
assert hal.torque_enabled is False
def test_controller_fall_protection_and_auto_stand_recovery(tmp_path: Path):
# Case 1: default auto_stand_on_fall=False -> FALL_SAFE torque off
ctrl, hal = _make_controller(tmp_path / "c1", auto_stand_on_fall=False)
ctrl.connect()
ctrl.request_stand()
ctrl.step_once(dt=0.02)
ctrl.step_once(dt=0.02)
assert ctrl.state_machine.lifecycle == LifecycleState.ACTIVE
# sync_step samples the updated mock gravity on the 1st tick, then 10 debounce ticks follow
hal.set_mock_gravity([0.9, 0.0, -0.1])
ctrl.step_once(dt=0.02)
for _ in range(10):
st = ctrl.step_once(dt=0.02)
assert st["lifecycle"] == LifecycleState.FALL_SAFE.value
assert st["torque_enabled"] is False
# Case 2: auto_stand_on_fall=True -> enters recovery using stand policy
ctrl2, hal2 = _make_controller(tmp_path / "c2", auto_stand_on_fall=True)
ctrl2.connect()
ctrl2.request_stand()
ctrl2.step_once(dt=0.02)
ctrl2.step_once(dt=0.02)
hal2.set_mock_gravity([0.9, 0.0, -0.1])
ctrl2.step_once(dt=0.02)
for _ in range(10):
ctrl2.step_once(dt=0.02)
assert ctrl2.state_machine.recovery_state == "fallen"
for _ in range(15):
st = ctrl2.step_once(dt=0.02)
assert st["recovery_state"] == "recovering"
assert st["active_policy"] == "stand"
# Upright restored -> 1 step to sample upright + 50 steps to return to walk
hal2.set_mock_gravity([0.0, 0.0, -1.0])
ctrl2.step_once(dt=0.02)
for _ in range(50):
st = ctrl2.step_once(dt=0.02)
assert st["recovery_state"] is None
assert st["active_policy"] == "walk"
def test_calibration_zero_capture_and_imu_bias(tmp_path: Path):
ctrl, hal = _make_controller(tmp_path)
ctrl.connect()
# Simulate joint 0 sitting at tick 2100 instead of 2048
hal.ticks[0] = 2100
ctrl.step_once(dt=0.02)
updated = ctrl.capture_joint_zero("left_hip_yaw")
assert updated["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 2100 - 2048
hal.set_mock_gyro([0.03, -0.02, 0.01])
imu_res = ctrl.calibrate_imu_zero(samples=10)
assert np.allclose(imu_res["gyro_bias_rad_s"], [0.03, -0.02, 0.01], atol=1e-5)
# Ping servos
ids = ctrl.ping_servos()
assert len(ids) == 14
def test_policy_set_onnx_requires_explicit_dummy_when_missing():
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
with pytest.raises((FileNotFoundError, RuntimeError)):
PolicySet.load_from_manifest(UNOQ_ROOT, manifest, backend="onnx")
+216
View File
@@ -0,0 +1,216 @@
import json
import math
import sys
from pathlib import Path
import numpy as np
import pytest
PYTHON_ROOT = Path(__file__).resolve().parents[1]
UNOQ_ROOT = PYTHON_ROOT.parent
if str(PYTHON_ROOT) not in sys.path:
sys.path.insert(0, str(PYTHON_ROOT))
from core.config import ( # noqa: E402
apply_imu_calibration,
load_hardware_profile,
load_robot_manifest,
rad_to_ticks,
ticks_to_rad,
)
from core.observations import ( # noqa: E402
build_observation,
decode_action,
script_command_at,
)
from core.state_machine import LifecycleState, MicroduckStateMachine # noqa: E402
def test_default_manifest_and_profile_match_microduck_contract():
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json")
assert manifest["obs_size"] == 61
assert manifest["action_size"] == 14
assert manifest["command_size"] == 13
assert len(manifest["joint_names"]) == 14
assert len(manifest["default_pose"]) == 14
assert set(manifest["joint_names"]) == set(profile["joints"].keys())
assert manifest["skills"]["walk"]["path"] == "policies/walk.onnx"
assert manifest["skills"]["stand"]["path"] == "policies/stand.onnx"
def test_rad_to_ticks_and_back_with_sign_and_offset():
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json")
names = manifest["joint_names"]
# Customize two joints with inverted direction and mechanical offset
profile["joints"]["left_hip_pitch"]["sign"] = -1
profile["joints"]["left_hip_pitch"]["zero_offset_ticks"] = 35
profile["joints"]["right_knee"]["sign"] = 1
profile["joints"]["right_knee"]["zero_offset_ticks"] = -20
q_in = np.array(manifest["default_pose"], dtype=np.float64)
ticks = rad_to_ticks(q_in, names, profile)
assert len(ticks) == 14
assert all(0 <= t <= 4095 for t in ticks)
q_out = ticks_to_rad(ticks, names, profile)
# 12-bit quantization resolution ~ 2*pi/4096 = 0.001534 rad -> max error <= 0.001 rad
assert np.allclose(q_out, q_in, atol=1e-3)
def test_rad_to_ticks_clamps_to_joint_mechanical_limits():
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json")
names = manifest["joint_names"]
q_extreme = np.full(14, 10.0, dtype=np.float64)
ticks = rad_to_ticks(q_extreme, names, profile)
q_clamped = ticks_to_rad(ticks, names, profile)
for i, name in enumerate(names):
assert q_clamped[i] <= profile["joints"][name]["max_rad"] + 2e-3
def test_imu_calibration_axis_remap_and_gyro_bias():
profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json")
profile["imu"]["axis_map"] = [
[0, 1, 0],
[-1, 0, 0],
[0, 0, 1],
]
profile["imu"]["gyro_bias_rad_s"] = [0.1, -0.05, 0.02]
gyro, grav = apply_imu_calibration(
np.array([0.2, 0.35, -0.08]),
np.array([0.0, 0.0, -1.0]),
profile,
)
# First subtract bias [0.1, 0.4, -0.1], then rotate via axis_map -> [0.4, -0.1, -0.1]
assert np.allclose(gyro, [0.4, -0.1, -0.1], atol=1e-6)
assert np.allclose(grav, [0.0, 0.0, -1.0], atol=1e-6)
def test_observation_layout_and_action_decoding():
default_pose = np.arange(14, dtype=np.float32) * 0.01
gyro = np.array([0.1, 0.2, 0.3], dtype=np.float32)
grav = np.array([0.0, 0.0, -1.0], dtype=np.float32)
q = default_pose + 0.05
qd = np.full(14, 0.25, dtype=np.float32)
last_act = np.full(14, -0.5, dtype=np.float32)
cmd = np.arange(13, dtype=np.float32) * 0.1
obs = build_observation(gyro, grav, q, qd, last_act, cmd, default_pose)
assert obs.shape == (61,)
assert obs.dtype == np.float32
assert np.allclose(obs[0:3], gyro)
assert np.allclose(obs[3:6], grav)
assert np.allclose(obs[6:20], 0.05, atol=1e-6)
assert np.allclose(obs[20:34], 0.25)
assert np.allclose(obs[34:48], -0.5)
assert np.allclose(obs[48:61], cmd)
targets = decode_action(last_act, default_pose, action_scale=1.0)
assert np.allclose(targets, default_pose - 0.5)
with pytest.raises(ValueError):
build_observation(np.array([np.nan, 0.0, 0.0]), grav, q, qd, last_act, cmd, default_pose)
def test_script_command_interpolation_matches_js():
script = {
"duration_s": 2.0,
"loop": True,
"keyframes": [
{"t": 0.0, "cmd": [0.0, 0.0, 0.0]},
{"t": 1.0, "cmd": [0.2, 0.0, -0.4]},
],
}
mid = script_command_at(script, 0.5, 13)
assert mid.shape == (13,)
assert np.allclose(mid[:3], [0.1, 0.0, -0.2])
assert np.allclose(mid[3:], 0.0)
def test_state_machine_lifecycle_soft_start_skills_and_fall():
manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json")
sm = MicroduckStateMachine(manifest)
assert sm.lifecycle == LifecycleState.IDLE
# Soft start transitions to ACTIVE walk after 1.5s (75 steps at 50Hz)
sm.request_stand(np.zeros(14, dtype=np.float64), soft_start_s=1.5)
assert sm.lifecycle == LifecycleState.SOFT_START
for _ in range(74):
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.torque_enabled is True
assert dec.policy_key is None
assert 0.0 < dec.soft_start_alpha <= 1.0
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert sm.lifecycle == LifecycleState.ACTIVE
assert dec.policy_key == "walk"
# Kick runs 25 steps then locks velocity for 20 steps
sm.set_velocity_cmd(0.2, 0.5, speed_scale=1.0)
assert sm.trigger_action("kickL") is True
for i in range(25):
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.policy_key == "kickL"
assert np.allclose(dec.command_13[:3], [0.0, 0.0, 0.0])
# Now back in walk, but velocity locked for 20 steps
for i in range(20):
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.policy_key == "walk"
assert np.allclose(dec.command_13[:3], [0.0, 0.0, 0.0])
# Lock expired: normalized [0.2, 0.5] scales by legs limits [0.25, -0.2, 1.0]
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.command_13[0] == pytest.approx(0.05)
assert dec.command_13[2] == pytest.approx(0.5)
sm.set_velocity_cmd(-1.0, 0.0, speed_scale=0.5)
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.command_13[0] == pytest.approx(-0.1)
# Ground pick encodes cos/sin phase and zeroes head offsets
sm.trigger_action("headToggle")
sm.set_head_target(1.0, 0.0, 0.0, 0.0)
sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert sm.trigger_action("groundPick") is True
dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert dec.policy_key == "groundpick"
assert np.allclose(dec.command_13[3:7], 0.0)
assert math.isclose(dec.command_13[0] ** 2 + dec.command_13[1] ** 2, 1.0, rel_tol=1e-5)
# Clear groundpick to finish
for _ in range(145):
sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False)
assert sm.skill_mode == "walk"
# Fall detection: 10 steps of gz > -0.5 triggers FALL_SAFE when auto_stand_on_fall=False
for _ in range(9):
dec = sm.step(0.02, proj_grav_z=-0.2, auto_stand_on_fall=False)
assert sm.lifecycle == LifecycleState.ACTIVE
dec = sm.step(0.02, proj_grav_z=-0.2, auto_stand_on_fall=False)
assert sm.lifecycle == LifecycleState.FALL_SAFE
assert dec.torque_enabled is False
# Re-stand and test auto_stand_on_fall=True
sm.request_stand(np.zeros(14, dtype=np.float64), soft_start_s=0.02)
sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=True)
assert sm.lifecycle == LifecycleState.ACTIVE
for _ in range(10):
dec = sm.step(0.02, proj_grav_z=-0.1, auto_stand_on_fall=True)
assert sm.recovery_state == "fallen"
for _ in range(15):
dec = sm.step(0.02, proj_grav_z=-0.1, auto_stand_on_fall=True)
assert sm.recovery_state == "recovering"
assert dec.policy_key == "stand"
for _ in range(50):
dec = sm.step(0.02, proj_grav_z=-0.95, auto_stand_on_fall=True)
assert sm.recovery_state is None
assert sm.skill_mode == "walk"
+197
View File
@@ -0,0 +1,197 @@
import io
import json
import shutil
import sys
import threading
import urllib.error
import urllib.request
import zipfile
from pathlib import Path
import pytest
PYTHON_ROOT = Path(__file__).resolve().parents[1]
UNOQ_ROOT = PYTHON_ROOT.parent
if str(PYTHON_ROOT) not in sys.path:
sys.path.insert(0, str(PYTHON_ROOT))
from core.config import load_hardware_profile, load_robot_manifest # noqa: E402
from core.controller import MicroduckController # noqa: E402
from core.deploy_service import DeployService # noqa: E402
from core.policy_runner import PolicySet # noqa: E402
from hal.mock_hal import MockHal # noqa: E402
from web.fallback_server import FallbackWebServer # noqa: E402
def _make_runtime_dir(tmp_path: Path) -> tuple[Path, MicroduckController]:
base = tmp_path / "unoq"
(base / "config").mkdir(parents=True)
(base / "policies").mkdir(parents=True)
shutil.copy2(UNOQ_ROOT / "config" / "robot_manifest.json", base / "config" / "robot_manifest.json")
shutil.copy2(UNOQ_ROOT / "config" / "hardware_profile.json", base / "config" / "hardware_profile.json")
manifest = load_robot_manifest(base / "config" / "robot_manifest.json")
profile = load_hardware_profile(base / "config" / "hardware_profile.json")
profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] = 77
from core.config import save_hardware_profile
save_hardware_profile(base / "config" / "hardware_profile.json", profile, manifest["joint_names"])
for skill in manifest["skills"].values():
(base / skill["path"]).write_bytes(b"MOCK_ONNX_V1")
hal = MockHal()
policies = PolicySet.load_from_manifest(base, manifest, backend="dummy")
ctrl = MicroduckController(base_dir=base, manifest=manifest, profile=profile, hal=hal, policies=policies)
ctrl.connect()
return base, ctrl
def _build_zip_bytes(
manifest: dict,
files: dict[str, bytes] | None = None,
profile: dict | None = None,
custom_script: dict | None = None,
prefix: str = "",
) -> bytes:
buf = io.BytesIO()
with zipfile.ZipFile(buf, "w", compression=zipfile.ZIP_DEFLATED) as zf:
zf.writestr(f"{prefix}config/robot_manifest.json", json.dumps(manifest))
if profile is not None:
zf.writestr(f"{prefix}config/hardware_profile.json", json.dumps(profile))
if custom_script is not None:
zf.writestr(f"{prefix}config/custom_move_script.json", json.dumps(custom_script))
if files is not None:
for rel, data in files.items():
zf.writestr(f"{prefix}{rel}", data)
else:
for skill in manifest.get("skills", {}).values():
zf.writestr(f"{prefix}{skill['path']}", b"MOCK_ONNX_V2")
return buf.getvalue()
def test_deploy_service_applies_valid_zip_and_preserves_profile_by_default(tmp_path: Path):
base, ctrl = _make_runtime_dir(tmp_path)
deploy = DeployService(base_dir=base, controller=ctrl, policy_backend="dummy")
manifest = load_robot_manifest(base / "config" / "robot_manifest.json")
manifest["variants"]["rollers"]["enabled"] = True
manifest["skills"]["drive"] = {
"path": "policies/drive.onnx",
"kind": "perpetual",
"slot": "drive",
"action_scale": 1.0,
}
incoming_profile = load_hardware_profile(base / "config" / "hardware_profile.json")
incoming_profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] = 0
custom_script = {
"name": "wave",
"durationS": 1.0,
"loop": True,
"keyframes": [{"t": 0.0, "cmd": [0.1] + [0.0] * 12}],
}
manifest["custom_move"] = {"name": "wave", "slot": "script", "type": "script"}
zip_bytes = _build_zip_bytes(manifest, profile=incoming_profile, custom_script=custom_script, prefix="microduck_unoq/")
result = deploy.apply_zip_bytes(zip_bytes, overwrite_profile=False)
assert result["ok"] is True
assert "drive" in result["loaded_skills"]
# On-robot calibration must be preserved unless overwrite_profile=True
assert ctrl.profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 77
assert ctrl.state_machine.rollers_enabled is True
assert ctrl.state_machine.custom_script["name"] == "wave"
assert (base / "policies" / "drive.onnx").read_bytes() == b"MOCK_ONNX_V2"
# Now deploy with overwrite_profile=True
result2 = deploy.apply_zip_bytes(zip_bytes, overwrite_profile=True)
assert result2["ok"] is True
assert ctrl.profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 0
def test_deploy_service_rejects_traversal_and_invalid_manifest(tmp_path: Path):
base, ctrl = _make_runtime_dir(tmp_path)
deploy = DeployService(base_dir=base, controller=ctrl, policy_backend="dummy")
manifest = load_robot_manifest(base / "config" / "robot_manifest.json")
# 1) Path traversal inside zip must be rejected without touching files
bad_buf = io.BytesIO()
with zipfile.ZipFile(bad_buf, "w") as zf:
zf.writestr("../evil.txt", b"pwn")
zf.writestr("config/robot_manifest.json", json.dumps(manifest))
with pytest.raises(ValueError):
deploy.apply_zip_bytes(bad_buf.getvalue())
assert not (base.parent / "evil.txt").exists()
# 2) Wrong obs_size must be rejected
bad_manifest = dict(manifest)
bad_manifest["obs_size"] = 99
with pytest.raises(ValueError):
deploy.apply_zip_bytes(_build_zip_bytes(bad_manifest))
# 3) Missing skill onnx file referenced in manifest must be rejected
partial_files = {"policies/stand.onnx": b"MOCK_ONNX"} # walk.onnx missing
with pytest.raises(ValueError):
deploy.apply_zip_bytes(_build_zip_bytes(manifest, files=partial_files))
def test_fallback_http_server_status_deploy_control_and_mobile_ui(tmp_path: Path):
base, ctrl = _make_runtime_dir(tmp_path)
deploy = DeployService(base_dir=base, controller=ctrl, policy_backend="dummy")
server = FallbackWebServer(host="127.0.0.1", port=0, controller=ctrl, deploy_service=deploy)
port = server.port
thread = threading.Thread(target=server.serve_forever, daemon=True)
thread.start()
try:
# 1) Mobile index page
with urllib.request.urlopen(f"http://127.0.0.1:{port}/", timeout=3) as resp:
html = resp.read().decode("utf-8")
assert resp.status == 200
assert "Microduck" in html
assert "E-STOP" in html
# 2) Initial status
with urllib.request.urlopen(f"http://127.0.0.1:{port}/api/status", timeout=3) as resp:
status = json.loads(resp.read().decode("utf-8"))
assert status["lifecycle"] == "IDLE"
assert "walk" in status["loaded_skills"]
# 3) HTTP push hot deploy (raw application/zip)
manifest = load_robot_manifest(base / "config" / "robot_manifest.json")
manifest["variants"]["rollers"]["enabled"] = True
manifest["skills"]["drive"] = {
"path": "policies/drive.onnx",
"kind": "perpetual",
"slot": "drive",
"action_scale": 1.0,
}
zip_bytes = _build_zip_bytes(manifest)
req = urllib.request.Request(
f"http://127.0.0.1:{port}/api/deploy?overwrite_profile=0",
data=zip_bytes,
method="POST",
headers={"Content-Type": "application/zip"},
)
with urllib.request.urlopen(req, timeout=5) as resp:
deployed = json.loads(resp.read().decode("utf-8"))
assert resp.status == 200
assert deployed["ok"] is True
assert "drive" in deployed["loaded_skills"]
# 4) Stand + step + E-STOP over HTTP
req_stand = urllib.request.Request(f"http://127.0.0.1:{port}/api/stand", data=b"{}", method="POST")
with urllib.request.urlopen(req_stand, timeout=3) as resp:
st = json.loads(resp.read().decode("utf-8"))
assert st["lifecycle"] == "SOFT_START"
ctrl.step_once(dt=0.02)
req_estop = urllib.request.Request(f"http://127.0.0.1:{port}/api/estop", data=b"{}", method="POST")
with urllib.request.urlopen(req_estop, timeout=3) as resp:
st2 = json.loads(resp.read().decode("utf-8"))
assert st2["lifecycle"] == "IDLE"
assert ctrl.profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 77
finally:
server.shutdown()
server.server_close()