"""Initialization contract and diagnostic regression; no robot optimizer or Kit launch.""" from pathlib import Path import pytest import torch from SkillFactory.legs.contact_cycle.config import load_config, validate_config from SkillFactory.legs.contact_cycle.mechanics import ContactAcquisition, SeparationLatch from SkillFactory.legs.contact_cycle.ppo import rollout_diagnostics from SkillFactory.legs.contact_cycle.task import FAILURE_NAMES, phase_failures ROOT = Path(__file__).resolve().parents[1] def acquisition(count=1): return ContactAcquisition(count, 'cpu', deadline=.05, dwell=.01, min_load_fraction=.5, max_gap=.0005) def test_initial_uncontacted_flight_has_bounded_distinct_failure_not_premature(): a = acquisition() sep = SeparationLatch(1, 'cpu', .001, .0005, .01) zero, weight, phase = torch.zeros(1), torch.tensor([50.]), torch.tensor([0]) for k in range(1, 26): t, gap = torch.tensor([k*.002]), torch.tensor([.003]) a.update(t, gap, zero, weight, .002) edge = sep.update(gap, torch.tensor([False]), phase, .002) assert not edge.any() and not sep.premature.any() and not a.acquired.any() assert bool(a.failed[0]) == (k == 25) # Late contact cannot rewrite an initialization failure as a success. for k in range(26, 40): a.update(torch.tensor([k*.002]), zero, weight, weight, .002) assert a.failed.all() and not a.acquired.any() def test_on_plane_load_acquires_and_true_unload_departure_still_fails(): a, sep = acquisition(), SeparationLatch(1, 'cpu', .001, .0005, .01) weight, zero = torch.tensor([50.]), torch.zeros(1) for k in range(1, 6): a.update(torch.tensor([k*.002]), zero, weight, weight, .002) sep.update(zero, torch.tensor([True]), torch.tensor([0]), .002) assert a.acquired.all() and not a.failed.any() for _ in range(5): edge = sep.update(torch.tensor([.002]), torch.tensor([False]), torch.tensor([1]), .002) assert edge.all() and sep.premature.all() a.update(torch.tensor([3.5]), torch.tensor([.002]), zero, weight, .002) assert not a.failed.any() # acquisition is one-shot; departure has its own cause def test_acquisition_partial_reset_and_contiguous_dwell(): a = acquisition(2) w, z = torch.full((2,), 50.), torch.zeros(2) for k in range(1, 5): a.update(torch.full((2,), k*.002), z, w, w, .002) a.update(torch.full((2,), .01), z, torch.tensor([0., 50.]), w, .002) assert a.acquired.tolist() == [False, True] a.update(torch.full((2,), .05), z, torch.zeros(2), w, .002) assert a.failed.tolist() == [True, False] a.reset(torch.tensor([1])) assert a.failed.tolist() == [True, False] and not a.acquired.any() a.reset(torch.tensor([0])) assert not a.failed.any() def test_load_dwell_completed_at_deadline_is_not_rejected(): a = acquisition() for k in range(21, 26): a.update(torch.tensor([k*.002]), torch.zeros(1), torch.tensor([50.]), torch.tensor([50.]), .002) assert a.acquired.all() and not a.failed.any() @pytest.mark.parametrize('key,value', [('foot_clearance_m', .003), ('contact_deadline_s', .5), ('dwell_s', .3), ('min_load_fraction', .1)]) def test_initialization_config_rejects_airborne_or_unbounded_start(key, value): c = load_config(ROOT/'configs/contact_cycle/go2_fl_v1.json') c['initialization'][key] = value with pytest.raises(ValueError): validate_config(c) def test_separate_phase_failures_preserve_thresholds(): r = phase_failures(torch.tensor([2, 3, 5]), torch.ones(3), torch.tensor([0., 0., .05]), torch.tensor([0., 0., 0.]), torch.full((3,), 50.)) assert r['lift_deadline'].tolist() == [True, False, False] assert r['swing_clearance'].tolist() == [False, True, False] assert r['reload_deadline'].tolist() == [False, False, True] def test_diagnostic_aggregation_does_not_confuse_missing_samples_or_change_rollout(): flags = torch.zeros(2, 2, len(FAILURE_NAMES), dtype=torch.bool) flags[1, 0, FAILURE_NAMES.index('contact_acquisition_timeout')] = True data = {'phase': torch.tensor([[0, -1], [1, 2]]), 'failure_flags': flags, 'reward': torch.ones(2, 2), 'value': torch.zeros(2, 2), 'next_value': torch.zeros(2, 2), 'done': torch.tensor([[False, False], [True, False]])} copy = {k:v.clone() for k,v in data.items()} r = rollout_diagnostics(data, {'gamma': .99, 'gae_lambda': .95}) assert r['phase_control_samples'] == [1, 1, 1, 0, 0, 0] assert r['missing_diagnostic_samples'] == 1 assert r['terminal_causes']['contact_acquisition_timeout'] == 1 assert r['terminal_causes']['premature_separation'] == 0 assert all(torch.equal(copy[k], data[k]) for k in data)