Files
SkillFactory/tests/test_leg_assets.py
T
chenlin 7f39abb613 release: SkillFactory v0.0.1
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
2026-10-10 09:58:30 +08:00

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)