Files
SkillFactory/tests/test_contact_cycle.py
T
chenlin 9fe9d27865 feat: checkpoint contact-cycle evaluation and RootXY PPO progress
Consolidate next-stage docs, archive superseded narratives, and preserve frozen document inputs. Retain invalid P2 pairs, partial RootXY training, failed attempts and nonselectable checkpoints as explicit limitations.

Include supervised evaluation/training, desktop GPU admission, bounded evidence and atomic-publication monitor repairs with CPU regressions. Ignore local dotenv configuration.

Validation: 1403 passed, 6 skipped for contact-cycle and related contracts/CLI tests. Full-suite legacy ABI and registration tests require Python 3.12/Torch 2.10/pxr; no runtime checks relaxed. No new physical run or accepted skill.
2026-10-15 14:18:17 +08:00

217 lines
8.9 KiB
Python

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