|
13 | 13 | ] |
14 | 14 |
|
15 | 15 | import xml.etree.ElementTree as ET |
| 16 | +from collections import defaultdict |
16 | 17 | from pathlib import Path |
17 | 18 | from typing import Any |
18 | 19 |
|
|
22 | 23 | from ..models.gazebo import GazeboElement, GazeboPlugin |
23 | 24 | from ..models.geometry import Transform |
24 | 25 | from ..models.joint import Joint, JointType |
25 | | -from ..models.link import Collision, Link, Visual |
| 26 | +from ..models.link import Collision, Link, LinkPhysics, Visual |
26 | 27 | from ..models.material import Material |
27 | 28 | from ..models.robot import Robot |
28 | 29 | from ..models.ros2_control import Ros2Control |
@@ -773,19 +774,13 @@ def _add_gazebo_element(self, parent: ET.Element, gazebo_elem: GazeboElement) -> |
773 | 774 | xml_add_text(gz_elem, "material", gazebo_elem.material) |
774 | 775 |
|
775 | 776 | # Add boolean properties |
776 | | - self._add_optional_bool_element(gz_elem, "selfCollide", gazebo_elem.self_collide) |
777 | 777 | self._add_optional_bool_element(gz_elem, "static", gazebo_elem.static) |
778 | | - self._add_optional_bool_element(gz_elem, "gravity", gazebo_elem.gravity) |
779 | 778 | self._add_optional_bool_element(gz_elem, "provideFeedback", gazebo_elem.provide_feedback) |
780 | 779 | self._add_optional_bool_element( |
781 | 780 | gz_elem, "implicitSpringDamper", gazebo_elem.implicit_spring_damper |
782 | 781 | ) |
783 | 782 |
|
784 | 783 | # Add numeric properties |
785 | | - self._add_optional_numeric_element(gz_elem, "mu1", gazebo_elem.mu1) |
786 | | - self._add_optional_numeric_element(gz_elem, "mu2", gazebo_elem.mu2) |
787 | | - self._add_optional_numeric_element(gz_elem, "kp", gazebo_elem.kp) |
788 | | - self._add_optional_numeric_element(gz_elem, "kd", gazebo_elem.kd) |
789 | 784 | self._add_optional_numeric_element(gz_elem, "stopCfm", gazebo_elem.stop_cfm) |
790 | 785 | self._add_optional_numeric_element(gz_elem, "stopErp", gazebo_elem.stop_erp) |
791 | 786 |
|
@@ -885,12 +880,75 @@ def add_gazebo(self, parent: ET.Element, robot: Robot) -> None: |
885 | 880 | parent: Parent XML element (robot) |
886 | 881 | robot: Robot model |
887 | 882 | """ |
888 | | - if robot.gazebo_elements: |
| 883 | + if robot.links or robot.gazebo_elements: |
889 | 884 | parent.append(ET.Comment(" Gazebo ")) |
890 | | - # Sort gazebo elements by reference for deterministic output |
891 | | - # Empty reference (global) comes first |
892 | | - for gazebo_elem in sorted(robot.gazebo_elements, key=lambda g: g.reference or ""): |
893 | | - self._add_gazebo_element(parent, gazebo_elem) |
| 885 | + |
| 886 | + grouped_elements: dict[str | None, list[GazeboElement]] = defaultdict(list) |
| 887 | + for gz in robot.gazebo_elements: |
| 888 | + grouped_elements[gz.reference].append(gz) |
| 889 | + |
| 890 | + # 1. Handle Link-level Gazebo tags (Physics + Extensions) |
| 891 | + for link in sorted(robot.links, key=lambda lnk: lnk.name): |
| 892 | + # Create a single tag for this link |
| 893 | + gz_tag = ET.SubElement(parent, "gazebo", reference=link.name) |
| 894 | + |
| 895 | + # Add Physics (Universal Truth) |
| 896 | + self._fill_link_physics(gz_tag, link.physics) |
| 897 | + |
| 898 | + # Add any explicit Gazebo elements for this link |
| 899 | + if link.name in grouped_elements: |
| 900 | + for elem in grouped_elements[link.name]: |
| 901 | + self._fill_gazebo_element(gz_tag, elem) |
| 902 | + # Mark as handled |
| 903 | + del grouped_elements[link.name] |
| 904 | + |
| 905 | + # 2. Handle Robot-level (reference=None) and other Gazebo tags |
| 906 | + # Sort by reference for deterministic output |
| 907 | + for ref in sorted(grouped_elements.keys(), key=lambda r: r or ""): |
| 908 | + attrib = {"reference": ref} if ref else {} |
| 909 | + gz_tag = ET.SubElement(parent, "gazebo", attrib) |
| 910 | + for elem in grouped_elements[ref]: |
| 911 | + self._fill_gazebo_element(gz_tag, elem) |
| 912 | + |
| 913 | + def _fill_link_physics(self, gz_elem: ET.Element, phys: LinkPhysics) -> None: |
| 914 | + """Fill an existing gazebo element with physics properties.""" |
| 915 | + # Boolean properties |
| 916 | + ET.SubElement(gz_elem, "selfCollide").text = "true" if phys.self_collide else "false" |
| 917 | + ET.SubElement(gz_elem, "gravity").text = "true" if phys.gravity else "false" |
| 918 | + |
| 919 | + # Friction parameters |
| 920 | + ET.SubElement(gz_elem, "mu1").text = format_float(phys.mu) |
| 921 | + ET.SubElement(gz_elem, "mu2").text = format_float(phys.mu2) |
| 922 | + |
| 923 | + # Contact parameters |
| 924 | + ET.SubElement(gz_elem, "kp").text = format_float(phys.kp) |
| 925 | + ET.SubElement(gz_elem, "kd").text = format_float(phys.kd) |
| 926 | + |
| 927 | + def _fill_gazebo_element(self, gz_elem: ET.Element, gazebo_elem: GazeboElement) -> None: |
| 928 | + """Fill an existing gazebo element with properties from a GazeboElement model.""" |
| 929 | + # Add material if specified |
| 930 | + if gazebo_elem.material is not None: |
| 931 | + xml_add_text(gz_elem, "material", gazebo_elem.material) |
| 932 | + |
| 933 | + # Add boolean properties |
| 934 | + self._add_optional_bool_element(gz_elem, "static", gazebo_elem.static) |
| 935 | + self._add_optional_bool_element(gz_elem, "provideFeedback", gazebo_elem.provide_feedback) |
| 936 | + self._add_optional_bool_element( |
| 937 | + gz_elem, "implicitSpringDamper", gazebo_elem.implicit_spring_damper |
| 938 | + ) |
| 939 | + |
| 940 | + # Add numeric properties |
| 941 | + self._add_optional_numeric_element(gz_elem, "stopCfm", gazebo_elem.stop_cfm) |
| 942 | + self._add_optional_numeric_element(gz_elem, "stopErp", gazebo_elem.stop_erp) |
| 943 | + |
| 944 | + # Add custom properties (sort by key for deterministic output) |
| 945 | + for key in sorted(gazebo_elem.properties.keys()): |
| 946 | + prop_elem = ET.SubElement(gz_elem, key) |
| 947 | + prop_elem.text = gazebo_elem.properties[key] |
| 948 | + |
| 949 | + # Add plugins (sort by name for deterministic output) |
| 950 | + for plugin in sorted(gazebo_elem.plugins, key=lambda p: p.name): |
| 951 | + self._add_gazebo_plugin_element(gz_elem, plugin) |
894 | 952 |
|
895 | 953 | def add_sensors(self, parent: ET.Element, robot: Robot) -> None: |
896 | 954 | """Add Sensors section to parent element. |
|
0 commit comments