135 lines
4.8 KiB
Python
135 lines
4.8 KiB
Python
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")
|