ceca51b0d8
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.
103 lines
3.9 KiB
Python
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)
|