Source code for linkforge.core.generators.srdf_generator

"""SRDF XML generator for LinkForge.

This module implements a generator to export LinkForge's semantic robot
description back to MoveIt-standard SRDF XML format.
"""

from __future__ import annotations

import xml.etree.ElementTree as ET
from pathlib import Path
from typing import Any

from .._utils.math_utils import format_float
from .._utils.xml_utils import create_xml_element, serialize_xml
from ..constants import (
    COMMENT_COLLISION_SPHERES,
    COMMENT_DISABLED_COLLISIONS,
    COMMENT_DISABLED_DEFAULT_COLLISIONS,
    COMMENT_ENABLED_COLLISIONS,
    COMMENT_END_EFFECTORS,
    COMMENT_GROUP_STATES,
    COMMENT_JOINT_PROPERTIES,
    COMMENT_PASSIVE_JOINTS,
    COMMENT_PLANNING_GROUPS,
    COMMENT_VIRTUAL_JOINTS,
)
from ..exceptions import RobotGeneratorError
from ..logging_config import get_logger
from ..models.robot import Robot
from ..models.srdf import (
    CollisionPair,
    EndEffector,
    GroupState,
    JointProperty,
    LinkSphereApproximation,
    PassiveJoint,
    PlanningGroup,
    VirtualJoint,
)
from ..validation import RobotValidator
from .xml_base import RobotXMLGenerator

logger = get_logger(__name__)


[docs] class SRDFGenerator(RobotXMLGenerator): """Semantic Robot Description Format (SRDF) generator."""
[docs] def __init__(self, pretty_print: bool = True, srdf_path: Path | None = None) -> None: """Initialize SRDF generator. Args: pretty_print: If True, format XML with indentation for readability (default: True) srdf_path: Path where SRDF will be saved. """ super().__init__(pretty_print=pretty_print, output_path=srdf_path)
[docs] def generate(self, robot: Robot, validate: bool = True, **kwargs: Any) -> str: """Generate SRDF XML string from robot. Args: robot: Robot model with semantic description. validate: Whether to validate robot structure before generation. **kwargs: Additional generation options (passed to serializer). Returns: SRDF XML as formatted string with proper indentation """ if validate: validator = RobotValidator() result = validator.validate(robot) if not result.is_valid: error_msgs = [str(issue) for issue in result.errors] raise RobotGeneratorError("Robot validation failed:\n" + "\n".join(error_msgs)) if robot.semantic is None: logger.warning(f"Robot '{robot.name}' has no semantic description to generate.") root = self.generate_robot_element(robot) version = kwargs.pop("version", None) return serialize_xml(root, pretty_print=self.pretty_print, version=version, **kwargs)
[docs] def generate_robot_element(self, robot: Robot) -> ET.Element: """Generate SRDF XML Element tree from robot.""" name = ( robot.semantic.robot_name if robot.semantic and robot.semantic.robot_name else robot.name ) root = ET.Element("robot", name=name) if not robot.semantic: return root semantic = robot.semantic self._add_virtual_joints(root, sorted(semantic.virtual_joints, key=lambda x: x.name)) self._add_groups(root, sorted(semantic.groups, key=lambda x: x.name)) self._add_group_states(root, sorted(semantic.group_states, key=lambda x: x.name)) self._add_end_effectors(root, sorted(semantic.end_effectors, key=lambda x: x.name)) self._add_passive_joints(root, sorted(semantic.passive_joints, key=lambda x: x.name)) self._add_disabled_collisions( root, sorted(semantic.disabled_collisions, key=lambda x: (x.link1, x.link2)) ) self._add_enabled_collisions( root, sorted(semantic.enabled_collisions, key=lambda x: (x.link1, x.link2)) ) self._add_no_default_collision_links(root, sorted(semantic.no_default_collision_links)) self._add_link_sphere_approximations( root, sorted(semantic.link_sphere_approximations, key=lambda x: x.link) ) self._add_joint_properties( root, sorted(semantic.joint_properties, key=lambda x: (x.joint_name, x.property_name)) ) return root
def _add_virtual_joints(self, root: ET.Element, virtual_joints: list[VirtualJoint]) -> None: """Add virtual joint elements to root.""" if not virtual_joints: return root.append(ET.Comment(COMMENT_VIRTUAL_JOINTS)) for vj in virtual_joints: create_xml_element( root, "virtual_joint", formatter=self._format_value, name=vj.name, type=vj.type, parent_frame=vj.parent_frame, child_link=vj.child_link, ) def _add_groups(self, root: ET.Element, groups: list[PlanningGroup]) -> None: """Add planning group elements to root.""" if not groups: return root.append(ET.Comment(COMMENT_PLANNING_GROUPS)) for group in groups: group_elem = ET.SubElement(root, "group", name=group.name) for link_name in group.links: ET.SubElement(group_elem, "link", name=link_name) for joint_name in group.joints: ET.SubElement(group_elem, "joint", name=joint_name) for chain in group.chains: ET.SubElement( group_elem, "chain", base_link=chain.base_link, tip_link=chain.tip_link ) for subgroup in group.subgroups: ET.SubElement(group_elem, "group", name=subgroup) def _add_group_states(self, root: ET.Element, states: list[GroupState]) -> None: """Add group state elements to root.""" if not states: return root.append(ET.Comment(COMMENT_GROUP_STATES)) for state in states: state_elem = ET.SubElement(root, "group_state", name=state.name, group=state.group) for j_name in sorted(state.joint_values.keys()): j_vals = state.joint_values[j_name] val_str = " ".join(format_float(v) for v in j_vals) ET.SubElement(state_elem, "joint", name=j_name, value=val_str) def _add_end_effectors(self, root: ET.Element, end_effectors: list[EndEffector]) -> None: """Add end effector elements to root.""" if not end_effectors: return root.append(ET.Comment(COMMENT_END_EFFECTORS)) for ee in end_effectors: create_xml_element( root, "end_effector", formatter=self._format_value, name=ee.name, group=ee.group, parent_link=ee.parent_link, parent_group=ee.parent_group, ) def _add_passive_joints(self, root: ET.Element, passive_joints: list[PassiveJoint]) -> None: """Add passive joint elements to root.""" if not passive_joints: return root.append(ET.Comment(COMMENT_PASSIVE_JOINTS)) for pj in passive_joints: ET.SubElement(root, "passive_joint", name=pj.name) def _add_disabled_collisions( self, root: ET.Element, disabled_collisions: list[CollisionPair] ) -> None: """Add disabled collision elements to root.""" if not disabled_collisions: return root.append(ET.Comment(COMMENT_DISABLED_COLLISIONS)) for dc in disabled_collisions: create_xml_element( root, "disable_collisions", formatter=self._format_value, link1=dc.link1, link2=dc.link2, reason=dc.reason, ) def _add_enabled_collisions( self, root: ET.Element, enabled_collisions: list[CollisionPair] ) -> None: """Add enabled collision elements to root.""" if not enabled_collisions: return root.append(ET.Comment(COMMENT_ENABLED_COLLISIONS)) for ec in enabled_collisions: create_xml_element( root, "enable_collisions", formatter=self._format_value, link1=ec.link1, link2=ec.link2, reason=ec.reason, ) def _add_no_default_collision_links(self, root: ET.Element, links: list[str]) -> None: """Add disable default collisions elements to root.""" if not links: return root.append(ET.Comment(COMMENT_DISABLED_DEFAULT_COLLISIONS)) for link in links: ET.SubElement(root, "disable_default_collisions", link=link) def _add_link_sphere_approximations( self, root: ET.Element, approximations: list[LinkSphereApproximation] ) -> None: """Add link sphere approximation elements to root.""" if not approximations: return root.append(ET.Comment(COMMENT_COLLISION_SPHERES)) for lsa in approximations: lsa_elem = ET.SubElement(root, "link_sphere_approximation", link=lsa.link) for sphere in lsa.spheres: center_str = f"{format_float(sphere.center_x)} {format_float(sphere.center_y)} {format_float(sphere.center_z)}" create_xml_element( lsa_elem, "sphere", formatter=self._format_value, center=center_str, radius=sphere.radius, ) def _add_joint_properties(self, root: ET.Element, properties: list[JointProperty]) -> None: """Add joint property elements to root.""" if not properties: return root.append(ET.Comment(COMMENT_JOINT_PROPERTIES)) for jp in properties: ET.SubElement( root, "joint_property", joint_name=jp.joint_name, property_name=jp.property_name, value=jp.value, )