feat: add Microduck UNO Q deployment and hotspot skill control
This commit is contained in:
@@ -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")
|
||||
Reference in New Issue
Block a user