likang/reducers
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add knowledge-driven joint design studio

Browse Source
This commit is contained in:
Jerry
2026-08-14 13:53:51 +08:00
parent 7d12459a32
commit 281afdfd9e
57 changed files with 269659 additions and 1152 deletions
Download Patch File Download Diff File Expand all files Collapse all files
backend/src/joint_module/urdf_exporter.py
+313 -5
View File
@@ -126,6 +126,8 @@ def _export_stl(shape, path: Path, *, linear_deflection_mm: float) -> None:
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}")
@@ -139,6 +141,10 @@ def _export_meshes(
) -> 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
@@ -171,7 +177,7 @@ def _add_materials(robot: ET.Element) -> None:
def _material_for(component_id: str, role: str) -> str:
if component_id == "housing":
return "housing_gray"
if component_id == "motor":
if component_id == "motor" or component_id.startswith("motor_"):
return "motor_dark"
if component_id == "ring":
return "ring_blue"
@@ -246,6 +252,73 @@ 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,
@@ -255,6 +328,52 @@ def _visual_origin_for_link(
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,
@@ -274,8 +393,19 @@ def build_urdf_xml(
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:
if motor is not None and not split_motor_added:
_add_link(
root,
link_name="motor_link",
@@ -302,7 +432,15 @@ def build_urdf_xml(
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"]
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:
@@ -337,6 +475,13 @@ def build_urdf_xml(
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:
@@ -484,12 +629,148 @@ def _mesh_component_from_uri(uri: str) -> str | 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":
@@ -510,12 +791,30 @@ def validate_urdf_export(
)
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=bool(reducer_validation["overall_passed"]),
overall_passed=summary["failed"] == 0,
generated_at=utc_now_iso(),
checks=checks,
summary=dict(reducer_validation["summary"]),
summary=summary,
)
if out_path:
write_model_json(out_path, report)
@@ -540,6 +839,14 @@ def validate_urdf_export(
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)]
@@ -664,6 +971,7 @@ def export_urdf(
mesh_files=mesh_files,
motion=motion,
reducer_instance=reducer_instance,
joint_manifest=manifest,
out_path=validation_path,
)
if not validation.overall_passed: