Files
SkillFactory/tests/test_loaded_leg.py
T
chenlin 093ab883be feat: release guided Go2 standing skill with explicit drift waiver
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.
2026-10-12 17:26:03 +08:00

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)