"""URDF and SRDF emission for the juno humanoid. Stdlib-only builders consumed by `juno.py` `gen_urdf()` / `gen_srdf()`. Design ledger (in addition to the chain ledger in `juno_parts/chain.py`): - Units: URDF output is meters/kilograms/radians; chain offsets and baked mass properties are millimeters (mesh sources are mm, hence the 0.001 mesh scale). Frames: +X forward, +Y robot-left, +Z up; the pelvis link is the root with the waist-yaw joint center at its origin. - Link frames: every link frame coincides with its parent joint frame and with the part-local modeling frame of the matching `juno_parts` builder, so all visual mesh origins are identity. - Visual geometry: per-link 3MF meshes under `3MF/` exported from the `STEP/.py` wrapper sources (XCAF colors preserved). - Collision geometry: axis-aligned bounding boxes of each part in its link frame — intentionally coarse, for viewer/planning sanity rather than contact-accurate simulation. - Inertials: CAD volume properties (OCP BRepGProp) at an ASSUMED uniform density chosen so the whole robot masses ASSUMED_TOTAL_MASS_KG. COM and the full inertia tensor (about the COM, link frame) come from the CAD solids; only the density is assumed. Confidence: estimated. - SRDF: planning groups are the standard humanoid split (arms, legs, torso, head, hand end effectors, whole_body union). Group states are whole-body named poses from `chain.named_poses_deg()` converted to radians. Disabled collisions: URDF-adjacent pairs (reason Adjacent) plus pairs whose collision boxes already overlap in the zero pose (reason Default), computed from the chain offsets at import time. """ from __future__ import annotations import math import xml.etree.ElementTree as ET from juno_parts import chain from juno_parts.mass_props import LINK_MASS_PROPS, TOTAL_VOLUME_MM3 ROBOT_NAME = "juno" # ~1.40 m G1-class research humanoid; the CAD models styled shells, not # component masses, so total mass is an assumption distributed by volume. ASSUMED_TOTAL_MASS_KG = 35.0 UNIFORM_DENSITY_KG_PER_MM3 = ASSUMED_TOTAL_MASS_KG / TOTAL_VOLUME_MM3 MM_TO_M = 0.001 # Unit-density inertia integrates r^2 dV in mm^5; kg*m^2 = density * mm^5 * 1e-6. INERTIA_MM5_TO_KG_M2 = UNIFORM_DENSITY_KG_PER_MM3 * 1e-6 MESH_DIR = "3MF" MESH_SCALE_FROM_MM = "0.001 0.001 0.001" HEADER_NOTE = ( " Generated by juno.py (juno_parts/description.py) for CAD Viewer and " "external URDF consumers. Link frames coincide with juno_parts part-local " "frames; meshes are mm 3MF exports of the same builders. Inertials use CAD " "volume properties at a uniform assumed density totalling 35 kg; collision " "geometry is per-link CAD bounding boxes; joint limits/efforts are assumed " "G1-class values documented in juno_parts/chain.py. " ) def _fmt(value: float) -> str: text = f"{value:.9f}".rstrip("0").rstrip(".") if text in ("-0", ""): return "0" return text def _vec(values, scale: float = 1.0) -> str: return " ".join(_fmt(v * scale) for v in values) def _link_element(robot: ET.Element, name: str) -> None: volume_mm3, com_mm, inertia_mm5, bbox_min_mm, bbox_max_mm = LINK_MASS_PROPS[name] link = ET.SubElement(robot, "link", {"name": name}) inertial = ET.SubElement(link, "inertial") ET.SubElement(inertial, "origin", {"xyz": _vec(com_mm, MM_TO_M), "rpy": "0 0 0"}) ET.SubElement(inertial, "mass", {"value": _fmt(volume_mm3 * UNIFORM_DENSITY_KG_PER_MM3)}) ixx, iyy, izz, ixy, ixz, iyz = (v * INERTIA_MM5_TO_KG_M2 for v in inertia_mm5) ET.SubElement( inertial, "inertia", { "ixx": _fmt(ixx), "ixy": _fmt(ixy), "ixz": _fmt(ixz), "iyy": _fmt(iyy), "iyz": _fmt(iyz), "izz": _fmt(izz), }, ) visual = ET.SubElement(link, "visual") ET.SubElement(visual, "origin", {"xyz": "0 0 0", "rpy": "0 0 0"}) geometry = ET.SubElement(visual, "geometry") ET.SubElement( geometry, "mesh", {"filename": f"{MESH_DIR}/{name}.3mf", "scale": MESH_SCALE_FROM_MM}, ) bbox_center_mm = [(lo + hi) / 2.0 for lo, hi in zip(bbox_min_mm, bbox_max_mm)] bbox_size_mm = [hi - lo for lo, hi in zip(bbox_min_mm, bbox_max_mm)] collision = ET.SubElement(link, "collision") ET.SubElement(collision, "origin", {"xyz": _vec(bbox_center_mm, MM_TO_M), "rpy": "0 0 0"}) geometry = ET.SubElement(collision, "geometry") ET.SubElement(geometry, "box", {"size": _vec(bbox_size_mm, MM_TO_M)}) def _joint_element(robot: ET.Element, joint: dict) -> None: element = ET.SubElement(robot, "joint", {"name": joint["name"], "type": "revolute"}) ET.SubElement(element, "parent", {"link": joint["parent"]}) ET.SubElement(element, "child", {"link": joint["child"]}) ET.SubElement(element, "origin", {"xyz": _vec(joint["origin_mm"], MM_TO_M), "rpy": "0 0 0"}) ET.SubElement(element, "axis", {"xyz": _vec(joint["axis"])}) lower_deg, upper_deg = joint["range_deg"] ET.SubElement( element, "limit", { "lower": _fmt(math.radians(lower_deg)), "upper": _fmt(math.radians(upper_deg)), "effort": _fmt(joint["effort_nm"]), "velocity": _fmt(joint["velocity_rad_s"]), }, ) def build_urdf() -> ET.Element: robot = ET.Element("robot", {"name": ROBOT_NAME}) robot.append(ET.Comment(HEADER_NOTE)) # Frame-only ground root (no inertial/visual/collision by intent): the # pelvis is lifted so the all-zero pose stands with its soles on z = 0. ET.SubElement(robot, "link", {"name": "base_footprint"}) for name in chain.all_links(): _link_element(robot, name) footprint_joint = ET.SubElement( robot, "joint", {"name": "base_footprint_to_pelvis", "type": "fixed"} ) ET.SubElement(footprint_joint, "parent", {"link": "base_footprint"}) ET.SubElement(footprint_joint, "child", {"link": "pelvis"}) ET.SubElement( footprint_joint, "origin", {"xyz": _vec((0.0, 0.0, chain.BASE_FOOTPRINT_TO_PELVIS_Z_MM), MM_TO_M), "rpy": "0 0 0"}, ) for joint in chain.all_joints(): _joint_element(robot, joint) return robot # ------------------------------------------------------------------ SRDF def _link_origins_world_mm() -> dict[str, tuple[float, float, float]]: """Link-frame origins in the pelvis frame at the all-zero pose. At zero joint angles every chain transform is a pure translation, so origins are cumulative offset sums. """ origins = {"pelvis": (0.0, 0.0, 0.0)} for joint in chain.all_joints(): px, py, pz = origins[joint["parent"]] ox, oy, oz = joint["origin_mm"] origins[joint["child"]] = (px + ox, py + oy, pz + oz) return origins def _zero_pose_overlapping_pairs() -> list[tuple[str, str]]: """Collision-box pairs that already overlap in the zero pose.""" origins = _link_origins_world_mm() boxes = {} for name in chain.all_links(): _, _, _, bbox_min_mm, bbox_max_mm = LINK_MASS_PROPS[name] origin = origins[name] boxes[name] = ( [o + lo for o, lo in zip(origin, bbox_min_mm)], [o + hi for o, hi in zip(origin, bbox_max_mm)], ) names = chain.all_links() pairs = [] for i, a in enumerate(names): a_min, a_max = boxes[a] for b in names[i + 1 :]: b_min, b_max = boxes[b] if all(a_min[k] < b_max[k] and a_max[k] > b_min[k] for k in range(3)): pairs.append((a, b)) return pairs def _adjacent_pairs() -> list[tuple[str, str]]: return [(joint["parent"], joint["child"]) for joint in chain.all_joints()] def _arm_group(robot: ET.Element, side: str) -> None: # The arm chain tips at the wrist carrier (the end-effector attach link) # so the hand end-effector group shares no links with its parent group. group = ET.SubElement(robot, "group", {"name": f"{side}_arm"}) ET.SubElement(group, "chain", {"base_link": "torso", "tip_link": f"wrist_carrier_{side}"}) def _leg_group(robot: ET.Element, side: str) -> None: group = ET.SubElement(robot, "group", {"name": f"{side}_leg"}) ET.SubElement(group, "chain", {"base_link": "pelvis", "tip_link": f"foot_{side}"}) def build_srdf() -> ET.Element: robot = ET.Element("robot", {"name": ROBOT_NAME}) robot.append( ET.Comment( " MoveIt2 planning semantics for juno.urdf. Groups: per-limb chains, " "torso/head joint groups, hand end-effector groups, and a whole_body " "union. Group states are whole-body named poses (radians) from " "juno_parts/chain.py. Disabled collisions: URDF-adjacent pairs plus " "zero-pose bounding-box overlaps (reason Default). " ) ) ET.SubElement( robot, "virtual_joint", { "name": "world_to_base", "type": "floating", "parent_frame": "world", "child_link": "base_footprint", }, ) torso_group = ET.SubElement(robot, "group", {"name": "torso"}) ET.SubElement(torso_group, "joint", {"name": "waist_yaw"}) head_group = ET.SubElement(robot, "group", {"name": "head"}) ET.SubElement(head_group, "joint", {"name": "neck_yaw"}) ET.SubElement(head_group, "joint", {"name": "neck_pitch"}) for side in chain.SIDES: _leg_group(robot, side) for side in chain.SIDES: _arm_group(robot, side) for side in chain.SIDES: # Owns wrist_pitch (and thereby the hand link) so the end-effector # group is disjoint from the arm chain ending at wrist_carrier. hand_group = ET.SubElement(robot, "group", {"name": f"{side}_hand"}) ET.SubElement(hand_group, "joint", {"name": f"wrist_pitch_{side}"}) whole_body = ET.SubElement(robot, "group", {"name": "whole_body"}) for name in ( "torso", "head", "left_leg", "right_leg", "left_arm", "right_arm", "left_hand", "right_hand", ): ET.SubElement(whole_body, "group", {"name": name}) joint_order = [joint["name"] for joint in chain.all_joints()] for pose_name, pose_deg in chain.named_poses_deg().items(): state = ET.SubElement(robot, "group_state", {"name": pose_name, "group": "whole_body"}) for joint_name in joint_order: ET.SubElement( state, "joint", {"name": joint_name, "value": _fmt(math.radians(pose_deg[joint_name]))}, ) for side in chain.SIDES: ET.SubElement( robot, "end_effector", { "name": f"{side}_hand_eef", "parent_link": f"wrist_carrier_{side}", "group": f"{side}_hand", "parent_group": f"{side}_arm", }, ) adjacent = _adjacent_pairs() adjacent_set = {frozenset(pair) for pair in adjacent} for link1, link2 in adjacent: ET.SubElement( robot, "disable_collisions", {"link1": link1, "link2": link2, "reason": "Adjacent"}, ) for link1, link2 in _zero_pose_overlapping_pairs(): if frozenset((link1, link2)) in adjacent_set: continue ET.SubElement( robot, "disable_collisions", {"link1": link1, "link2": link2, "reason": "Default"}, ) return robot