"""CPU contracts, not physical evidence. Kit runs are separately supervised and archived.""" import copy import json from pathlib import Path import pytest import torch from SkillFactory.legs.contact_cycle.config import load_config, require_scenario_coverage, validate_config from SkillFactory.legs.contact_cycle.mechanics import ( ContactLatch, WrenchSchedule, cone_violation, reduce_to_com, rotate, uniform_directions, ) from SkillFactory.legs.contact_cycle.metrics import wilson from SkillFactory.legs.contact_cycle.task import DelayLine, command, phase_objective ROOT = Path(__file__).resolve().parents[1] CONFIG = ROOT / "configs/contact_cycle/go2_fl_v1.json" def test_config_and_unchanged_standing_vertical_support(): c = load_config(CONFIG) assert c["contract"] == "go2-fl-contact-cycle-v1" assert json.loads((ROOT/c["support_config"]).read_text())["guide_stiffness"][2] == 0 plan = json.loads((ROOT/"configs/contact_cycle/acceptance_v1.json").read_text()) assert plan["proposed_training"]["authorization"] is None @pytest.mark.parametrize("key,value", [ ("clearance_m", float("nan")), ("clearance_m", True), ("unknown", 1), ("phase_durations_s", [1]*5), ("swing_displacement_m", [0, 0, 0.01]), ("schema_version", 2), ("base_motion", {"amplitude_m": [0, 0, 1], "frequency_Hz": 0.2}), ]) def test_invalid_config(key, value): c = load_config(CONFIG) c[key] = value with pytest.raises(ValueError): validate_config(c) @pytest.mark.parametrize("key,value", [ ("ramp_s", 0), ("duration_s", 0.5), ("frame", "camera"), ("body", "trunk"), ("force_N", [8, 8, 8]), ("couple_Nm", [1, 1, 1]), ("point_m", [0.2, 0, 0]), ("trigger", "invalid"), ("phase", True), ("scenario", 0), ("start_s", 60.0), ]) def test_invalid_events(key, value): c = load_config(CONFIG) c["events"][0][key] = value with pytest.raises(ValueError): validate_config(c) def test_duplicate_json_rejected(tmp_path): p = tmp_path/"bad.json" p.write_text('{"schema_version": 1, "schema_version": 1}') with pytest.raises(ValueError, match="Duplicate"): load_config(p) def test_wrench_rotates_force_and_couple_shifts_once(): q = torch.tensor([[0, 0, 2**-0.5, 2**-0.5]]) f, tau = torch.tensor([[2., 0, 0]]), torch.tensor([[0., 1, 0]]) point, com = torch.tensor([[1., 0, 0]]), torch.zeros(1, 3) w = reduce_to_com(f, tau, q, point, com) torch.testing.assert_close(w, torch.tensor([[0., 2, 0, -1, 0, 2]]), atol=1e-6, rtol=1e-6) # Rotation + translation preserves mechanical power when twisting about corresponding origins. omega, v_com = torch.tensor([[0.1, -0.2, 0.3]]), torch.tensor([[0.2, 0.3, 0.1]]) v_point = v_com + torch.cross(omega, point-com, dim=-1) p1 = (rotate(q, f)*v_point).sum() + (rotate(q, tau)*omega).sum() p2 = (w[:, :3]*v_com).sum() + (w[:, 3:]*omega).sum() torch.testing.assert_close(p1, p2) def test_superposition_and_zero_force_pure_couple(): q = torch.tensor([[0., 0, 0, 1]]) com, point = torch.zeros(1, 3), torch.ones(1, 3) f, t = torch.tensor([[1., 2, 3]]), torch.tensor([[3., 1, 2]]) a = reduce_to_com(f, t, q, point, com) b = reduce_to_com(-f, -t, q, point, com) assert (a+b == 0).all() torch.testing.assert_close(reduce_to_com(com, t, q, point, com)[:, 3:], t) def test_continuous_directions_not_axis_grid(): d = uniform_directions(10000, torch.Generator().manual_seed(13)) torch.testing.assert_close(d.norm(dim=-1), torch.ones(10000)) assert (d.mean(0).abs() < 0.02).all() assert ((d > 0).any(0) & (d < 0).any(0)).all() assert (d.abs() > 0.001).all(-1).float().mean() > 0.99 def test_unilateral_and_friction_are_not_external_force_constraints(): f = torch.tensor([[3., 4, 10], [0., 0, -3], [9., 0, 10]]) n = torch.tensor([[0., 0, 1.]]).expand_as(f) u, friction = cone_violation(f, n, 0.6) torch.testing.assert_close(u, torch.tensor([0., 3, 0])) torch.testing.assert_close(friction, torch.tensor([0., 0, 3])) def test_schedule_lifecycle_reset_no_refire_and_rate(): e = copy.deepcopy(load_config(CONFIG)["events"][0]) e.update(start_s=0.1, duration_s=0.4, ramp_s=0.1) bank = WrenchSchedule([e], 2, "cpu") phase, edge, scenario = torch.zeros(2).long(), torch.zeros(2).bool(), torch.ones(2).long() out = [] for k in range(71): out.append(bank.step(torch.full((2,), k*0.01), phase, edge, edge, scenario)) out = torch.stack(out)[:, 0, 0] assert (out[:11] == 0).all() and (out[50:] == 0).all() torch.testing.assert_close(out[20], torch.tensor(e["force_N"]+e["couple_Nm"])) assert ((out[1:, :3]-out[:-1, :3]).norm(dim=-1)/0.01 <= 54).all() bank.reset(torch.tensor([0])) assert not bank.fired[0].any() and bank.fired[1].all() torch.testing.assert_close(bank.start[1], torch.tensor([0.1])) def test_contact_event_phase_filter_delay_and_one_shot(): e = copy.deepcopy(load_config(CONFIG)["events"][-1]) e.update(start_s=0.1) b = WrenchSchedule([e], 1, "cpu") yes, no = torch.tensor([True]), torch.tensor([False]) sc = torch.tensor([3]) assert not b.step(torch.tensor([0.]), torch.tensor([0]), no, yes, sc).any() assert not b.fired.any() # initial settling contact cannot consume landing event assert not b.step(torch.tensor([6.]), torch.tensor([4]), no, yes, sc).any() assert b.fired.all() assert b.step(torch.tensor([6.2]), torch.tensor([4]), no, no, sc).abs().sum() > 0 assert not b.step(torch.tensor([7.]), torch.tensor([4]), no, yes, sc).any() def test_contact_hysteresis_and_partial_reset(): c = ContactLatch(2, "cpu", dwell=0.01) for _ in range(4): off, on = c.update(torch.tensor([3., 3]), 0.002) assert not on.any() _, on = c.update(torch.tensor([3., 3]), 0.002) assert on.all() for _ in range(20): off, on = c.update(torch.tensor([1., 1]), 0.002) assert not off.any() and not on.any() c.reset(torch.tensor([0])) assert c.contact.tolist() == [False, True] for _ in range(5): off, _ = c.update(torch.tensor([0., 0]), 0.002) assert off.tolist() == [False, True] def test_task_continuity_clearance_load_and_phase_deadlines(): cfg = load_config(CONFIG) origin, weight = torch.zeros(1, 3), torch.tensor([50.]) time = 0 for duration in cfg["phase_durations_s"][:-1]: time += duration a = command(torch.tensor([time-1e-5]), cfg, origin, weight) b = command(torch.tensor([time+1e-5]), cfg, origin, weight) assert (a[2]-b[2]).abs().max() < 1e-5 assert (a[3]-b[3]).abs().max() < 0.001 assert (a[4]-b[4]).abs().max() < 0.001 p, _, target, force, fraction = command(torch.tensor([5.2]), cfg, origin, weight) assert p == 3 and target[0, 2] >= 0.05 and force == 0 and fraction == 1 p, _, _, force, fraction = command(torch.tensor([10.]), cfg, origin, weight) assert p == 5 and force == 50 and fraction == 0 def test_reward_cannot_hide_in_air_or_stay_on_ground(): phase, progress = torch.tensor([3, 3, 5, 5]), torch.ones(4) reward, fail = phase_objective(phase, progress, torch.zeros(4), torch.tensor([0., 0.05, 0.05, 0.]), torch.tensor([50., 0, 0, 50]), torch.tensor([0., 0, 50, 50]), torch.zeros(4), torch.zeros(4), torch.full((4,), 50.)) assert reward[1] > reward[0] and reward[3] > reward[2] assert fail.tolist() == [True, False, True, False] def test_delay_line_latency_and_reset_isolation(): d = DelayLine(2, 1, 2, "cpu") assert not d.push(torch.ones(2, 1)).any() assert not d.push(2*torch.ones(2, 1)).any() assert (d.push(3*torch.ones(2, 1)) == 1).all() old = d.data[:, 1].clone() d.reset(torch.tensor([0]), torch.tensor([[7.]])) assert (d.data[:, 0] == 7).all() and torch.equal(old, d.data[:, 1]) def test_more_configured_scenarios_cannot_be_silently_omitted(): c = load_config(CONFIG) c["scenarios"].append("fifth") validate_config(c) with pytest.raises(ValueError, match="every configured"): require_scenario_coverage(c, 4) require_scenario_coverage(c, 5) def test_event_fired_is_not_delivered_or_completed(): e = copy.deepcopy(load_config(CONFIG)["events"][0]) e.update(trigger="phase", phase=0, start_s=0.2, duration_s=0.4, ramp_s=0.1) b = WrenchSchedule([e], 1, "cpu") phase, sc, edge = torch.tensor([0]), torch.tensor([1]), torch.tensor([False]) b.step(torch.tensor([0.]), phase, edge, edge, sc, 0.01) assert b.fired.all() and not b.integral.any() and not b.completed.any() for k in range(1, 70): b.step(torch.tensor([k*0.01]), phase, edge, edge, sc, 0.01) assert b.completed.all() torch.testing.assert_close(b.integral, torch.tensor([[0.3]])) b.reset(torch.tensor([0])) assert not b.integral.any() and not b.completed.any() def test_wilson_and_no_claim_from_one_trial(): assert wilson(1, 1)[0] < 0.21 assert wilson(96, 100)[0] > 0.90 with pytest.raises(ValueError): wilson(0, 0)