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