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