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
+216
View File
@@ -0,0 +1,216 @@
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"