7f39abb613
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
215 lines
9.0 KiB
Python
215 lines
9.0 KiB
Python
"""Declarative extraction tests, including non-Go2 names and arbitrary axes."""
|
|
|
|
import dataclasses
|
|
import json
|
|
import math
|
|
import xml.etree.ElementTree as ET
|
|
from pathlib import Path
|
|
|
|
import pytest
|
|
from SkillFactory.legs import JointSpec, LegSpec
|
|
from SkillFactory.legs.spec import load_bundle
|
|
from SkillFactory.legs.urdf import extract_leg, forward, multiply, pose
|
|
|
|
ROOT = Path(__file__).parents[1]
|
|
|
|
|
|
def fixture_spec(tmp_path, n=2, box=False):
|
|
robot = ET.Element("robot", name="fixture")
|
|
for name in ["mount", *(f"segment{i}" for i in range(n)), "toe"]:
|
|
link = ET.SubElement(robot, "link", name=name)
|
|
link.append(
|
|
ET.fromstring('<inertial><mass value="0.2"/><inertia ixx="0.001" iyy="0.001" izz="0.001"/></inertial>')
|
|
)
|
|
if name == "toe":
|
|
link.append(
|
|
ET.fromstring(
|
|
'<collision><origin xyz="0.01 0 0" rpy="0 0.2 0"/><geometry>'
|
|
+ ('<box size="0.03 0.04 0.05"/>' if box else '<sphere radius="0.02"/>')
|
|
+ "</geometry></collision>"
|
|
)
|
|
)
|
|
for index in range(n):
|
|
joint = ET.SubElement(robot, "joint", name=f"pivot{index}", type="revolute")
|
|
ET.SubElement(joint, "parent", link="mount" if index == 0 else f"segment{index - 1}")
|
|
ET.SubElement(joint, "child", link=f"segment{index}")
|
|
ET.SubElement(
|
|
joint,
|
|
"origin",
|
|
xyz="0.2 0.1 -0.3" if index == 0 else "0.1 0 -0.15",
|
|
rpy="0.1 0.2 0.3" if index == 0 else "0 0 0",
|
|
)
|
|
ET.SubElement(joint, "axis", xyz="0.2 0.7 -0.3")
|
|
ET.SubElement(joint, "limit", lower="-1", upper="1", effort="10", velocity="5")
|
|
joint = ET.SubElement(robot, "joint", name="toe_mount", type="fixed")
|
|
ET.SubElement(joint, "parent", link=f"segment{n - 1}")
|
|
ET.SubElement(joint, "child", link="toe")
|
|
ET.SubElement(joint, "origin", xyz="0 0 -0.1", rpy="0 0 0")
|
|
ET.ElementTree(robot).write(tmp_path / "robot.urdf")
|
|
(tmp_path / "LICENSE").write_text("Fixture license\n")
|
|
return LegSpec(
|
|
1,
|
|
"fixture",
|
|
tmp_path,
|
|
"robot.urdf",
|
|
"fixture",
|
|
"LICENSE",
|
|
"test",
|
|
"pivot0",
|
|
(),
|
|
tuple(JointSpec(f"pivot{i}", 0.0, 0.1, 20.0, 0.5) for i in range(n)),
|
|
"toe",
|
|
)
|
|
|
|
|
|
@pytest.mark.parametrize("preset", ["go2_fl", "go1_fl"])
|
|
def test_real_legs_deterministic_and_complete(tmp_path, preset):
|
|
spec = LegSpec.load(ROOT / f"configs/legs/{preset}.json")
|
|
first = extract_leg(spec, tmp_path / "one")
|
|
second = extract_leg(spec, tmp_path / "two")
|
|
assert first == second
|
|
assert (tmp_path / "one/leg.urdf").read_bytes() == (tmp_path / "two/leg.urdf").read_bytes()
|
|
assert first["joint_order"] == [item.name for item in spec.joints]
|
|
assert spec.foot_body in first["links"] and "FL_hip_rotor" in first["links"]
|
|
assert all(name.startswith("FL_") for name in first["links"])
|
|
assert first["skill"]["trained"] is False
|
|
assert load_bundle(tmp_path / "one")["source"]["sha256"] == first["source"]["sha256"]
|
|
|
|
|
|
@pytest.mark.parametrize("n", [1, 2, 4])
|
|
def test_arbitrary_names_axes_and_mount_frame(tmp_path, n):
|
|
spec = fixture_spec(tmp_path, n)
|
|
bundle = extract_leg(spec, tmp_path / "out")
|
|
q = {joint.name: 0.3 for joint in spec.joints}
|
|
links, _ = forward(bundle["tree"], q)
|
|
expected = pose([0.2, 0.1, -0.3], [0.1, 0.2, 0.3])
|
|
original = ET.parse(tmp_path / "robot.urdf").getroot()
|
|
from SkillFactory.legs.urdf import origin, rotation, vector
|
|
|
|
for joint in original.findall("joint"):
|
|
expected = multiply(expected, pose(**origin(joint))) if joint.get("name") != "pivot0" else expected
|
|
if joint.get("type") == "revolute":
|
|
axis = vector(joint.find("axis").get("xyz"))
|
|
norm = math.sqrt(sum(v * v for v in axis))
|
|
expected = multiply(expected, rotation([v / norm for v in axis], 0.3))
|
|
actual = multiply(pose([0.2, 0.1, -0.3], [0.1, 0.2, 0.3]), links["toe"])
|
|
assert max(abs(a - b) for row_a, row_b in zip(actual, expected) for a, b in zip(row_a, row_b)) < 1e-10
|
|
assert len(bundle["joints"]) == n
|
|
|
|
|
|
def test_box_foot_and_declared_actuator_order(tmp_path):
|
|
spec = fixture_spec(tmp_path, 2, box=True)
|
|
spec = dataclasses.replace(spec, joints=tuple(reversed(spec.joints)))
|
|
bundle = extract_leg(spec, tmp_path / "out")
|
|
assert bundle["foot"]["type"] == "box"
|
|
assert bundle["joint_order"] == ["pivot1", "pivot0"]
|
|
assert bundle["tree"][0]["name"] == "pivot0"
|
|
|
|
|
|
def test_invalid_uncontrolled_and_stale_inputs(tmp_path):
|
|
spec = fixture_spec(tmp_path)
|
|
with pytest.raises(ValueError, match="moving joints"):
|
|
extract_leg(dataclasses.replace(spec, joints=spec.joints[:1]), tmp_path / "bad")
|
|
with pytest.raises(ValueError):
|
|
JointSpec("a", 0.0, 0.0, 20.0, 0.5)
|
|
with pytest.raises(ValueError):
|
|
dataclasses.replace(spec, decimation=0)
|
|
extract_leg(spec, tmp_path / "out")
|
|
with (tmp_path / "out/leg.urdf").open("a") as stream:
|
|
stream.write("<!-- tampered -->")
|
|
with pytest.raises(ValueError, match="hash mismatch"):
|
|
load_bundle(tmp_path / "out")
|
|
|
|
|
|
def test_missing_license_and_unsafe_resource(tmp_path):
|
|
spec = fixture_spec(tmp_path)
|
|
with pytest.raises(FileNotFoundError):
|
|
extract_leg(dataclasses.replace(spec, license_file="missing"), tmp_path / "bad")
|
|
robot = ET.parse(tmp_path / "robot.urdf")
|
|
visual = ET.SubElement(robot.getroot().find("link[@name='toe']"), "visual")
|
|
ET.SubElement(ET.SubElement(visual, "geometry"), "mesh", filename="../../outside.stl")
|
|
robot.write(tmp_path / "robot.urdf")
|
|
with pytest.raises(ValueError, match="escapes"):
|
|
extract_leg(spec, tmp_path / "bad")
|
|
|
|
|
|
def test_output_cannot_overwrite_sources_or_escape_through_symlink(tmp_path):
|
|
spec = fixture_spec(tmp_path)
|
|
before = (tmp_path / "robot.urdf").read_bytes()
|
|
with pytest.raises(ValueError, match="source root"):
|
|
extract_leg(spec, tmp_path)
|
|
assert (tmp_path / "robot.urdf").read_bytes() == before
|
|
output = tmp_path / "out"
|
|
output.mkdir()
|
|
(output / "leg.urdf").symlink_to(tmp_path / "robot.urdf")
|
|
with pytest.raises(ValueError, match="symlink"):
|
|
extract_leg(spec, output)
|
|
assert (tmp_path / "robot.urdf").read_bytes() == before
|
|
|
|
|
|
def test_nested_output_does_not_overwrite_an_existing_source_leg(tmp_path):
|
|
source = tmp_path / "nested"
|
|
source.mkdir()
|
|
spec = fixture_spec(source)
|
|
(source / "robot.urdf").rename(source / "leg.urdf")
|
|
spec = dataclasses.replace(spec, source_root=tmp_path, urdf="nested/leg.urdf", license_file="nested/LICENSE")
|
|
before = (source / "leg.urdf").read_bytes()
|
|
with pytest.raises(ValueError, match="overwrite source file"):
|
|
extract_leg(spec, source)
|
|
assert (source / "leg.urdf").read_bytes() == before
|
|
|
|
|
|
@pytest.mark.parametrize("change", ["missing_parent", "missing_limit", "missing_geometry"])
|
|
def test_malformed_urdf_fails_with_actionable_error(tmp_path, change):
|
|
spec = fixture_spec(tmp_path)
|
|
tree = ET.parse(tmp_path / "robot.urdf")
|
|
root = tree.getroot()
|
|
if change == "missing_geometry":
|
|
item = root.find("link[@name='toe']/collision")
|
|
item.remove(item.find("geometry"))
|
|
else:
|
|
item = root.find("joint[@name='pivot0']")
|
|
item.remove(item.find("parent" if change == "missing_parent" else "limit"))
|
|
tree.write(tmp_path / "robot.urdf")
|
|
with pytest.raises(ValueError, match="Missing"):
|
|
extract_leg(spec, tmp_path / "out")
|
|
|
|
|
|
@pytest.mark.parametrize("invalid", ["scale", "mapping", "layer", "support"])
|
|
def test_stale_or_invalid_conversion_rejected_before_kit(tmp_path, invalid):
|
|
from SkillFactory.legs.urdf import digest
|
|
|
|
bundle = extract_leg(fixture_spec(tmp_path), tmp_path / "out")
|
|
directory = tmp_path / "out"
|
|
(directory / "leg.usda").write_text("#usda 1.0\\n")
|
|
conversion = {
|
|
"usd_path": "leg.usda",
|
|
"manifest_sha256": digest(directory / "manifest.json"),
|
|
"used_layers": ["leg.usda"],
|
|
"fixed_to_world": True,
|
|
"body_paths": {name: "/Geometry/" + name for name in (*bundle["links"], bundle["anchor_body"])},
|
|
"joint_paths": {name: "/Joints/" + name for name in bundle["joint_order"]},
|
|
}
|
|
if invalid == "scale":
|
|
bundle["length_scale_m"] = 0
|
|
(directory / "manifest.json").write_text(json.dumps(bundle))
|
|
conversion["manifest_sha256"] = digest(directory / "manifest.json")
|
|
elif invalid == "mapping":
|
|
conversion["body_paths"]["toe"] = "/Geometry/" + bundle["anchor_body"]
|
|
elif invalid == "layer":
|
|
conversion["used_layers"] = []
|
|
else:
|
|
conversion["fixed_to_world"] = False
|
|
(directory / "conversion.json").write_text(json.dumps(conversion))
|
|
with pytest.raises(ValueError):
|
|
load_bundle(directory, require_usd=True)
|
|
|
|
|
|
def test_unknown_spec_fields_rejected(tmp_path):
|
|
data = LegSpec.load(ROOT / "configs/legs/go2_fl.json").to_dict()
|
|
data["typo"] = True
|
|
path = tmp_path / "spec.json"
|
|
path.write_text(json.dumps(data))
|
|
with pytest.raises(TypeError):
|
|
LegSpec.load(path)
|