Split project into frontend and backend
This commit is contained in:
@@ -0,0 +1,688 @@
|
||||
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)
|
||||
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)
|
||||
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":
|
||||
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 _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 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)
|
||||
motor = manifest.components.get("motor")
|
||||
if motor is not None:
|
||||
_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"])
|
||||
|
||||
fixed_base_components = ["housing", "motor", "ring", "output_bearing_1", "output_bearing_2"]
|
||||
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,
|
||||
)
|
||||
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 validate_urdf_export(
|
||||
*,
|
||||
urdf_path: Path,
|
||||
mesh_files: dict[str, str],
|
||||
motion: dict[str, float | str | dict[str, float]],
|
||||
reducer_instance: ReducerInstance,
|
||||
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"]
|
||||
]
|
||||
report = UrdfValidationReport(
|
||||
run_id=reducer_instance.run_id,
|
||||
overall_passed=bool(reducer_validation["overall_passed"]),
|
||||
generated_at=utc_now_iso(),
|
||||
checks=checks,
|
||||
summary=dict(reducer_validation["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:
|
||||
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,
|
||||
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
|
||||
Reference in New Issue
Block a user