093ab883be
Add loaded-standing physics, PPO training, fixed-candidate acceptance, isolated inference, and fail-closed publication. Preserve the original 120-second drift failure and bind the operator-approved exception to the exact candidate and evidence. Include pending Reach training/release infrastructure and archived packages. Leave pre-existing short-worker visualization changes and large generated training artifacts out of this commit.
205 lines
7.8 KiB
Python
205 lines
7.8 KiB
Python
"""Pure configuration/mechanics oracles; these are NOT physical standing evidence."""
|
|
|
|
import copy
|
|
import json
|
|
import subprocess
|
|
import sys
|
|
import xml.etree.ElementTree as ET
|
|
from pathlib import Path
|
|
|
|
import pytest
|
|
import torch
|
|
from SkillFactory.legs.support import (
|
|
disturbance_wrench,
|
|
load_config,
|
|
load_support_asset,
|
|
negative_control_collapsed,
|
|
passive_wrench,
|
|
payload_urdf,
|
|
relative_point_velocity,
|
|
sha256,
|
|
shift_wrench,
|
|
)
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
CONFIG = ROOT / "configs/standing/go2_fl_support_v0.json"
|
|
|
|
|
|
@pytest.mark.parametrize("message,passed", [("normal completion", True), ("[Error] backend rejected a setting", False)])
|
|
def test_supervisor_rejects_critical_log_even_with_zero_child_exit(tmp_path, message, passed):
|
|
report = tmp_path / "process.json"
|
|
result = subprocess.run(
|
|
[
|
|
sys.executable,
|
|
str(ROOT / "scripts/supervise_sim.py"),
|
|
"--log",
|
|
str(tmp_path / "worker.log"),
|
|
"--report",
|
|
str(report),
|
|
"--",
|
|
sys.executable,
|
|
"-c",
|
|
f"print({message!r})",
|
|
],
|
|
capture_output=True,
|
|
text=True,
|
|
)
|
|
data = json.loads(report.read_text())
|
|
assert data["returncode"] == 0 and data["cleanup"]["clean"]
|
|
assert data["passed"] is passed and (result.returncode == 0) is passed
|
|
assert bool(data["critical_log_lines"]) is not passed
|
|
|
|
|
|
def test_support_config():
|
|
cfg = load_config(CONFIG)
|
|
assert cfg["payload_mass_kg"] == 3
|
|
for key in ("guide_stiffness", "guide_damping", "guide_wrench_limit"):
|
|
assert cfg[key][2] == 0
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"key,value",
|
|
[
|
|
("payload_mass_kg", 0),
|
|
("payload_mass_kg", True),
|
|
("payload_mass_kg", float("nan")),
|
|
("guide_stiffness", [1, 1, 1, 1, 1, 1]),
|
|
("guide_damping", [1, 1, 1, 1, 1, 1]),
|
|
("guide_wrench_limit", [1, 1, 1, 1, 1, 1]),
|
|
("guide_stiffness", [-1, 0, 0, 0, 0, 0]),
|
|
("payload_size_m", [0.1, 0.1]),
|
|
("payload_size_m", [0, 0.1, 0.1]),
|
|
("disturbance_amplitude", [5, 5, 10, 1, 1, 0]),
|
|
("contact_fraction_min", 1.01),
|
|
("episode_seconds", 5),
|
|
("seed", 1.5),
|
|
("unknown", 42),
|
|
],
|
|
)
|
|
def test_bad_config_rejected(tmp_path, key, value):
|
|
data = json.loads(CONFIG.read_text())
|
|
data[key] = value
|
|
p = tmp_path / "config.json"
|
|
p.write_text(json.dumps(data))
|
|
with pytest.raises(ValueError):
|
|
load_config(p)
|
|
|
|
|
|
def test_derived_payload_is_separate_and_preserves_original_mechanics(tmp_path):
|
|
# Synthetic URDF fixture, independently specified root and child.
|
|
source = tmp_path / "source.urdf"
|
|
source.write_text(
|
|
'<robot name="fixture"><link name="anchor_base"><inertial><mass value="1"/>'
|
|
'<inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/>'
|
|
'</inertial></link><link name="limb"/><joint name="actuator" type="revolute">'
|
|
'<parent link="anchor_base"/><child link="limb"/><axis xyz="0 1 0"/></joint></robot>'
|
|
)
|
|
before = source.read_bytes()
|
|
cfg = load_config(CONFIG)
|
|
tree = ET.fromstring(payload_urdf(source, cfg))
|
|
assert source.read_bytes() == before
|
|
sensor = tree.find("joint[@name='mount_ft_joint']")
|
|
assert sensor.get("type") == "fixed" and sensor.get("dont_collapse") == "true"
|
|
assert sensor.find("parent").get("link") == "virtual_payload"
|
|
assert sensor.find("child").get("link") == "anchor_base"
|
|
children = {j.find("child").get("link") for j in tree.findall("joint")}
|
|
assert {link.get("name") for link in tree.findall("link")} - children == {"virtual_payload"}
|
|
assert tree.find("link[@name='anchor_base']/inertial/mass").get("value") == "0.05"
|
|
inertia = tree.find("link[@name='virtual_payload']/inertial/inertia")
|
|
assert float(inertia.get("ixx")) == pytest.approx(3 * (0.18**2 + 0.08**2) / 12)
|
|
|
|
def structure(node):
|
|
return node.tag, node.attrib, tuple(structure(child) for child in node)
|
|
|
|
assert structure(tree.find("joint[@name='actuator']")) == structure(
|
|
ET.fromstring(before).find("joint[@name='actuator']")
|
|
)
|
|
|
|
|
|
def test_bushing_passivity_and_no_vertical_hanging():
|
|
cfg = load_config(CONFIG)
|
|
k, d, limit = [torch.tensor(cfg[x]) for x in ("guide_stiffness", "guide_damping", "guide_wrench_limit")]
|
|
torch.manual_seed(7)
|
|
velocity = torch.randn(64, 6)
|
|
damping = passive_wrench(torch.zeros_like(velocity), velocity, k, d, limit)
|
|
assert ((damping * velocity).sum(-1) <= 0).all()
|
|
displacement = torch.randn(64, 6)
|
|
restoring = passive_wrench(displacement, torch.zeros_like(displacement), k, d, limit)
|
|
assert ((restoring * displacement).sum(-1) <= 0).all()
|
|
assert torch.equal(restoring[:, 2], torch.zeros(64))
|
|
assert (restoring.abs() <= limit).all()
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"time,amplitude", [(0, 0), (3, 0), (3.25, 0.5), (4, 1), (5.75, 0.5), (6, 0), (7, 0), (9, 1), (12, 0)]
|
|
)
|
|
def test_all_thirteen_signed_disturbance_cases(time, amplitude):
|
|
amps = torch.tensor([5.0, 5.0, 10.0, 1.0, 1.0, 1.0])
|
|
result = disturbance_wrench(torch.full((13,), float(time)), torch.arange(13), amps)
|
|
expected = torch.zeros(13, 6)
|
|
for axis in range(6):
|
|
expected[1 + 2 * axis, axis] = amplitude * amps[axis]
|
|
expected[2 + 2 * axis, axis] = -amplitude * amps[axis]
|
|
torch.testing.assert_close(result, expected, rtol=0, atol=0)
|
|
|
|
|
|
def test_rotating_frame_relative_velocity_and_wrench_origin():
|
|
r = torch.tensor([[1.0, 0, 0]])
|
|
omega = torch.tensor([[0.0, 0, 2.0]])
|
|
v_root = torch.tensor([[3.0, 0, 0]])
|
|
# A point rigidly attached to a rotating root has zero relative velocity.
|
|
v_point = v_root + torch.cross(omega, r, dim=-1)
|
|
torch.testing.assert_close(relative_point_velocity(v_point, v_root, omega, r), torch.zeros(1, 3))
|
|
force, torque = shift_wrench(torch.tensor([[0.0, 5, 0]]), torch.zeros(1, 3), r)
|
|
torch.testing.assert_close(torque, torch.tensor([[0.0, 0, 5]]))
|
|
torch.testing.assert_close(force, torch.tensor([[0.0, 5, 0]]))
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"drops,torques,expected",
|
|
[
|
|
([0.2, 0.3], [0, 0], True),
|
|
([0.0, 0.3], [0, 0], False),
|
|
([0.09, 0.3], [0, 0], False),
|
|
([0.2, 0.3], [0.1, 0], False),
|
|
([], [], False),
|
|
],
|
|
)
|
|
def test_negative_control_requires_downward_collapse_of_all_envs(drops, torques, expected):
|
|
rows = [
|
|
{"max_base_drop_m": drop, "max_normalized_torque": torque, "max_height_error_m": 0.5}
|
|
for drop, torque in zip(drops, torques)
|
|
]
|
|
assert negative_control_collapsed(rows) is expected
|
|
|
|
|
|
def test_support_identity_and_path_integrity(tmp_path):
|
|
cfg = copy.deepcopy(load_config(CONFIG))
|
|
source = tmp_path / "source"
|
|
source.mkdir()
|
|
(source / "manifest.json").write_text("source identity")
|
|
cfg["source_bundle"] = str(source)
|
|
asset = tmp_path / "support"
|
|
asset.mkdir()
|
|
(asset / "a.usda").write_text("fixture, not an actual USD")
|
|
manifest = {
|
|
"contract": "go2-fl-loaded-stand-v0",
|
|
"fixed_to_world": False,
|
|
"mount_joint": "mount_ft_joint",
|
|
"usd_path": "a.usda",
|
|
"source_manifest_sha256": sha256(source / "manifest.json"),
|
|
"files": {"a.usda": sha256(asset / "a.usda")},
|
|
**{k: cfg[k] for k in ("payload_mass_kg", "payload_size_m", "payload_com_m", "sensor_carrier_mass_kg")},
|
|
}
|
|
path = asset / "support.json"
|
|
path.write_text(json.dumps(manifest))
|
|
assert load_support_asset(asset, cfg)["fixed_to_world"] is False
|
|
cfg["payload_mass_kg"] = 4
|
|
with pytest.raises(ValueError, match="config mismatch"):
|
|
load_support_asset(asset, cfg)
|
|
cfg["payload_mass_kg"] = 3
|
|
manifest["files"] = {"../source/manifest.json": sha256(source / "manifest.json")}
|
|
path.write_text(json.dumps(manifest))
|
|
with pytest.raises(ValueError, match="integrity"):
|
|
load_support_asset(asset, cfg)
|