Files
SkillFactory/tests/test_four_skills_forces.py

209 lines
9.5 KiB
Python

"""CPU synthetic schedule tests, not simulator, hardware or task-success proof."""
import importlib.util
import json
import math
from pathlib import Path
import random
import pytest
import torch
PATH = (Path(__file__).resolve().parents[1]
/ "source/SkillFactory/SkillFactory/legs/four_skills/forces.py")
SPEC = importlib.util.spec_from_file_location("four_skills_forces", PATH)
MODULE = importlib.util.module_from_spec(SPEC)
SPEC.loader.exec_module(MODULE)
ForceSchedule = MODULE.ForceSchedule
def schedule(**kwargs):
options = dict(seed=137, env_ids=[10, 20, 30], tier="full", device="cpu")
options.update(kwargs)
return ForceSchedule(**options)
def test_paired_analytic_trained_schedules_are_identical_and_order_independent():
analytic = schedule(episode_ids=[3, 4, 5])
trained = schedule(episode_ids=[3, 4, 5])
assert analytic.manifest() == trained.manifest()
for t in torch.linspace(0, 300, 501, dtype=torch.float64):
assert torch.equal(analytic.at(t.repeat(3)), trained.at(t.repeat(3)))
reordered = schedule(env_ids=[30, 10, 20], episode_ids=[5, 3, 4])
rows = analytic.manifest()["rows"]
assert reordered.manifest()["rows"] == [rows[2], rows[0], rows[1]]
alone = schedule(env_ids=[20], episode_ids=[4])
assert alone.manifest()["rows"] == [rows[1]]
assert schedule(seed=138).manifest()["rows"] != schedule().manifest()["rows"]
def test_reset_order_subset_and_tiers_preserve_unselected_rows():
first, second = schedule(), schedule()
before = first.manifest()["rows"]
first.reset([2, 0], [9, 7], tier="small")
second.reset([0], [7], tier="small")
second.reset([2], [9], tier="small")
assert first.manifest() == second.manifest()
assert first.manifest()["rows"][1] == before[1]
expected = schedule(env_ids=[10, 30], episode_ids=[7, 9], tier="small")
assert [first.manifest()["rows"][i] for i in [0, 2]] == expected.manifest()["rows"]
first.reset([0], [8])
assert first.manifest()["rows"][0]["tier"] == "small"
first.reset([0], [7])
assert first.manifest() == second.manifest()
for t in [0.4, 8.5, 32.9, 99.4]:
assert torch.equal(first.at(torch.full((3,), t)), second.at(torch.full((3,), t)))
@pytest.mark.parametrize("tier,maximum", [("none", 0), ("small", 1.5), ("medium", 4), ("full", 8)])
def test_event_bounds_nonoverlap_and_force_bounds(tier, maximum):
s = schedule(tier=tier)
manifest = s.manifest()
assert manifest["tier_maxima_N"] == dict(none=0, small=1.5, medium=4, full=8)
assert manifest["frame"] == "world"
assert manifest["application_point"] == "payload_com"
assert manifest["requested_couple_Nm"] == [0, 0, 0]
for row in manifest["rows"]:
previous_end = -1
for e in row["events"]:
assert 0.2 - 1e-12 <= e["onset_s"] - e["slot_start_s"] <= 0.5 + 1e-12
assert 0.5 <= e["duration_s"] <= 1.3
assert 0.25 * maximum <= e["amplitude_N"] <= maximum
assert math.isclose(sum(v*v for v in e["direction_world"]), 1, abs_tol=1e-14)
assert previous_end < e["onset_s"]
assert e["end_s"] < e["slot_start_s"] + 2
previous_end = e["end_s"]
for t in torch.linspace(0, 300, 1001):
force = s.at(t.repeat(3))
assert force.shape == (3, 3) and force.dtype == torch.float32
assert force.device.type == "cpu"
assert bool((force.norm(dim=1) <= maximum + 1e-6).all())
def test_isotropic_directions_active_probability_and_uniform_sampling_sanity():
# Fixed seed, broad statistical checks: implementation still defines the distribution.
s = schedule(env_ids=list(range(40)))
events = [e for r in s.manifest()["rows"] for e in r["events"]]
directions = torch.tensor([e["direction_world"] for e in events], dtype=torch.float64)
assert bool((directions.mean(0).abs() < 0.035).all())
assert bool(((directions.square().mean(0) - 1/3).abs() < 0.025).all())
assert 0.77 < sum(e["active"] for e in events) / len(events) < 0.83
assert abs(sum(e["amplitude_N"] for e in events) / len(events) - 5) < 0.12
def test_raised_cosine_values_ramp_bound_inactive_and_gaps():
s = schedule(env_ids=[10])
events = s.manifest()["rows"][0]["events"]
active = next(e for e in events if e["active"])
inactive = next(e for e in events if not e["active"])
peak = torch.tensor(active["peak_force_world_N"], dtype=torch.float32)
start, end = active["onset_s"], active["end_s"]
for t, scale in [(start, 0), (start + .075, .5), (start + .15, 1),
(end - .15, 1), (end - .075, .5), (end, 0)]:
torch.testing.assert_close(s.at(torch.tensor([t], dtype=torch.float64))[0],
scale * peak, atol=1e-6, rtol=1e-6)
for t in [0, active["slot_start_s"], end + .05,
inactive["onset_s"] + .2, 300]:
assert torch.equal(s.at(torch.tensor([t], dtype=torch.float64)), torch.zeros(1, 3))
# Sample both ramps at high resolution, measuring vector (not component) slope.
times = torch.cat([torch.linspace(start - .01, start + .16, 1701, dtype=torch.float64),
torch.linspace(end - .16, end + .01, 1701, dtype=torch.float64)])
values = torch.stack([s.at(t.reshape(1))[0] for t in times])
for segment in [slice(0, 1701), slice(1701, 3402)]:
rates = values[segment].diff(dim=0).norm(dim=1) / times[segment].diff()
assert rates.max() <= 100
assert s.manifest()["maximum_force_derivative_N_per_s"] < 100
def test_no_global_rng_mutation_on_construct_reset_or_query():
python_state = random.getstate()
torch_state = torch.random.get_rng_state().clone()
s = schedule()
s.reset([1, 0], [5, 9], tier="medium")
s.at(torch.tensor([1., 2., 3.]))
s.manifest()
assert random.getstate() == python_state
assert torch.equal(torch.random.get_rng_state(), torch_state)
def test_manifest_is_complete_detached_and_reconstructs_force():
s = schedule(duration_s=3.1)
manifest = json.loads(json.dumps(s.manifest(), allow_nan=False))
for i, row in enumerate(manifest["rows"]):
assert len(row["events"]) == 2
for event in row["events"]:
assert event["truncated_by_horizon"] == (event["end_s"] > 3.1)
assert event["onset_beyond_horizon"] == (event["onset_s"] > 3.1)
t = min(3.1, event["onset_s"] + .1)
edge = max(0, min(.15, t - event["onset_s"], event["end_s"] - t))
scale = .5 * (1 - math.cos(math.pi * edge / .15))
expected = torch.tensor(event["peak_force_world_N"]) * scale
result = s.at(torch.full((3,), t, dtype=torch.float64))[i]
torch.testing.assert_close(result, expected, atol=1e-6, rtol=1e-6)
manifest["rows"][0]["events"][0]["amplitude_N"] = -1
assert s.manifest()["rows"][0]["events"][0]["amplitude_N"] >= 2
short = schedule(duration_s=.1)
assert all(r["events"][0]["onset_beyond_horizon"] for r in short.manifest()["rows"])
assert torch.equal(short.at(torch.full((3,), .1, dtype=torch.float64)), torch.zeros(3, 3))
s.at(torch.full((3,), 3.1, dtype=torch.float64))
with pytest.raises(ValueError, match="exceeds"):
s.at(torch.full((3,), 3.10000001, dtype=torch.float64))
@pytest.mark.parametrize("kwargs,error", [
({"seed": True}, TypeError), ({"seed": 1.0}, TypeError),
({"env_ids": [1, True]}, TypeError), ({"env_ids": [1., 2]}, TypeError),
({"env_ids": [1, 1]}, ValueError), ({"env_ids": "12"}, TypeError),
({"episode_ids": [1]}, ValueError), ({"episode_ids": [0, 0, False]}, TypeError),
({"tier": "large"}, ValueError), ({"tier": []}, ValueError),
({"duration_s": True}, ValueError), ({"duration_s": "300"}, ValueError),
({"duration_s": 0}, ValueError), ({"duration_s": -1}, ValueError),
({"duration_s": float("nan")}, ValueError), ({"duration_s": float("inf")}, ValueError),
])
def test_constructor_bad_inputs(kwargs, error):
with pytest.raises(error):
schedule(**kwargs)
@pytest.mark.parametrize("time,error", [
([0, 0, 0], TypeError), (torch.zeros(3, 1), ValueError),
(torch.zeros(2), ValueError), (torch.tensor([0, 0, -1]), ValueError),
(torch.tensor([0., float("nan"), 0]), ValueError),
(torch.tensor([0., float("inf"), 0]), ValueError),
(torch.tensor([0, 301, 0]), ValueError), (torch.ones(3, dtype=torch.bool), TypeError),
(torch.zeros(3, dtype=torch.complex64), TypeError),
])
def test_at_bad_inputs(time, error):
with pytest.raises(error):
schedule().at(time)
@pytest.mark.parametrize("ids,episodes,tier,error", [
([0, 0], [1, 2], None, ValueError), ([3], [0], None, ValueError),
([-1], [0], None, ValueError), ([True], [0], None, TypeError),
([1.0], [0], None, TypeError), ([0], [True], None, TypeError),
([0], [], None, ValueError), ([0], [0], "unknown", ValueError),
([0, 9], [1, 2], "small", ValueError),
])
def test_reset_bad_inputs_do_not_mutate(ids, episodes, tier, error):
s = schedule()
before = s.manifest()
with pytest.raises(error):
s.reset(ids, episodes, tier)
assert s.manifest() == before
def test_empty_batch_empty_reset_and_integer_times():
empty = schedule(env_ids=[])
assert empty.at(torch.empty(0)).shape == (0, 3)
empty.reset([], [])
assert empty.manifest()["rows"] == []
s = schedule()
before = s.manifest()
s.reset([], [], tier="none")
assert s.manifest() == before
assert s.at(torch.zeros(3, dtype=torch.int64)).shape == (3, 3)
# No unspecified nonnegative constraint on integer identity keys.
assert schedule(seed=-1, env_ids=[-10], episode_ids=[-3]).manifest()["seed"] == -1