Files
UniversalTrain-ai/tests/test_locomotion_policy_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

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))