Merge branch 'main' into dependabot/uv/urllib3-2.7.0

Author: arounamounchili

CONTRIBUTING.md

@@ -214,8 +214,8 @@ def test_link_creation():
     """Test creating a link with valid parameters."""
     link = Link(
         name="test_link",
-        initial_visuals=[],
-        initial_collisions=[],
+        visuals=[],
+        collisions=[],
         inertial=Inertial(mass=1.0, inertia=InertiaTensor.zero())
     )
     assert link.name == "test_link"

core/src/linkforge_core/composer/link_builder.py

@@ -18,7 +18,7 @@
     JointSafetyController,
     JointType,
 )
-from ..models.link import Collision, Inertial, InertiaTensor, Link, Visual
+from ..models.link import Collision, Inertial, InertiaTensor, Link, LinkPhysics, Visual
 from ..models.material import Material
 from ..models.robot import Robot
 from ..models.ros2_control import Ros2ControlJoint
@@ -57,6 +57,7 @@ class _LinkState:
     visuals: list[Visual] = field(default_factory=list)
     collisions: list[Collision] = field(default_factory=list)
     sensors: list[Sensor] = field(default_factory=list)
+    physics: LinkPhysics = field(default_factory=LinkPhysics)
     gazebo_params: dict[str, Any] = field(default_factory=dict)
 
 
@@ -363,6 +364,49 @@ def prismatic(
         )
         return self._configure_joint(name, xyz, rpy)
 
+    def floating(
+        self,
+        name: str | None = None,
+        xyz: tuple[float, float, float] | None = None,
+        rpy: tuple[float, float, float] | None = None,
+    ) -> LinkBuilder:
+        """Configure the connection as a FLOATING (6 DOF) joint.
+
+        Args:
+            name: Unique joint name.
+            xyz: Joint origin translation.
+            rpy: Joint origin rotation.
+
+        Returns:
+            The LinkBuilder instance.
+        """
+        self._check_not_committed()
+        self._joint.type = JointType.FLOATING
+        return self._configure_joint(name, xyz, rpy)
+
+    def planar(
+        self,
+        axis: tuple[float, float, float],
+        name: str | None = None,
+        xyz: tuple[float, float, float] | None = None,
+        rpy: tuple[float, float, float] | None = None,
+    ) -> LinkBuilder:
+        """Configure the connection as a PLANAR joint.
+
+        Args:
+            axis: Plane normal unit vector.
+            name: Unique joint name.
+            xyz: Joint origin translation.
+            rpy: Joint origin rotation.
+
+        Returns:
+            The LinkBuilder instance.
+        """
+        self._check_not_committed()
+        self._joint.type = JointType.PLANAR
+        self._joint.axis = self._normalize_axis(axis)
+        return self._configure_joint(name, xyz, rpy)
+
     def dynamics(self, damping: float = 0.0, friction: float = 0.0) -> LinkBuilder:
         """Set the physical dynamics for the joint.
 
@@ -431,21 +475,33 @@ def calibration(self, rising: float | None = None, falling: float | None = None)
         self._joint.calibration = JointCalibration(rising=rising, falling=falling)
         return self
 
-    def simulation(self, **kwargs: Any) -> LinkBuilder:
-        """Set Gazebo-specific simulation properties for this link.
+    def physics(self, **kwargs: Any) -> LinkBuilder:
+        """Set surface and contact physics properties for this link.
+
+        Supports both typed LinkPhysics fields and raw engine-specific parameters.
 
         Common arguments:
             self_collide (bool): Enable self-collision.
             gravity (bool): Enable gravity.
-            static (bool): Mark link as static.
-            mu1, mu2 (float): Friction coefficients.
+            mu, mu2 (float): Friction coefficients.
             kp, kd (float): Contact stiffness and damping.
 
         Returns:
             The LinkBuilder instance.
         """
         self._check_not_committed()
-        self._link.gazebo_params.update(kwargs)
+
+        phys_fields = {f.name for f in LinkPhysics.__dataclass_fields__.values()}
+        phys_updates = {k: v for k, v in kwargs.items() if k in phys_fields}
+
+        if phys_updates:
+            self._link.physics = replace(self._link.physics, **phys_updates)
+
+        # Store non-physics fields in gazebo_params
+        remaining_kwargs = {k: v for k, v in kwargs.items() if k not in phys_fields}
+        if remaining_kwargs:
+            self._link.gazebo_params.update(remaining_kwargs)
+
         return self
 
     def _configure_joint(
@@ -837,9 +893,10 @@ def _finalize_link(self, inertial: Inertial | None) -> None:
         l_state = self._link
         link = Link(
             name=self._link_name,
-            initial_visuals=l_state.visuals,
-            initial_collisions=l_state.collisions,
+            visuals=l_state.visuals,
+            collisions=l_state.collisions,
             inertial=inertial,
+            physics=l_state.physics,
         )
         self._builder.robot.add_link(link)
 
@@ -914,11 +971,16 @@ def _finalize_ros2_control(self, joint: Joint) -> None:
         else:
             target_system = self._builder.robot.ros2_controls[0]
 
-        target_system.joints.append(
-            Ros2ControlJoint(
-                name=joint.name,
-                command_interfaces=self._control_interfaces[0],
-                state_interfaces=self._control_interfaces[1],
-                parameters=self._control_interfaces[2],
-            )
+        new_system = replace(
+            target_system,
+            joints=(
+                *target_system.joints,
+                Ros2ControlJoint(
+                    name=joint.name,
+                    command_interfaces=self._control_interfaces[0],
+                    state_interfaces=self._control_interfaces[1],
+                    parameters=self._control_interfaces[2],
+                ),
+            ),
         )
+        self._builder.robot.update_ros2_control(new_system)

core/src/linkforge_core/constants.py

@@ -0,0 +1,50 @@
+"""Central constants for the LinkForge project."""
+
+from __future__ import annotations
+
+# Physics Defaults (Simulation)
+# ----------------------------
+
+# Default static friction coefficient (Coulomb)
+DEFAULT_FRICTION_MU = 1.0
+
+# Default dynamic friction coefficient (Coulomb)
+DEFAULT_FRICTION_MU2 = 1.0
+
+# Default contact stiffness (N/m)
+# 1e12 is the industry standard for 'hard' contact in Gazebo/GZ
+DEFAULT_CONTACT_KP = 1e12
+
+# Default contact damping (N s/m)
+DEFAULT_CONTACT_KD = 1.0
+
+# Default gravity inclusion
+DEFAULT_GRAVITY = True
+
+# Default self-collision inclusion
+DEFAULT_SELF_COLLIDE = False
+
+
+# XML and XACRO Namespaces
+# ----------------------------
+
+# Official XACRO namespace URIs
+XACRO_URIS = {
+    "http://www.ros.org/wiki/xacro",
+    "http://wiki.ros.org/xacro",
+    "http://ros.org/xacro",
+}
+
+# Standard prefix for internal structural processing
+XACRO_PREFIX = "xacro:"
+
+
+# Validation Limits (Sanity Checks)
+# ----------------------------
+
+# Maximum absolute value allowed for floats in robot models
+# 1e18 is safe for stiffness (kp) while preventing simulation-breaking overflows
+MAX_REASONABLE_FLOAT = 1e18
+
+# Maximum absolute value allowed for integers (IDs, sample counts, etc.)
+MAX_REASONABLE_INT = 1000000

core/src/linkforge_core/generators/urdf_generator.py

@@ -13,6 +13,7 @@
 ]
 
 import xml.etree.ElementTree as ET
+from collections import defaultdict
 from pathlib import Path
 from typing import Any
 
@@ -22,7 +23,7 @@
 from ..models.gazebo import GazeboElement, GazeboPlugin
 from ..models.geometry import Transform
 from ..models.joint import Joint, JointType
-from ..models.link import Collision, Link, Visual
+from ..models.link import Collision, Link, LinkPhysics, Visual
 from ..models.material import Material
 from ..models.robot import Robot
 from ..models.ros2_control import Ros2Control
@@ -773,19 +774,13 @@ def _add_gazebo_element(self, parent: ET.Element, gazebo_elem: GazeboElement) ->
             xml_add_text(gz_elem, "material", gazebo_elem.material)
 
         # Add boolean properties
-        self._add_optional_bool_element(gz_elem, "selfCollide", gazebo_elem.self_collide)
         self._add_optional_bool_element(gz_elem, "static", gazebo_elem.static)
-        self._add_optional_bool_element(gz_elem, "gravity", gazebo_elem.gravity)
         self._add_optional_bool_element(gz_elem, "provideFeedback", gazebo_elem.provide_feedback)
         self._add_optional_bool_element(
             gz_elem, "implicitSpringDamper", gazebo_elem.implicit_spring_damper
         )
 
         # Add numeric properties
-        self._add_optional_numeric_element(gz_elem, "mu1", gazebo_elem.mu1)
-        self._add_optional_numeric_element(gz_elem, "mu2", gazebo_elem.mu2)
-        self._add_optional_numeric_element(gz_elem, "kp", gazebo_elem.kp)
-        self._add_optional_numeric_element(gz_elem, "kd", gazebo_elem.kd)
         self._add_optional_numeric_element(gz_elem, "stopCfm", gazebo_elem.stop_cfm)
         self._add_optional_numeric_element(gz_elem, "stopErp", gazebo_elem.stop_erp)
 
@@ -885,12 +880,89 @@ def add_gazebo(self, parent: ET.Element, robot: Robot) -> None:
             parent: Parent XML element (robot)
             robot: Robot model
         """
-        if robot.gazebo_elements:
-            parent.append(ET.Comment(" Gazebo "))
-        # Sort gazebo elements by reference for deterministic output
-        # Empty reference (global) comes first
-        for gazebo_elem in sorted(robot.gazebo_elements, key=lambda g: g.reference or ""):
-            self._add_gazebo_element(parent, gazebo_elem)
+        grouped_elements: dict[str | None, list[GazeboElement]] = defaultdict(list)
+        for gz in robot.gazebo_elements:
+            grouped_elements[gz.reference].append(gz)
+
+        # 0. Check if we have any Gazebo content at all before adding header
+        default_physics = LinkPhysics()
+        has_modified_physics = any(lnk.physics != default_physics for lnk in robot.links)
+        if not robot.gazebo_elements and not has_modified_physics:
+            return
+
+        parent.append(ET.Comment(" Gazebo "))
+
+        # 1. Handle Link-level Gazebo tags (Physics + Extensions)
+        # We only generate these if physics are non-default or if there are explicit elements
+        for link in sorted(robot.links, key=lambda lnk: lnk.name):
+            has_explicit = link.name in grouped_elements
+            is_physics_modified = link.physics != default_physics
+
+            # Skip if nothing to export for this link
+            if not has_explicit and not is_physics_modified:
+                continue
+
+            # Create a single tag for this link
+            gz_tag = ET.SubElement(parent, "gazebo", reference=link.name)
+
+            # Add Physics (only if modified or if tag already created for explicit elements)
+            if is_physics_modified:
+                self._fill_link_physics(gz_tag, link.physics)
+
+            # Add any explicit Gazebo elements for this link
+            if has_explicit:
+                for elem in grouped_elements[link.name]:
+                    self._fill_gazebo_element(gz_tag, elem)
+                # Mark as handled
+                del grouped_elements[link.name]
+
+        # 2. Handle Robot-level (reference=None) and other Gazebo tags
+        # Sort by reference for deterministic output
+        for ref in sorted(grouped_elements.keys(), key=lambda r: r or ""):
+            attrib = {"reference": ref} if ref else {}
+            gz_tag = ET.SubElement(parent, "gazebo", attrib)
+            for elem in grouped_elements[ref]:
+                self._fill_gazebo_element(gz_tag, elem)
+
+    def _fill_link_physics(self, gz_elem: ET.Element, phys: LinkPhysics) -> None:
+        """Fill an existing gazebo element with physics properties."""
+        # Boolean properties
+        ET.SubElement(gz_elem, "selfCollide").text = "true" if phys.self_collide else "false"
+        ET.SubElement(gz_elem, "gravity").text = "true" if phys.gravity else "false"
+
+        # Friction parameters
+        ET.SubElement(gz_elem, "mu1").text = format_float(phys.mu)
+        ET.SubElement(gz_elem, "mu2").text = format_float(phys.mu2)
+
+        # Contact parameters
+        ET.SubElement(gz_elem, "kp").text = format_float(phys.kp)
+        ET.SubElement(gz_elem, "kd").text = format_float(phys.kd)
+
+    def _fill_gazebo_element(self, gz_elem: ET.Element, gazebo_elem: GazeboElement) -> None:
+        """Fill an existing gazebo element with properties from a GazeboElement model."""
+        # Add material if specified
+        if gazebo_elem.material is not None:
+            xml_add_text(gz_elem, "material", gazebo_elem.material)
+
+        # Add boolean properties
+        self._add_optional_bool_element(gz_elem, "static", gazebo_elem.static)
+        self._add_optional_bool_element(gz_elem, "provideFeedback", gazebo_elem.provide_feedback)
+        self._add_optional_bool_element(
+            gz_elem, "implicitSpringDamper", gazebo_elem.implicit_spring_damper
+        )
+
+        # Add numeric properties
+        self._add_optional_numeric_element(gz_elem, "stopCfm", gazebo_elem.stop_cfm)
+        self._add_optional_numeric_element(gz_elem, "stopErp", gazebo_elem.stop_erp)
+
+        # Add custom properties (sort by key for deterministic output)
+        for key in sorted(gazebo_elem.properties.keys()):
+            prop_elem = ET.SubElement(gz_elem, key)
+            prop_elem.text = gazebo_elem.properties[key]
+
+        # Add plugins (sort by name for deterministic output)
+        for plugin in sorted(gazebo_elem.plugins, key=lambda p: p.name):
+            self._add_gazebo_plugin_element(gz_elem, plugin)
 
     def add_sensors(self, parent: ET.Element, robot: Robot) -> None:
         """Add Sensors section to parent element.

core/src/linkforge_core/models/__init__.py

@@ -27,7 +27,7 @@
     JointSafetyController,
     JointType,
 )
-from .link import Collision, Inertial, InertiaTensor, Link, Visual
+from .link import Collision, Inertial, InertiaTensor, Link, LinkPhysics, Visual
 from .material import Color, Material
 from .robot import Robot
 from .ros2_control import Ros2Control, Ros2ControlJoint
@@ -56,7 +56,6 @@
     VirtualJoint,
 )
 from .transmission import (
-    HardwareInterface,
     Transmission,
     TransmissionActuator,
     TransmissionJoint,
@@ -82,6 +81,7 @@
     "Visual",
     "Collision",
     "Link",
+    "LinkPhysics",
     # Joint
     "JointType",
     "JointLimits",
@@ -107,7 +107,6 @@
     "Sensor",
     # Transmission
     "TransmissionType",
-    "HardwareInterface",
     "TransmissionJoint",
     "TransmissionActuator",
     "Transmission",