from __future__ import annotations import math import uuid import xml.etree.ElementTree as ET from pathlib import Path from typing import Iterable from ..cad.ocp_interference import _import_ocp, read_step_shape from ..kinematics import solve_instance from ..models import AssemblyManifest, ReducerInstance, read_json, utc_now_iso, write_json, write_model_json from ..reducer_urdf_exporter import ( build_reducer_urdf_xml, validate_reducer_urdf_export, ) from .models import ( ComponentPlacement, JointModuleManifest, UrdfExportManifest, UrdfValidationCheck, UrdfValidationReport, ) from .step_geometry import apply_transform PACKAGE_NAME = "planetary_reducer_joint_module" ROBOT_NAME = "planetary_reducer_joint_module" MM_TO_M = 0.001 class UrdfExportError(RuntimeError): pass def load_joint_manifest(path: Path) -> JointModuleManifest: return JointModuleManifest.model_validate(read_json(path)) def _m(value_mm: float) -> float: return float(value_mm) * MM_TO_M def _fmt(values: Iterable[float]) -> str: return " ".join(f"{float(value):.12g}" for value in values) def _axis_tuple(values: Iterable[float]) -> tuple[float, float, float]: items = tuple(float(value) for value in values) length = math.sqrt(sum(value * value for value in items)) if length <= 1e-12: return (0.0, 0.0, 1.0) return tuple(value / length for value in items) # type: ignore[return-value] def _axis_origin(component) -> tuple[float, float, float]: axis = component.placed_metrics.inferred_axis if axis is None: return (0.0, 0.0, 0.0) return tuple(float(value) for value in axis.origin_mm) def _axis_direction(component) -> tuple[float, float, float]: axis = component.placed_metrics.inferred_axis if axis is None: return (0.0, 0.0, 1.0) return _axis_tuple(axis.direction_xyz) def _planet_index(component_id: str) -> int | None: parts = component_id.split("_") if len(parts) != 2 or parts[0] != "planet": return None suffix = parts[1] if suffix.isdigit(): return int(suffix) return None def _planet_component_ids(manifest: JointModuleManifest) -> list[str]: return sorted( [ component_id for component_id in manifest.components if component_id.startswith("planet_") and _planet_index(component_id) is not None ], key=lambda value: int(value.rsplit("_", 1)[-1]), ) def compute_urdf_motion_map( *, reducer_instance: ReducerInstance, ) -> dict[str, float | str | dict[str, float]]: solution = solve_instance(reducer_instance) input_name = reducer_instance.boundary.input input_speed = solution.speeds[input_name] if abs(input_speed) <= 1e-12: raise UrdfExportError("input speed is zero; cannot build URDF mimic multipliers") carrier_multiplier = solution.speeds["carrier"] / input_speed planet_relative_multiplier = (solution.speeds["planet"] - solution.speeds["carrier"]) / input_speed return { "input_joint": "sun_input_joint", "carrier_joint_multiplier": carrier_multiplier, "planet_spin_joint_multiplier": planet_relative_multiplier, "sun_speed": solution.speeds["sun"], "ring_speed": solution.speeds["ring"], "carrier_speed": solution.speeds["carrier"], "planet_absolute_speed": solution.speeds["planet"], "ratio": abs(solution.ratio), "direction": solution.direction, "speed_mode": solution.speed_mode, "speeds": solution.speeds, "formula": { "carrier_joint": "theta_carrier = theta_sun * omega_carrier / omega_sun", "planet_spin_joint": "theta_planet_relative = theta_sun * (omega_planet - omega_carrier) / omega_sun", }, } def _export_stl(shape, path: Path, *, linear_deflection_mm: float) -> None: ocp = _import_ocp() path.parent.mkdir(parents=True, exist_ok=True) mesher = ocp["BRepMesh_IncrementalMesh"](shape, float(linear_deflection_mm)) if hasattr(mesher, "Perform"): mesher.Perform() writer = ocp["StlAPI_Writer"]() if hasattr(writer, "SetASCIIMode"): writer.SetASCIIMode(False) elif hasattr(writer, "ASCIIMode"): writer.ASCIIMode = False ok = writer.Write(shape, str(path)) if ok is False: raise UrdfExportError(f"stl_write_failed: {path}") def _export_meshes( *, manifest: JointModuleManifest, mesh_dir: Path, linear_deflection_mm: float, ) -> dict[str, str]: mesh_paths: dict[str, str] = {} mesh_dir.mkdir(parents=True, exist_ok=True) # This directory is a generated artifact. Remove meshes from an earlier # motor representation so the viewer cannot accidentally reuse them. for stale_mesh in mesh_dir.glob("*.stl"): stale_mesh.unlink() for component_id, component in manifest.components.items(): if not component.included_in_joint_step: continue source_path = Path(component.source_step_path) if not source_path.exists(): raise UrdfExportError(f"missing_component_step_for_urdf_mesh: {component_id}: {source_path}") shape = read_step_shape(source_path) placed = apply_transform(shape, component.placement) mesh_path = mesh_dir / f"{component_id}.stl" _export_stl(placed, mesh_path, linear_deflection_mm=linear_deflection_mm) mesh_paths[component_id] = str(mesh_path.resolve()) return mesh_paths def _add_materials(robot: ET.Element) -> None: materials = { "housing_gray": "0.45 0.48 0.50 0.45", "motor_dark": "0.08 0.09 0.10 0.75", "gear_gold": "0.95 0.70 0.18 1.0", "ring_blue": "0.20 0.32 0.75 0.75", "carrier_green": "0.20 0.65 0.35 0.85", "bearing_silver": "0.75 0.75 0.78 0.85", "steel": "0.65 0.66 0.68 1.0", } for name, rgba in materials.items(): material = ET.SubElement(robot, "material", {"name": name}) ET.SubElement(material, "color", {"rgba": rgba}) def _material_for(component_id: str, role: str) -> str: if component_id == "housing": return "housing_gray" if component_id == "motor" or component_id.startswith("motor_"): return "motor_dark" if component_id == "ring": return "ring_blue" if "bearing" in component_id: return "bearing_silver" if component_id in {"carrier", "carrier_front_plate", "output_shaft"} or component_id.startswith(("planet_pin_", "planet_spacer_")): return "carrier_green" if role == "planet" or component_id == "sun": return "gear_gold" return "steel" def _add_link( robot: ET.Element, *, link_name: str, mesh_filename: str | None = None, material: str = "steel", visual_origin_xyz: tuple[float, float, float] = (0.0, 0.0, 0.0), collision: bool = True, ) -> None: link = ET.SubElement(robot, "link", {"name": link_name}) if mesh_filename is None: return for tag in ["visual", "collision"] if collision else ["visual"]: element = ET.SubElement(link, tag) ET.SubElement(element, "origin", {"xyz": _fmt(visual_origin_xyz), "rpy": "0 0 0"}) geometry = ET.SubElement(element, "geometry") ET.SubElement( geometry, "mesh", { "filename": mesh_filename, "scale": "0.001 0.001 0.001", }, ) if tag == "visual": ET.SubElement(element, "material", {"name": material}) def _add_joint( robot: ET.Element, *, name: str, joint_type: str, parent: str, child: str, origin_xyz: tuple[float, float, float] = (0.0, 0.0, 0.0), axis: tuple[float, float, float] | None = None, mimic: tuple[str, float, float] | None = None, ) -> None: joint = ET.SubElement(robot, "joint", {"name": name, "type": joint_type}) ET.SubElement(joint, "origin", {"xyz": _fmt(origin_xyz), "rpy": "0 0 0"}) ET.SubElement(joint, "parent", {"link": parent}) ET.SubElement(joint, "child", {"link": child}) if axis is not None: ET.SubElement(joint, "axis", {"xyz": _fmt(axis)}) if mimic is not None: joint_name, multiplier, offset = mimic ET.SubElement( joint, "mimic", { "joint": joint_name, "multiplier": f"{float(multiplier):.12g}", "offset": f"{float(offset):.12g}", }, ) def _mesh_uri(package_name: str, component_id: str) -> str: return f"package://{package_name}/meshes/{component_id}.stl" def _add_motor_links( robot: ET.Element, *, manifest: JointModuleManifest, package_name: str, input_joint_name: str, axis: tuple[float, float, float], ) -> bool: split_ids = { "motor_stator_housing", "motor_rotor_housing", "motor_output_shaft", "motor_windings", } if not split_ids <= set(manifest.components): return False for component_id in ("motor_stator_housing", "motor_windings"): component = manifest.components[component_id] link_name = f"{component_id}_link" _add_link( robot, link_name=link_name, mesh_filename=_mesh_uri(package_name, component_id), material=_material_for(component_id, component.role), ) _add_joint( robot, name=f"base_to_{component_id}", joint_type="fixed", parent="base_link", child=link_name, ) rotor = manifest.components["motor_rotor_housing"] _add_link( robot, link_name="motor_rotor_housing_link", mesh_filename=_mesh_uri(package_name, "motor_rotor_housing"), material=_material_for("motor_rotor_housing", rotor.role), ) _add_joint( robot, name="motor_rotor_joint", joint_type="continuous", parent="base_link", child="motor_rotor_housing_link", axis=axis, mimic=(input_joint_name, 1.0, 0.0), ) output_shaft = manifest.components["motor_output_shaft"] _add_link( robot, link_name="motor_output_shaft_link", mesh_filename=_mesh_uri(package_name, "motor_output_shaft"), material=_material_for("motor_output_shaft", output_shaft.role), ) _add_joint( robot, name="motor_rotor_to_output_shaft", joint_type="fixed", parent="motor_rotor_housing_link", child="motor_output_shaft_link", ) return True def _visual_origin_for_link( manifest: JointModuleManifest, component_id: str, *, link_frame_origin_mm: tuple[float, float, float] = (0.0, 0.0, 0.0), ) -> tuple[float, float, float]: return tuple(-_m(value) for value in link_frame_origin_mm) def _adapt_reducer_urdf_to_joint_frame( root: ET.Element, *, manifest: JointModuleManifest, axis: tuple[float, float, float], ) -> None: """Retarget raw reducer motion frames to the placed joint-module frame.""" for joint in root.findall("joint"): if joint.get("type") not in {"continuous", "revolute"}: continue axis_node = joint.find("axis") if axis_node is not None: axis_node.set("xyz", _fmt(axis)) for component_id, component in manifest.components.items(): if not ( component_id.startswith("s1_planet_") or component_id.startswith("s2_planet_") ): continue parts = component_id.rsplit("_", 1) if len(parts) != 2 or not parts[1].isdigit(): continue stage_id = component_id.split("_", 1)[0] index = int(parts[1]) center_mm = _axis_origin(component) orbit_joint = root.find( f"./joint[@name='{stage_id}_carrier_to_planet_{index}_orbit']" ) if orbit_joint is not None: origin = orbit_joint.find("origin") if origin is not None: origin.set("xyz", _fmt(tuple(_m(value) for value in center_mm))) planet_link = root.find(f"./link[@name='{component_id}_link']") if planet_link is not None: visual_origin = _visual_origin_for_link( manifest, component_id, link_frame_origin_mm=center_mm, ) for origin in planet_link.findall("./visual/origin") + planet_link.findall( "./collision/origin" ): origin.set("xyz", _fmt(visual_origin)) def build_urdf_xml( *, manifest: JointModuleManifest, reducer_instance: ReducerInstance, package_name: str = PACKAGE_NAME, robot_name: str = ROBOT_NAME, ) -> tuple[str, dict[str, float | str | dict[str, float]]]: if reducer_instance.topology_family != "simple_2k_h": reducer_manifest_path = Path(manifest.reducer_output_dir) / "cad" / "assembly_manifest.json" if not reducer_manifest_path.exists(): raise UrdfExportError(f"missing_reducer_assembly_manifest: {reducer_manifest_path}") reducer_manifest = AssemblyManifest.model_validate(read_json(reducer_manifest_path)) reducer_xml, motion = build_reducer_urdf_xml( manifest=reducer_manifest, reducer_instance=reducer_instance, package_name=package_name, robot_name=robot_name, ) root = ET.fromstring(reducer_xml) input_joint_name = str(motion.get("input_joint") or "sun_input_joint") axis_component = manifest.components.get("motor") axis = _axis_direction(axis_component) if axis_component is not None else (0.0, 0.0, 1.0) _adapt_reducer_urdf_to_joint_frame(root, manifest=manifest, axis=axis) split_motor_added = _add_motor_links( root, manifest=manifest, package_name=package_name, input_joint_name=input_joint_name, axis=axis, ) motor = manifest.components.get("motor") if motor is not None and not split_motor_added: _add_link( root, link_name="motor_link", mesh_filename=_mesh_uri(package_name, "motor"), material=_material_for("motor", motor.role), ) _add_joint( root, name="base_to_motor", joint_type="fixed", parent="base_link", child="motor_link", ) ET.indent(root, space=" ") return ET.tostring(root, encoding="unicode", xml_declaration=True), motion if reducer_instance.boundary.input != "sun" or reducer_instance.boundary.fixed != "ring": raise UrdfExportError("URDF exporter currently supports sun input and fixed ring reducer boundary") motion = compute_urdf_motion_map(reducer_instance=reducer_instance) robot = ET.Element("robot", {"name": robot_name}) _add_materials(robot) _add_link(robot, link_name="base_link", collision=False) axis = _axis_direction(manifest.components["sun"]) carrier_multiplier = float(motion["carrier_joint_multiplier"]) planet_spin_multiplier = float(motion["planet_spin_joint_multiplier"]) split_motor_enabled = { "motor_stator_housing", "motor_rotor_housing", "motor_output_shaft", "motor_windings", } <= set(manifest.components) fixed_base_components = ["housing", "ring", "output_bearing_1", "output_bearing_2"] if not split_motor_enabled: fixed_base_components.insert(1, "motor") for component_id in fixed_base_components: component = manifest.components.get(component_id) if component is None: continue link_name = f"{component_id}_link" _add_link( robot, link_name=link_name, mesh_filename=_mesh_uri(package_name, component_id), material=_material_for(component_id, component.role), ) _add_joint( robot, name=f"base_to_{component_id}", joint_type="fixed", parent="base_link", child=link_name, ) sun_component = manifest.components["sun"] _add_link( robot, link_name="sun_link", mesh_filename=_mesh_uri(package_name, "sun"), material=_material_for("sun", sun_component.role), ) _add_joint( robot, name="sun_input_joint", joint_type="continuous", parent="base_link", child="sun_link", axis=axis, ) _add_motor_links( robot, manifest=manifest, package_name=package_name, input_joint_name="sun_input_joint", axis=axis, ) for component_id in ["sun_input_shaft", "sun_input_key"]: component = manifest.components.get(component_id) if component is None: continue link_name = f"{component_id}_link" _add_link( robot, link_name=link_name, mesh_filename=_mesh_uri(package_name, component_id), material=_material_for(component_id, component.role), ) _add_joint( robot, name=f"sun_to_{component_id}", joint_type="fixed", parent="sun_link", child=link_name, ) carrier_component = manifest.components["carrier"] _add_link( robot, link_name="carrier_link", mesh_filename=_mesh_uri(package_name, "carrier"), material=_material_for("carrier", carrier_component.role), ) _add_joint( robot, name="carrier_output_joint", joint_type="continuous", parent="base_link", child="carrier_link", axis=axis, mimic=("sun_input_joint", carrier_multiplier, 0.0), ) for component_id in ["carrier_front_plate", "output_shaft"]: component = manifest.components.get(component_id) if component is None: continue link_name = f"{component_id}_link" _add_link( robot, link_name=link_name, mesh_filename=_mesh_uri(package_name, component_id), material=_material_for(component_id, component.role), ) _add_joint( robot, name=f"carrier_to_{component_id}", joint_type="fixed", parent="carrier_link", child=link_name, ) for planet_id in _planet_component_ids(manifest): index = int(planet_id.rsplit("_", 1)[-1]) planet_component = manifest.components[planet_id] planet_origin_mm = _axis_origin(planet_component) planet_origin_m = tuple(_m(value) for value in planet_origin_mm) orbit_link = f"planet_{index}_orbit_link" planet_link = f"planet_{index}_link" _add_link(robot, link_name=orbit_link, collision=False) _add_joint( robot, name=f"carrier_to_planet_{index}_orbit", joint_type="fixed", parent="carrier_link", child=orbit_link, origin_xyz=planet_origin_m, ) _add_link( robot, link_name=planet_link, mesh_filename=_mesh_uri(package_name, planet_id), material=_material_for(planet_id, planet_component.role), visual_origin_xyz=_visual_origin_for_link( manifest, planet_id, link_frame_origin_mm=planet_origin_mm, ), ) _add_joint( robot, name=f"planet_{index}_spin_joint", joint_type="continuous", parent=orbit_link, child=planet_link, axis=axis, mimic=("sun_input_joint", planet_spin_multiplier, 0.0), ) for suffix, parent_link in [ ("pin", "carrier_link"), ("spacer", "carrier_link"), ("bearing", planet_link), ]: component_id = f"planet_{suffix}_{index}" if component_id not in manifest.components: continue component = manifest.components[component_id] link_name = f"{component_id}_link" link_frame_origin = planet_origin_mm if parent_link == planet_link else (0.0, 0.0, 0.0) _add_link( robot, link_name=link_name, mesh_filename=_mesh_uri(package_name, component_id), material=_material_for(component_id, component.role), visual_origin_xyz=_visual_origin_for_link( manifest, component_id, link_frame_origin_mm=link_frame_origin, ), ) _add_joint( robot, name=f"{parent_link.removesuffix('_link')}_to_{component_id}", joint_type="fixed", parent=parent_link, child=link_name, origin_xyz=(0.0, 0.0, 0.0), ) ET.indent(robot, space=" ") return ET.tostring(robot, encoding="unicode", xml_declaration=True), motion def _package_xml(package_name: str) -> str: root = ET.Element("package", {"format": "3"}) ET.SubElement(root, "name").text = package_name ET.SubElement(root, "version").text = "0.1.0" ET.SubElement(root, "description").text = "Generated planetary reducer joint module URDF package" ET.SubElement(root, "maintainer", {"email": "noreply@example.com"}).text = "planetary_reducer_system_demo" ET.SubElement(root, "license").text = "UNLICENSED" ET.indent(root, space=" ") return ET.tostring(root, encoding="unicode", xml_declaration=True) def _joint_elements(root: ET.Element) -> list[ET.Element]: return list(root.findall("joint")) def _mesh_component_from_uri(uri: str) -> str | None: prefix = f"package://{PACKAGE_NAME}/meshes/" if not uri.startswith(prefix) or not uri.endswith(".stl"): return None return uri.removeprefix(prefix).removesuffix(".stl") def _parse_xyz(value: str | None) -> tuple[float, float, float] | None: if not value: return None try: parts = tuple(float(item) for item in value.split()) except ValueError: return None return parts if len(parts) == 3 else None def _joint_frame_validation_checks( *, root: ET.Element, manifest: JointModuleManifest, motion: dict[str, float | str | dict[str, float]], ) -> list[UrdfValidationCheck]: checks: list[UrdfValidationCheck] = [] def add(code: str, passed: bool, message: str, actual=None, expected=None) -> None: checks.append( UrdfValidationCheck( code=code, passed=bool(passed), message=message, actual=actual, expected=expected, ) ) motor = manifest.components.get("motor") expected_axis = _axis_direction(motor) if motor is not None else None moving_axes: dict[str, tuple[float, float, float] | None] = {} if expected_axis is not None: for joint in _joint_elements(root): if joint.get("type") not in {"continuous", "revolute"}: continue axis_node = joint.find("axis") moving_axes[str(joint.get("name"))] = _parse_xyz( None if axis_node is None else axis_node.get("xyz") ) bad_axes = { name: axis for name, axis in moving_axes.items() if axis is None or abs(abs(sum(a * b for a, b in zip(axis, expected_axis))) - 1.0) > 1e-9 } add( "joint_moving_axes_match_placed_assembly_axis", bool(moving_axes) and not bad_axes, "all moving URDF joints use the placed joint-module axis", actual=bad_axes, expected={"axis_parallel_to": expected_axis}, ) split_ids = { "motor_stator_housing", "motor_rotor_housing", "motor_output_shaft", "motor_windings", } if split_ids <= set(manifest.components): expected_links = {f"{component_id}_link" for component_id in split_ids} found_links = {str(link.get("name")) for link in root.findall("link")} rotor_joint = root.find("./joint[@name='motor_rotor_joint']") mimic = None if rotor_joint is None else rotor_joint.find("mimic") input_joint = str(motion.get("input_joint") or "") multiplier = None if mimic is not None: try: multiplier = float(mimic.get("multiplier", "nan")) except ValueError: multiplier = None add( "split_motor_links_and_drive_present", expected_links <= found_links and rotor_joint is not None and rotor_joint.get("type") == "continuous" and mimic is not None and mimic.get("joint") == input_joint and multiplier is not None and abs(multiplier - 1.0) <= 1e-12, "stator and windings are fixed while rotor housing and output shaft follow the reducer input 1:1", actual={ "missing_links": sorted(expected_links - found_links), "rotor_joint_type": None if rotor_joint is None else rotor_joint.get("type"), "mimic_joint": None if mimic is None else mimic.get("joint"), "mimic_multiplier": multiplier, }, expected={"mimic_joint": input_joint, "mimic_multiplier": 1.0}, ) planet_frame_errors: list[dict[str, object]] = [] for component_id, component in manifest.components.items(): if not component_id.startswith(("s1_planet_", "s2_planet_")): continue suffix = component_id.rsplit("_", 1)[-1] if not suffix.isdigit(): continue stage_id = component_id.split("_", 1)[0] orbit_joint = root.find( f"./joint[@name='{stage_id}_carrier_to_planet_{int(suffix)}_orbit']" ) planet_link = root.find(f"./link[@name='{component_id}_link']") origin_node = None if orbit_joint is None else orbit_joint.find("origin") visual_node = None if planet_link is None else planet_link.find("./visual/origin") actual_origin = _parse_xyz(None if origin_node is None else origin_node.get("xyz")) actual_visual = _parse_xyz(None if visual_node is None else visual_node.get("xyz")) center_m = tuple(_m(value) for value in _axis_origin(component)) expected_visual = tuple(-value for value in center_m) if ( actual_origin is None or actual_visual is None or any(abs(a - b) > 1e-9 for a, b in zip(actual_origin, center_m)) or any(abs(a - b) > 1e-9 for a, b in zip(actual_visual, expected_visual)) ): planet_frame_errors.append( { "component_id": component_id, "orbit_origin": actual_origin, "expected_orbit_origin": center_m, "visual_origin": actual_visual, "expected_visual_origin": expected_visual, } ) if any(component_id.startswith(("s1_planet_", "s2_planet_")) for component_id in manifest.components): add( "planet_orbit_frames_match_placed_gear_centers", not planet_frame_errors, "planet orbit joints and mesh-local origins use the placed assembly centers", actual=planet_frame_errors, expected=[], ) return checks def validate_urdf_export( *, urdf_path: Path, mesh_files: dict[str, str], motion: dict[str, float | str | dict[str, float]], reducer_instance: ReducerInstance, joint_manifest: JointModuleManifest | None = None, out_path: Path | None = None, ) -> UrdfValidationReport: if reducer_instance.topology_family != "simple_2k_h": reducer_validation = validate_reducer_urdf_export( urdf_path=urdf_path, mesh_files=mesh_files, motion=motion, reducer_instance=reducer_instance, package_name=PACKAGE_NAME, ) checks = [ UrdfValidationCheck( code=str(item["code"]), passed=bool(item["passed"]), message=str(item["message"]), actual=item.get("actual"), expected=item.get("expected"), ) for item in reducer_validation["checks"] ] if joint_manifest is not None: try: root = ET.parse(urdf_path).getroot() except Exception: # noqa: BLE001 root = None if root is not None: checks.extend( _joint_frame_validation_checks( root=root, manifest=joint_manifest, motion=motion, ) ) summary = { "total": len(checks), "passed": sum(1 for item in checks if item.passed), "failed": sum(1 for item in checks if not item.passed), } report = UrdfValidationReport( run_id=reducer_instance.run_id, overall_passed=summary["failed"] == 0, generated_at=utc_now_iso(), checks=checks, summary=summary, ) if out_path: write_model_json(out_path, report) return report checks: list[UrdfValidationCheck] = [] def check(code: str, passed: bool, message: str, actual=None, expected=None) -> None: checks.append( UrdfValidationCheck( code=code, passed=bool(passed), message=message, actual=actual, expected=expected, ) ) try: root = ET.parse(urdf_path).getroot() except Exception as exc: # noqa: BLE001 root = None check("urdf_xml_parse", False, f"URDF XML parse failed: {exc}") if root is not None: if joint_manifest is not None: checks.extend( _joint_frame_validation_checks( root=root, manifest=joint_manifest, motion=motion, ) ) check("urdf_robot_root", root.tag == "robot" and bool(root.get("name")), "URDF root is a named robot") link_names = [item.get("name") for item in root.findall("link")] joint_names = [item.get("name") for item in _joint_elements(root)] check( "urdf_unique_links", len(link_names) == len(set(link_names)) and "base_link" in set(link_names), "URDF link names are unique and include base_link", actual=link_names, ) check( "urdf_unique_joints", len(joint_names) == len(set(joint_names)), "URDF joint names are unique", actual=joint_names, ) links = {name for name in link_names if name} child_links = [] bad_refs = [] for joint in _joint_elements(root): parent = joint.find("parent") child = joint.find("child") parent_name = parent.get("link") if parent is not None else None child_name = child.get("link") if child is not None else None if parent_name not in links or child_name not in links: bad_refs.append({"joint": joint.get("name"), "parent": parent_name, "child": child_name}) if child_name: child_links.append(child_name) check("urdf_joint_link_references", not bad_refs, "every joint parent/child references an existing link", actual=bad_refs, expected=[]) duplicate_children = sorted({item for item in child_links if child_links.count(item) > 1}) check("urdf_tree_children_unique", not duplicate_children and "base_link" not in child_links, "URDF joints form a tree with one parent per child link", actual=duplicate_children, expected=[]) mimic_by_joint = { joint.get("name"): joint.find("mimic") for joint in _joint_elements(root) if joint.find("mimic") is not None } carrier_mimic = mimic_by_joint.get("carrier_output_joint") carrier_multiplier = None if carrier_mimic is None else float(carrier_mimic.get("multiplier", "nan")) expected_carrier = float(motion["carrier_joint_multiplier"]) check( "urdf_carrier_mimic_ratio", carrier_multiplier is not None and abs(carrier_multiplier - expected_carrier) <= 1e-12, "carrier output joint mimics sun input with the solved reducer speed ratio", actual=carrier_multiplier, expected=expected_carrier, ) expected_planet = float(motion["planet_spin_joint_multiplier"]) planet_mimics = { name: float(element.get("multiplier", "nan")) for name, element in mimic_by_joint.items() if name and name.startswith("planet_") and name.endswith("_spin_joint") } check( "urdf_planet_spin_mimic_ratio", bool(planet_mimics) and all(abs(value - expected_planet) <= 1e-12 for value in planet_mimics.values()), "planet spin joints mimic sun input with solved relative planet spin speed", actual=planet_mimics, expected=expected_planet, ) mesh_components = [] missing_meshes = [] for mesh in root.findall(".//mesh"): component_id = _mesh_component_from_uri(mesh.get("filename", "")) if component_id: mesh_components.append(component_id) mesh_path = Path(mesh_files.get(component_id, "")) if not mesh_path.exists() or mesh_path.stat().st_size <= 0: missing_meshes.append(component_id) check("urdf_mesh_files_exist", not missing_meshes and bool(mesh_components), "all package:// mesh references exist on disk", actual=missing_meshes, expected=[]) summary = { "total": len(checks), "passed": sum(1 for item in checks if item.passed), "failed": sum(1 for item in checks if not item.passed), } report = UrdfValidationReport( run_id=reducer_instance.run_id, overall_passed=summary["failed"] == 0, generated_at=utc_now_iso(), checks=checks, summary=summary, ) if out_path: write_model_json(out_path, report) return report def export_urdf( *, joint_manifest_path: Path, reducer_instance_path: Path, out_dir: Path, linear_deflection_mm: float = 0.25, package_name: str = PACKAGE_NAME, robot_name: str = ROBOT_NAME, ) -> UrdfExportManifest: manifest = load_joint_manifest(joint_manifest_path) reducer_instance = ReducerInstance.model_validate(read_json(reducer_instance_path)) out_dir.mkdir(parents=True, exist_ok=True) mesh_dir = out_dir / "meshes" mesh_files = _export_meshes( manifest=manifest, mesh_dir=mesh_dir, linear_deflection_mm=linear_deflection_mm, ) urdf_xml, motion = build_urdf_xml( manifest=manifest, reducer_instance=reducer_instance, package_name=package_name, robot_name=robot_name, ) urdf_path = out_dir / "joint_module.urdf" package_xml_path = out_dir / "package.xml" motion_demo_path = out_dir / "joint_motion_demo.json" validation_path = out_dir / "urdf_validation_report.json" urdf_path.write_text(urdf_xml, encoding="utf-8") package_xml_path.write_text(_package_xml(package_name), encoding="utf-8") write_json(motion_demo_path, motion) validation = validate_urdf_export( urdf_path=urdf_path, mesh_files=mesh_files, motion=motion, reducer_instance=reducer_instance, joint_manifest=manifest, out_path=validation_path, ) if not validation.overall_passed: raise UrdfExportError(f"urdf_validation_failed: {validation.summary}") export_manifest_path = out_dir / "urdf_export_manifest.json" export_manifest = UrdfExportManifest( run_id=f"urdf_{uuid.uuid4().hex[:12]}", joint_run_id=manifest.run_id, reducer_run_id=manifest.reducer_run_id, generated_at=utc_now_iso(), robot_name=robot_name, package_name=package_name, urdf_path=str(urdf_path.resolve()), package_xml_path=str(package_xml_path.resolve()), mesh_dir=str(mesh_dir.resolve()), motion_demo_path=str(motion_demo_path.resolve()), validation_report_path=str(validation_path.resolve()), mesh_files=mesh_files, transmission_summary=motion, ) write_model_json(export_manifest_path, export_manifest) return export_manifest