Jerry
997 lines
36 KiB
Python
997 lines
36 KiB
Python
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
|