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.
118 lines
5.7 KiB
Python
118 lines
5.7 KiB
Python
"""Reset discontinuities and contact chatter must remain visible in diagnostics."""
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from tools.checks.diagnose_locomotion_policy import contact_events, summarize_samples
|
|
|
|
|
|
def _samples():
|
|
target = np.array([0., 0., 100., 0., 2., 2.]).reshape(6, 1, 1)
|
|
done = np.zeros((6, 1), dtype=bool)
|
|
done[2] = True
|
|
velocity = np.full((6, 1, 3), .2)
|
|
velocity[2] = 999.
|
|
reward = np.full((6, 1, 1), 1.)
|
|
reward[2] = 999.
|
|
root = np.zeros((6, 1, 3))
|
|
root[..., 2] = 1.1
|
|
root[2, :, 2] = .2
|
|
return dict(action=target.copy(), target=target, qpos=target.copy(), torque=target.copy(),
|
|
dones=done, contacts=np.ones((6, 1, 2), dtype=bool),
|
|
height=np.zeros((6, 1, 2)), velocity=velocity, rewards=reward,
|
|
root_height=root[..., 2], root_link_pos_w=root)
|
|
|
|
|
|
def test_reset_frames_are_excluded_but_real_target_jumps_remain():
|
|
sample = _samples()
|
|
sample['angular_velocity'] = np.full((6, 1, 3), .3)
|
|
sample['angular_velocity'][2] = 999.
|
|
result = summarize_samples(sample, .02, ['joint'], np.array([10.]), ['reward'],
|
|
np.array([.2, .2, 0.]))
|
|
assert result['valid_samples'] == 5 and result['excluded_reset_samples'] == 1
|
|
assert result['valid_target_difference_samples'] == 3
|
|
# The real 0->2 rad action transition remains; 0->100->0 reset edges vanish.
|
|
assert result['joints']['joint']['target_speed_rms_rad_s'] == pytest.approx(100 / np.sqrt(3))
|
|
assert result['joints']['joint']['qpos']['max'] == 2.
|
|
assert result['joints']['joint']['torque_saturation_fraction'] == 0.
|
|
assert result['reward_rate']['reward'] == 1.
|
|
assert result['mean_velocity'] == pytest.approx([.2, .2, .2])
|
|
assert result['mean_velocity_axes'] == ['vx', 'vy', 'vz']
|
|
assert result['mean_angular_velocity'] == pytest.approx([.3, .3, .3])
|
|
assert result['mean_yaw_tracking_error'] == pytest.approx(.3)
|
|
assert result['mean_height'] == pytest.approx(1.1)
|
|
assert result['root_height_m']['min'] == pytest.approx(1.1)
|
|
# A control without resets must detect the large real jump, not suppress it.
|
|
sample['dones'][:] = False
|
|
control = summarize_samples(sample, .02, ['joint'], np.array([10.]), ['reward'],
|
|
np.array([.2, .2, 0.]))
|
|
assert control['joints']['joint']['target_speed_rms_rad_s'] > 3000.
|
|
assert control['joints']['joint']['qpos']['max'] == 100.
|
|
assert control['joints']['joint']['torque_saturation_fraction'] > 0.
|
|
|
|
|
|
def test_stance_slip_ignores_swing_and_reset_samples():
|
|
sample = _samples()
|
|
sample['sole_velocity_w'] = np.zeros((6, 1, 2, 3))
|
|
sample['sole_velocity_w'][..., 0] = .03
|
|
sample['sole_velocity_w'][2] = 999. # Reset frame.
|
|
sample['contacts'][3, 0, 1] = False
|
|
sample['sole_velocity_w'][3, 0, 1] = 100. # Airborne foot.
|
|
result = summarize_samples(sample, .02, ['joint'], np.array([10.]), ['reward'],
|
|
np.array([.2, .2, 0.]))
|
|
assert result['stance_sole_horizontal_speed_m_s']['max'] == pytest.approx(.03)
|
|
assert result['joints']['joint']['qpos']['per_env_p95_p05_excursion_rad'] == [2.]
|
|
|
|
|
|
def test_reset_aborts_swings_and_same_foot_periods_do_not_bridge_episodes():
|
|
contacts = np.ones((14, 1, 2), dtype=bool)
|
|
contacts[[1, 2, 4, 5, 7, 8, 11, 12], 0, 0] = False
|
|
dones = np.zeros((14, 1), dtype=bool)
|
|
dones[5] = True
|
|
height = np.where(contacts, 0., .03)
|
|
result = contact_events(contacts, height, dones, .02)
|
|
assert result['completed_swings_per_env_foot'] == [[3, 0]]
|
|
summary = result['contact_event_summary']
|
|
assert summary['complete_event_count'] == 3
|
|
assert summary['airtime_s']['median'] == pytest.approx(.04)
|
|
assert summary['same_foot_landing_period_s']['min'] == pytest.approx(.08)
|
|
assert summary['same_foot_landing_period_s']['max'] == pytest.approx(.08)
|
|
assert summary['events_airtime_ge_0_1s_and_height_ge_0_02m'] == 0
|
|
|
|
|
|
@pytest.mark.parametrize('dt', [.01, .02])
|
|
def test_descriptive_threshold_does_not_change_existing_40ms_count(dt):
|
|
# One 40ms shallow event and one 120ms higher event; include leading and
|
|
# trailing truncated air phases, neither of which is a complete swing.
|
|
states = [False, True] + [False] * round(.04 / dt) + [True, True] \
|
|
+ [False] * round(.12 / dt) + [True, True, False]
|
|
contacts = np.ones((len(states), 1, 2), dtype=bool)
|
|
contacts[:, 0, 0] = states
|
|
heights = np.where(contacts, 0., .01)
|
|
start = 4 + round(.04 / dt)
|
|
heights[start:start + round(.12 / dt), 0, 0] = .03
|
|
result = contact_events(contacts, heights, np.zeros((len(states), 1), dtype=bool), dt)
|
|
assert result['completed_swings_per_env_foot'] == [[2, 0]]
|
|
summary = result['contact_event_summary']
|
|
assert summary['complete_event_count'] == 2
|
|
assert summary['airtime_s']['min'] == pytest.approx(.04)
|
|
assert summary['airtime_s']['max'] == pytest.approx(.12)
|
|
assert summary['events_airtime_ge_0_1s_and_height_ge_0_02m'] == 1
|
|
|
|
|
|
def test_one_frame_chatter_is_described_but_not_counted_as_40ms_swing():
|
|
contacts = np.ones((3, 1, 2), dtype=bool)
|
|
contacts[1, 0, 0] = False
|
|
result = contact_events(contacts, np.where(contacts, 0., .005),
|
|
np.zeros((3, 1), dtype=bool), .02)
|
|
assert result['completed_swings_per_env_foot'] == [[0, 0]]
|
|
assert result['contact_event_summary']['complete_event_count'] == 1
|
|
assert result['contact_event_summary']['airtime_s']['median'] == .02
|
|
assert result['contact_event_summary']['same_foot_landing_period_s'] is None
|
|
|
|
|
|
def test_no_usable_samples_fails_explicitly():
|
|
sample = _samples()
|
|
sample['dones'][:] = True
|
|
with pytest.raises(ValueError, match='No non-reset samples'):
|
|
summarize_samples(sample, .02, ['joint'], [10.], ['reward'], np.zeros(3))
|