"""URDF and SRDF emission for the lyra dexterous hand. Stdlib-only builders consumed by `lyra.py` `gen_urdf()` / `gen_srdf()`. Design ledger (in addition to the chain ledger in `lyra_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: RIGHT hand, +Z distal, +Y palmar, +X radial; the palm link is the root body with the wrist-flange mount face center at its origin, plus a frame-only `wrist_mount` link at the same point for attaching the hand to a robot forearm. - Link frames: every link frame coincides with its parent joint frame and with the part-local modeling frame of the matching `lyra_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 hand 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: per-digit chain groups, a `fingers` union, and a whole-`hand` joint group; group states are the 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 lyra_parts import chain from lyra_parts.mass_props import LINK_MASS_PROPS, TOTAL_VOLUME_MM3 ROBOT_NAME = chain.ROBOT_NAME # Tendon-driven research-hand concept: shells, spines, and pins only; the # CAD does not model actuators, so total mass is an assumption distributed # by CAD volume. ASSUMED_TOTAL_MASS_KG = 0.62 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 lyra.py (lyra_parts/description.py) for CAD Viewer and " "external URDF consumers. Link frames coincide with lyra_parts part-local " "frames; meshes are mm 3MF exports of the same builders. Inertials use CAD " "volume properties at a uniform assumed density totalling 0.62 kg; " "collision geometry is per-link CAD bounding boxes; joint limits/efforts " "are assumed tendon-drive values documented in lyra_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 mounting root (no inertial/visual/collision by intent): # external consumers attach the hand at the wrist-flange face frame. ET.SubElement(robot, "link", {"name": "wrist_mount"}) for name in chain.all_links(): _link_element(robot, name) mount_joint = ET.SubElement( robot, "joint", {"name": "wrist_mount_to_palm", "type": "fixed"} ) ET.SubElement(mount_joint, "parent", {"link": "wrist_mount"}) ET.SubElement(mount_joint, "child", {"link": "palm"}) ET.SubElement(mount_joint, "origin", {"xyz": "0 0 0", "rpy": "0 0 0"}) for joint in chain.all_joints(): _joint_element(robot, joint) return robot # ------------------------------------------------------------------ SRDF def _link_origins_zero_mm() -> dict[str, tuple[float, float, float]]: """Link-frame origins in the palm frame at the all-zero pose. At zero joint angles every chain transform is a pure translation, so origins are cumulative offset sums. """ origins = {"palm": (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_zero_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 build_srdf() -> ET.Element: robot = ET.Element("robot", {"name": ROBOT_NAME}) robot.append( ET.Comment( " MoveIt2 planning semantics for lyra.urdf. Groups: per-digit " "chains, a fingers union, and the whole-hand joint group. Group " "states are named hand poses (radians) from lyra_parts/chain.py. " "Disabled collisions: URDF-adjacent pairs plus zero-pose " "bounding-box overlaps (reason Default). " ) ) ET.SubElement( robot, "virtual_joint", { "name": "world_to_wrist", "type": "fixed", "parent_frame": "world", "child_link": "wrist_mount", }, ) # Per-digit groups own their flexion joints, so digit links are in the # group but the palm is not: the hand end effector can then attach at # the palm (adjacent to, not inside, the group). digits = list(chain.FINGERS) + ["thumb"] digit_joints = {finger: chain.finger_joints(finger) for finger in chain.FINGERS} digit_joints["thumb"] = chain.thumb_joints() for digit in digits: group = ET.SubElement(robot, "group", {"name": digit}) for joint in digit_joints[digit]: ET.SubElement(group, "joint", {"name": joint["name"]}) fingers_group = ET.SubElement(robot, "group", {"name": "fingers"}) for finger in chain.FINGERS: ET.SubElement(fingers_group, "group", {"name": finger}) hand_group = ET.SubElement(robot, "group", {"name": "hand"}) for digit in digits: ET.SubElement(hand_group, "group", {"name": digit}) 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": "hand"}) for joint_name in joint_order: ET.SubElement( state, "joint", {"name": joint_name, "value": _fmt(math.radians(pose_deg[joint_name]))}, ) ET.SubElement( robot, "end_effector", { "name": "hand_eef", "parent_link": "palm", "group": "hand", }, ) 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