9fe9d27865
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.
217 lines
8.9 KiB
Python
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)
|