Jerry
267 lines
10 KiB
Python
267 lines
10 KiB
Python
"""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/<link>.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
|