"""Control-oriented URDF validation, run automatically when a robot loads.
Every check returns a Finding dict {level, code, message, subject} with
level 'ok' | 'warn' | 'error'. Errors indicate the linearization would be
meaningless (bad inertia, disconnected tree); warnings flag things the user
should know about (mimic joints, missing limits) but do not block.
"""
import numpy as np
from .introspect import RobotInfo, parse_robot
def _finding(level: str, code: str, message: str, subject: str = '') -> dict:
return {'level': level, 'code': code, 'message': message,
'subject': subject}
def _inertia_checks(root, findings):
import xml.etree.ElementTree as ET # noqa: F401 (root is an Element)
for link in root.findall('link'):
name = link.attrib.get('name', '?')
inertial = link.find('inertial')
if inertial is None:
# Massless frame links (couplers, optical frames) are normal.
continue
mass_elem = inertial.find('mass')
mass = float(mass_elem.attrib['value']) if mass_elem is not None else 0.0
if mass < 0:
findings.append(_finding(
'error', 'negative_mass',
f'link {name!r} has negative mass {mass}', name))
continue
if mass == 0:
findings.append(_finding(
'warn', 'zero_mass',
f'link {name!r} has an <inertial> with zero mass', name))
continue
inertia = inertial.find('inertia')
if inertia is None:
findings.append(_finding(
'error', 'missing_inertia',
f'link {name!r} has mass but no <inertia> tensor', name))
continue
a = {k: float(inertia.attrib.get(k, 0.0))
for k in ('ixx', 'iyy', 'izz', 'ixy', 'ixz', 'iyz')}
I = np.array([
[a['ixx'], a['ixy'], a['ixz']],
[a['ixy'], a['iyy'], a['iyz']],
[a['ixz'], a['iyz'], a['izz']],
])
eig = np.linalg.eigvalsh(I)
if np.any(eig <= 0):
findings.append(_finding(
'error', 'inertia_not_pd',
f'link {name!r}: inertia tensor is not positive definite '
f'(principal moments {np.array2string(eig, precision=3)})',
name))
continue
# Triangle inequality on principal moments (physical realizability).
s = np.sort(eig)
if s[0] + s[1] < s[2] * (1 - 1e-9):
findings.append(_finding(
'error', 'inertia_triangle',
f'link {name!r}: inertia violates the triangle inequality '
f'(principal moments {np.array2string(s, precision=3)}); '
'no rigid body can have this tensor', name))
def _tree_checks(robot: RobotInfo, findings):
links = set(robot.links)
children = {j.child for j in robot.joints}
roots = links - children
if len(roots) > 1:
findings.append(_finding(
'error', 'disconnected_links',
f'kinematic tree has {len(roots)} roots {sorted(roots)}; '
'all links but one must be the child of some joint',
','.join(sorted(roots))))
for j in robot.joints:
for end, role in ((j.parent, 'parent'), (j.child, 'child')):
if end not in links:
findings.append(_finding(
'error', 'unknown_link',
f'joint {j.name!r}: {role} link {end!r} is not defined',
j.name))
def _joint_checks(robot: RobotInfo, findings):
for j in robot.joints:
if j.type in ('revolute', 'prismatic') and (
j.lower is None or j.upper is None):
findings.append(_finding(
'warn', 'missing_limits',
f'joint {j.name!r} ({j.type}) has no <limit lower/upper>; '
'sliders will default to ±π', j.name))
if j.mimic is not None:
findings.append(_finding(
'warn', 'mimic_joint',
f'joint {j.name!r} mimics {j.mimic!r}: Pinocchio ignores '
'<mimic> and treats it as an independent DoF, so the '
'linearized dynamics differ from the visual model', j.name))
if j.type == 'continuous':
findings.append(_finding(
'warn', 'continuous_joint',
f'joint {j.name!r} is continuous (nq=2 in Pinocchio); '
'setting q_eq for it is not supported in this version',
j.name))
if j.type == 'floating':
findings.append(_finding(
'warn', 'floating_joint',
f'joint {j.name!r} is a floating joint; consider the '
'floating_base option instead', j.name))
def _actuation_checks(root, robot: RobotInfo, findings):
movable = robot.movable_joints
if not movable:
findings.append(_finding(
'error', 'no_movable_joints',
'URDF has no movable joints; nothing to linearize'))
if root.find('transmission') is None and root.find('ros2_control') is None:
findings.append(_finding(
'ok', 'no_ros2_control',
'no <transmission>/<ros2_control> tags: actuation interfaces are '
'not declared in the URDF (informational; the wizard’s '
'actuated-joint selection is independent of them)'))
def _pinocchio_check(urdf_xml: str, findings):
try:
from urdf_state_space.state_space import _load_model
_load_model(urdf_xml)
findings.append(_finding(
'ok', 'pinocchio_build', 'Pinocchio model builds cleanly'))
except Exception as exc: # pinocchio raises various types
findings.append(_finding(
'error', 'pinocchio_build',
f'Pinocchio failed to build a model from this URDF: {exc}'))
[docs]
def validate_urdf(urdf_xml: str) -> list:
"""Run all control-oriented checks; returns a list of Finding dicts."""
import xml.etree.ElementTree as ET
findings = []
robot = parse_robot(urdf_xml) # raises ValueError on malformed XML
root = ET.fromstring(urdf_xml)
_inertia_checks(root, findings)
_tree_checks(robot, findings)
_joint_checks(robot, findings)
_actuation_checks(root, robot, findings)
_pinocchio_check(urdf_xml, findings)
if not any(f['level'] == 'error' for f in findings):
findings.insert(0, _finding(
'ok', 'summary',
f'{robot.name}: {len(robot.links)} links, '
f'{len(robot.movable_joints)} movable of {len(robot.joints)} '
'joints; masses and inertias are physically valid'))
return findings