Skip to content
Open
4 changes: 2 additions & 2 deletions CONTRIBUTING.md
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
73 changes: 65 additions & 8 deletions core/src/linkforge_core/composer/link_builder.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)


Expand Down Expand Up @@ -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.

Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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)

Expand Down
25 changes: 25 additions & 0 deletions core/src/linkforge_core/constants.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
"""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
82 changes: 70 additions & 12 deletions core/src/linkforge_core/generators/urdf_generator.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
]

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

Expand All @@ -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
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -885,12 +880,75 @@ def add_gazebo(self, parent: ET.Element, robot: Robot) -> None:
parent: Parent XML element (robot)
robot: Robot model
"""
if robot.gazebo_elements:
if robot.links or 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)

# 1. Handle Link-level Gazebo tags (Physics + Extensions)
for link in sorted(robot.links, key=lambda lnk: lnk.name):
# Create a single tag for this link
gz_tag = ET.SubElement(parent, "gazebo", reference=link.name)

# Add Physics (Universal Truth)
self._fill_link_physics(gz_tag, link.physics)

# Add any explicit Gazebo elements for this link
if link.name in grouped_elements:
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.
Expand Down
3 changes: 2 additions & 1 deletion core/src/linkforge_core/models/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -82,6 +82,7 @@
"Visual",
"Collision",
"Link",
"LinkPhysics",
# Joint
"JointType",
"JointLimits",
Expand Down
12 changes: 2 additions & 10 deletions core/src/linkforge_core/models/gazebo.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,24 +19,16 @@ class GazeboElement:
properties: dict[str, str] = field(default_factory=dict)
plugins: list[GazeboPlugin] = field(default_factory=list)

# Common properties for links
# Common properties for links (platform-specific)
material: str | None = None # Gazebo material (e.g., "Gazebo/Red")
self_collide: bool | None = None
static: bool | None = None
gravity: bool | None = None

# Common properties for joints
# Common properties for joints (platform-specific)
stop_cfm: float | None = None # Constraint force mixing for joint stops
stop_erp: float | None = None # Error reduction parameter for joint stops
provide_feedback: bool | None = None # Enable force-torque feedback
implicit_spring_damper: bool | None = None

# Friction parameters
mu1: float | None = None # Friction coefficient in first friction direction
mu2: float | None = None # Friction coefficient in second friction direction
kp: float | None = None # Contact stiffness
kd: float | None = None # Contact damping

def __post_init__(self) -> None:
"""Validate Gazebo element."""
# If reference is specified, it must be non-empty
Expand Down
Loading
Loading