"""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('') ) if name == "toe": link.append( ET.fromstring( '' + ('' if box else '') + "" ) ) 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("") 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)