Files
UniversalTrain-ai/tests/test_policy_gait_diagnostic.py
chenlin ceca51b0d8
Checks / Layout, translation and Colab checks (push) Has been cancelled
Checks / Ruff (push) Has been cancelled
Checks / Framework tests (native backend, CPU) (push) Has been cancelled
Checks / Portable Rust controller (push) Has been cancelled
Improve generic robot onboarding, walking controls and end-effector interaction
Default humanoid intake to command-following gait and support bounded Cartesian target dragging for arms. Carry model and actuator semantics through calibration, native verification and packaging. Document reusable intake decisions and cover them with regression tests.
2026-11-06 10:54:58 +08:00

103 lines
3.9 KiB
Python

"""Contact evidence cannot count stationary, chatter, or partial air phases."""
import mujoco
import pytest
from tools.checks.measure_policy_gait import (
ContactCycles,
SoleMeasurements,
foot_site_ids,
horizontal_ground_height,
)
def _sample(states, dt=.02):
tracker = ContactCycles()
for index, state in enumerate(states):
tracker.sample(state, index * dt)
return tracker.result(.04, 2)
def test_repeated_stance_swing_cycles_are_detected():
result = _sample([True] * 4 + [False] * 4 + [True] * 4 + [False] * 4 + [True] * 4)
assert result["passed"]
assert result["qualifying_swings"] == result["qualifying_stances"] == 2
assert result["takeoffs"] == result["landings"] == 2
def test_standing_and_contact_chatter_do_not_pass():
assert not _sample([True] * 100)["passed"]
chatter = _sample([True, False] * 100)
assert not chatter["passed"]
assert chatter["qualifying_swings"] == 0
def test_warmup_truncated_and_unlanded_swings_do_not_count():
result = _sample([False] * 5 + [True] * 5 + [False] * 5)
assert result["landings"] == 1
assert result["takeoffs"] == 1
assert result["completed_air_times_s"] == []
assert not result["passed"]
def test_stride_duration_measures_two_observed_landings():
result = _sample([False] * 3 + [True] * 4 + [False] * 4 + [True] * 4)
assert result["landing_to_landing_stride_times_s"] == pytest.approx([.16])
def test_sole_peaks_exclude_partial_swings_and_slip_uses_only_loaded_samples():
sole = SoleMeasurements()
sole.sample(False, .5, 20.) # Warmup-truncated flight must not set a peak.
sole.sample(True, 0., .1)
sole.sample(False, .02, 50.)
sole.sample(False, .07, 50.)
sole.sample(False, .04, 50.)
sole.sample(True, 0., .3)
sole.sample(False, .9, 50.) # An unfinished final swing is also excluded.
result = sole.result(.02)
assert result["completed_swing_peak_heights_m"] == [.07]
assert result["mean_completed_swing_peak_height_m"] == .07
assert result["contact_samples"] == 2
assert result["mean_contact_slip_speed_m_s"] == pytest.approx(.2)
assert result["rms_contact_slip_speed_m_s"] == pytest.approx((.05) ** .5)
assert result["integrated_contact_slip_distance_m"] == pytest.approx(.008)
def _sole_model(ground='type="plane" size="2 2 .1" pos="0 0 .2"'):
return mujoco.MjModel.from_xml_string(f"""
<mujoco><worldbody>
<geom name="ground" {ground}/>
<body name="left" pos="0 .2 .3"><site name="left_sole"/></body>
<body name="right" pos="0 -.2 .4"><site name="right_sole"/></body>
</worldbody></mujoco>
""")
def test_sole_mapping_checks_body_association_and_plane_height():
model = _sole_model()
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
ids = foot_site_ids(model, ["right", "left"], ["right_sole", "left_sole"])
height = horizontal_ground_height(model, data, model.geom("ground").id)
assert height == .2
assert data.site_xpos[ids["left"], 2] - height == pytest.approx(.1)
assert data.site_xpos[ids["right"], 2] - height == pytest.approx(.2)
with pytest.raises(ValueError, match="attached"):
foot_site_ids(model, ["right", "left"], ["left_sole", "right_sole"])
with pytest.raises(ValueError, match="one unique"):
foot_site_ids(model, ["right", "left"], ["right_sole", "right_sole"])
with pytest.raises(ValueError, match="one unique"):
foot_site_ids(model, ["right", "left"], ["right_sole"])
@pytest.mark.parametrize("ground", [
'type="box" size="2 2 .1"',
'type="plane" size="2 2 .1" euler="20 0 0"',
])
def test_sole_height_rejects_nonplane_and_tilted_ground(ground):
model = _sole_model(ground)
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
with pytest.raises(ValueError, match="plane"):
horizontal_ground_height(model, data, model.geom("ground").id)