Jerry
285 lines
11 KiB
Python
285 lines
11 KiB
Python
"""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/<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 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
|