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"