"""URDF joint/link introspection with the stdlib XML parser.
Pinocchio flattens some of this information away (fixed joints, mimic tags),
so the wizard reads the URDF directly for the joint table, slider limits and
validation, and leaves the dynamics to urdf_state_space.
"""
import xml.etree.ElementTree as ET
from dataclasses import dataclass, field
from typing import Dict, List, Optional, Tuple
MOVABLE_TYPES = ('revolute', 'continuous', 'prismatic', 'planar', 'floating')
[docs]
@dataclass
class JointInfo:
name: str
type: str
parent: str
child: str
axis: Optional[Tuple[float, float, float]] = None
lower: Optional[float] = None
upper: Optional[float] = None
effort: Optional[float] = None
velocity: Optional[float] = None
mimic: Optional[str] = None
movable: bool = False
[docs]
def to_dict(self) -> Dict:
return {
'name': self.name, 'type': self.type,
'parent': self.parent, 'child': self.child,
'axis': list(self.axis) if self.axis else None,
'lower': self.lower, 'upper': self.upper,
'effort': self.effort, 'velocity': self.velocity,
'mimic': self.mimic, 'movable': self.movable,
}
[docs]
@dataclass
class RobotInfo:
name: str
links: List[str]
joints: List[JointInfo]
materials: Dict = field(default_factory=dict)
@property
def movable_joints(self) -> List[JointInfo]:
return [j for j in self.joints if j.movable]
def _float_attr(elem, attr: str) -> Optional[float]:
if elem is None or attr not in elem.attrib:
return None
return float(elem.attrib[attr])
[docs]
def parse_robot(urdf_xml: str) -> RobotInfo:
"""Parse robot name, links and joints out of a URDF XML string."""
try:
root = ET.fromstring(urdf_xml)
except ET.ParseError as exc:
raise ValueError(f'URDF is not well-formed XML: {exc}') from exc
if root.tag != 'robot':
raise ValueError(f"expected a <robot> root element, got <{root.tag}>")
links = [link.attrib.get('name', '') for link in root.findall('link')]
joints = []
for j in root.findall('joint'):
jtype = j.attrib.get('type', '')
axis_elem = j.find('axis')
axis = None
if axis_elem is not None and 'xyz' in axis_elem.attrib:
axis = tuple(float(v) for v in axis_elem.attrib['xyz'].split())
limit = j.find('limit')
mimic_elem = j.find('mimic')
parent = j.find('parent')
child = j.find('child')
mimic = mimic_elem.attrib.get('joint') if mimic_elem is not None else None
joints.append(JointInfo(
name=j.attrib.get('name', ''),
type=jtype,
parent=parent.attrib.get('link', '') if parent is not None else '',
child=child.attrib.get('link', '') if child is not None else '',
axis=axis,
lower=_float_attr(limit, 'lower'),
upper=_float_attr(limit, 'upper'),
effort=_float_attr(limit, 'effort'),
velocity=_float_attr(limit, 'velocity'),
mimic=mimic,
movable=jtype in MOVABLE_TYPES and mimic is None,
))
return RobotInfo(
name=root.attrib.get('name', 'robot'),
links=links,
joints=joints,
)
[docs]
def parse_joints(urdf_xml: str) -> List[JointInfo]:
return parse_robot(urdf_xml).joints