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