Skip to content

Commit eba7d18

Browse files
refactor(core): address API design findings for ergonomics and stability
- Renamed DEFAULT_GRAVITY to GRAVITY_ENABLED everywhere. - Updated InertiaTensor with optional off-diagonals (reordered) and removed .zero() in favor of .stability_floor(). - Added SensorType.RAY as an alias for Gazebo Classic compatibility. - Updated SemanticBuilder methods to return self and introduced .done(). - Fixed cycle detection returning NO_ROOT instead of HAS_CYCLE in robot_builder.py. - Updated tests to reflect all API changes and validated clean suite. Signed-off-by: arounamounchili <patouossa.mounchili@gmail.com>
1 parent 77399d4 commit eba7d18

16 files changed

Lines changed: 83 additions & 70 deletions

File tree

CONTRIBUTING.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -216,7 +216,7 @@ def test_link_creation():
216216
name="test_link",
217217
visuals=[],
218218
collisions=[],
219-
inertial=Inertial(mass=1.0, inertia=InertiaTensor.zero())
219+
inertial=Inertial(mass=1.0, inertia=InertiaTensor.stability_floor())
220220
)
221221
assert link.name == "test_link"
222222
assert link.inertial.mass == 1.0

core/src/linkforge/core/__init__.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -47,9 +47,9 @@
4747

4848
# Core Infrastructure: Constants and Exceptions
4949
from .constants import (
50-
DEFAULT_GRAVITY,
5150
DEFAULT_LINK_MASS,
5251
EPSILON,
52+
GRAVITY_ENABLED,
5353
PI,
5454
)
5555
from .exceptions import (
@@ -293,7 +293,7 @@
293293
# Constants
294294
"PI",
295295
"EPSILON",
296-
"DEFAULT_GRAVITY",
296+
"GRAVITY_ENABLED",
297297
"DEFAULT_LINK_MASS",
298298
# Logging
299299
"get_logger",

core/src/linkforge/core/composer/link_builder.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -983,7 +983,7 @@ def _finalize_inertial(self) -> Inertial | None:
983983
if l_state.inertial_origin is None:
984984
l_state.inertial_origin = source_origin
985985
else:
986-
l_state.inertia = InertiaTensor.zero()
986+
l_state.inertia = InertiaTensor.stability_floor()
987987

988988
return Inertial(
989989
mass=l_state.mass,

core/src/linkforge/core/composer/robot_builder.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -230,16 +230,16 @@ def build(self, validate: bool = True) -> Robot:
230230
self._active_link_builders.clear()
231231

232232
if validate:
233-
# Trigger root search to verify connectivity (raises error if no root)
234-
_ = self.robot.root_link
235-
236233
if self.robot.has_cycle:
237234
raise RobotValidationError(
238235
ValidationErrorCode.HAS_CYCLE,
239236
"Robot kinematic chain contains a cycle (not supported in URDF)",
240237
target="KinematicTree",
241238
)
242239

240+
# Trigger root search to verify connectivity (raises error if no root)
241+
_ = self.robot.root_link
242+
243243
return self.robot
244244

245245
def export_urdf(self, validate: bool = True, pretty_print: bool = True) -> str:

core/src/linkforge/core/composer/semantic_builder.py

Lines changed: 26 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,10 @@ def __init__(self, builder: IComposer) -> None:
3636
"""Initialize semantic builder."""
3737
self._builder = builder
3838

39+
def done(self) -> IComposer:
40+
"""Return to the parent RobotBuilder."""
41+
return self._builder
42+
3943
def group(
4044
self,
4145
name: str,
@@ -45,7 +49,7 @@ def group(
4549
subgroups: list[str] | None = None,
4650
base_link: str | None = None,
4751
tip_link: str | None = None,
48-
) -> IComposer:
52+
) -> SemanticBuilder:
4953
"""Define a planning group for MoveIt.
5054
5155
Args:
@@ -75,11 +79,11 @@ def group(
7579

7680
semantic = self._builder.robot.semantic
7781
self._builder.robot.semantic = replace(semantic, groups=tuple(semantic.groups) + (group,))
78-
return self._builder
82+
return self
7983

8084
def group_state(
8185
self, name: str, group: str, values: dict[str, float | tuple[float, ...]]
82-
) -> IComposer:
86+
) -> SemanticBuilder:
8387
"""Define a named state (e.g. 'home') for a planning group.
8488
8589
Args:
@@ -98,11 +102,11 @@ def group_state(
98102
self._builder.robot.semantic = replace(
99103
semantic, group_states=tuple(semantic.group_states) + (state,)
100104
)
101-
return self._builder
105+
return self
102106

103107
def end_effector(
104108
self, name: str, group: str, parent_link: str, parent_group: str | None = None
105-
) -> IComposer:
109+
) -> SemanticBuilder:
106110
"""Define an end effector for MoveIt.
107111
108112
Args:
@@ -119,9 +123,9 @@ def end_effector(
119123
self._builder.robot.semantic = replace(
120124
semantic, end_effectors=tuple(semantic.end_effectors) + (ee,)
121125
)
122-
return self._builder
126+
return self
123127

124-
def passive_joint(self, name: str) -> IComposer:
128+
def passive_joint(self, name: str) -> SemanticBuilder:
125129
"""Mark a joint as passive (not actuated) for MoveIt.
126130
127131
Args:
@@ -135,15 +139,15 @@ def passive_joint(self, name: str) -> IComposer:
135139
self._builder.robot.semantic = replace(
136140
semantic, passive_joints=tuple(semantic.passive_joints) + (pj,)
137141
)
138-
return self._builder
142+
return self
139143

140144
def virtual_joint(
141145
self,
142146
name: str,
143147
child_link: str,
144148
parent_frame: str = "world",
145149
joint_type: str = SRDF_VJOIN_FIXED,
146-
) -> IComposer:
150+
) -> SemanticBuilder:
147151
"""Define a virtual joint connecting the robot to the world frame.
148152
149153
Args:
@@ -162,11 +166,11 @@ def virtual_joint(
162166
self._builder.robot.semantic = replace(
163167
semantic, virtual_joints=tuple(semantic.virtual_joints) + (vj,)
164168
)
165-
return self._builder
169+
return self
166170

167171
def disable_collisions(
168172
self, link1: str, link2: str, reason: str = SRDF_REASON_ADJACENT
169-
) -> IComposer:
173+
) -> SemanticBuilder:
170174
"""Instruct MoveIt to ignore collisions between two specific links.
171175
172176
Args:
@@ -181,9 +185,11 @@ def disable_collisions(
181185
self._builder.robot.semantic = replace(
182186
semantic, disabled_collisions=tuple(semantic.disabled_collisions) + (dc,)
183187
)
184-
return self._builder
188+
return self
185189

186-
def enable_collisions(self, link1: str, link2: str, reason: str | None = None) -> IComposer:
190+
def enable_collisions(
191+
self, link1: str, link2: str, reason: str | None = None
192+
) -> SemanticBuilder:
187193
"""Explicitly re-enable collision checking between two specific links.
188194
189195
Args:
@@ -198,9 +204,9 @@ def enable_collisions(self, link1: str, link2: str, reason: str | None = None) -
198204
self._builder.robot.semantic = replace(
199205
semantic, enabled_collisions=tuple(semantic.enabled_collisions) + (ec,)
200206
)
201-
return self._builder
207+
return self
202208

203-
def disable_default_collisions(self, link: str) -> IComposer:
209+
def disable_default_collisions(self, link: str) -> SemanticBuilder:
204210
"""Disable all default collisions for a specific link.
205211
206212
Args:
@@ -214,9 +220,9 @@ def disable_default_collisions(self, link: str) -> IComposer:
214220
semantic,
215221
no_default_collision_links=tuple(semantic.no_default_collision_links) + (link,),
216222
)
217-
return self._builder
223+
return self
218224

219-
def joint_property(self, joint_name: str, property_name: str, value: str) -> IComposer:
225+
def joint_property(self, joint_name: str, property_name: str, value: str) -> SemanticBuilder:
220226
"""Add a custom property/metadata to a joint.
221227
222228
Args:
@@ -232,9 +238,9 @@ def joint_property(self, joint_name: str, property_name: str, value: str) -> ICo
232238
self._builder.robot.semantic = replace(
233239
semantic, joint_properties=tuple(semantic.joint_properties) + (jp,)
234240
)
235-
return self._builder
241+
return self
236242

237-
def approximate_link_collision(self, link: str, spheres: list[SrdfSphere]) -> IComposer:
243+
def approximate_link_collision(self, link: str, spheres: list[SrdfSphere]) -> SemanticBuilder:
238244
"""Add sphere-based collision approximation for a link.
239245
240246
Args:
@@ -250,4 +256,4 @@ def approximate_link_collision(self, link: str, spheres: list[SrdfSphere]) -> IC
250256
semantic,
251257
link_sphere_approximations=tuple(semantic.link_sphere_approximations) + (lsa,),
252258
)
253-
return self._builder
259+
return self

core/src/linkforge/core/constants.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,7 @@
9090
DEFAULT_CONTACT_KD: Final[float] = 1.0 # N·s/m
9191

9292
# Simulation toggles
93-
DEFAULT_GRAVITY: Final[bool] = True
93+
GRAVITY_ENABLED: Final[bool] = True
9494
DEFAULT_SELF_COLLIDE: Final[bool] = False
9595

9696

@@ -178,6 +178,7 @@
178178
SENSOR_CAMERA: Final[str] = "camera"
179179
SENSOR_DEPTH_CAMERA: Final[str] = "depth_camera"
180180
SENSOR_LIDAR: Final[str] = "lidar"
181+
SENSOR_RAY: Final[str] = "ray"
181182
SENSOR_GPU_LIDAR: Final[str] = "gpu_lidar"
182183
SENSOR_IMU: Final[str] = "imu"
183184
SENSOR_GPS: Final[str] = "gps"

core/src/linkforge/core/models/link.py

Lines changed: 16 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -21,9 +21,9 @@
2121
DEFAULT_CONTACT_KP,
2222
DEFAULT_FRICTION_MU,
2323
DEFAULT_FRICTION_MU2,
24-
DEFAULT_GRAVITY,
2524
DEFAULT_SELF_COLLIDE,
2625
EPSILON,
26+
GRAVITY_ENABLED,
2727
MIN_REASONABLE_INERTIA,
2828
)
2929
from ..exceptions import RobotPhysicsError, RobotValidationError, ValidationErrorCode
@@ -46,11 +46,11 @@ class InertiaTensor:
4646
"""
4747

4848
ixx: float
49-
ixy: float
50-
ixz: float
5149
iyy: float
52-
iyz: float
5350
izz: float
51+
ixy: float = 0.0
52+
ixz: float = 0.0
53+
iyz: float = 0.0
5454

5555
def __post_init__(self) -> None:
5656
"""Validate inertia tensor values."""
@@ -78,15 +78,16 @@ def __post_init__(self) -> None:
7878
)
7979

8080
@classmethod
81-
def zero(cls) -> InertiaTensor:
82-
"""Create a minimal valid inertia tensor (for massless links)."""
81+
def stability_floor(cls) -> InertiaTensor:
82+
"""Create a minimal valid inertia tensor for stability (e.g. massless links).
83+
84+
Returns a tensor with MIN_REASONABLE_INERTIA on the diagonals to prevent
85+
physics solver instability.
86+
"""
8387
return cls(
84-
MIN_REASONABLE_INERTIA,
85-
0.0,
86-
0.0,
87-
MIN_REASONABLE_INERTIA,
88-
0.0,
89-
MIN_REASONABLE_INERTIA,
88+
ixx=MIN_REASONABLE_INERTIA,
89+
iyy=MIN_REASONABLE_INERTIA,
90+
izz=MIN_REASONABLE_INERTIA,
9091
)
9192

9293

@@ -96,7 +97,7 @@ class Inertial:
9697

9798
mass: float
9899
origin: Transform = Transform.identity()
99-
inertia: InertiaTensor = field(default_factory=InertiaTensor.zero)
100+
inertia: InertiaTensor = field(default_factory=InertiaTensor.stability_floor)
100101

101102
def __post_init__(self) -> None:
102103
"""Validate inertial properties.
@@ -122,7 +123,7 @@ class LinkPhysics:
122123
"""
123124

124125
self_collide: bool = DEFAULT_SELF_COLLIDE
125-
gravity: bool = DEFAULT_GRAVITY
126+
gravity: bool = GRAVITY_ENABLED
126127
mu: float = DEFAULT_FRICTION_MU
127128
mu2: float = DEFAULT_FRICTION_MU2
128129
kp: float = DEFAULT_CONTACT_KP
@@ -211,7 +212,7 @@ def mass(self) -> float:
211212
@property
212213
def inertia(self) -> InertiaTensor:
213214
"""Get link inertia tensor (zero tensor if not defined)."""
214-
return self.inertial.inertia if self.inertial else InertiaTensor.zero()
215+
return self.inertial.inertia if self.inertial else InertiaTensor.stability_floor()
215216

216217
@property
217218
def inertial_origin(self) -> Transform:

core/src/linkforge/core/models/sensor.py

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -50,18 +50,25 @@
5050
SENSOR_GPU_LIDAR,
5151
SENSOR_IMU,
5252
SENSOR_LIDAR,
53+
SENSOR_RAY,
5354
)
5455
from ..exceptions import RobotValidationError, ValidationErrorCode
5556
from .gazebo import GazeboPlugin
5657
from .geometry import Transform
5758

5859

5960
class SensorType(StrEnum):
60-
"""Enumeration of supported sensor types in the LinkForge IR."""
61+
"""Enumeration of supported sensor types in the LinkForge IR.
62+
63+
Note on LIDAR vs RAY:
64+
- LIDAR is the modern Gazebo Harmonic/GZ standard ("lidar")
65+
- RAY is provided for Gazebo Classic / ROS1 compatibility ("ray")
66+
"""
6167

6268
CAMERA = SENSOR_CAMERA
6369
DEPTH_CAMERA = SENSOR_DEPTH_CAMERA
6470
LIDAR = SENSOR_LIDAR
71+
RAY = SENSOR_RAY
6572
GPU_LIDAR = SENSOR_GPU_LIDAR
6673
IMU = SENSOR_IMU
6774
GPS = SENSOR_GPS

core/src/linkforge/core/parsers/xml_base.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -317,8 +317,8 @@ def _parse_inertial_element(self, inertial_elem: ET.Element | None) -> Inertial
317317
inertia = InertiaTensor(ixx=ixx, iyy=iyy, izz=izz, ixy=ixy, ixz=ixz, iyz=iyz)
318318
except RobotModelError:
319319
# If triangle inequality is still violated, fall back to minimal valid
320-
inertia = InertiaTensor.zero()
320+
inertia = InertiaTensor.stability_floor()
321321
else:
322-
inertia = InertiaTensor.zero()
322+
inertia = InertiaTensor.stability_floor()
323323

324324
return Inertial(mass=mass, origin=origin, inertia=inertia)

docs/source/reference/api/generators.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ robot = Robot(
6565
name="base_link",
6666
inertial=Inertial(
6767
mass=10.0,
68-
inertia=InertiaTensor(ixx=1.0, ixy=0.0, ixz=0.0, iyy=1.0, iyz=0.0, izz=1.0)
68+
inertia=InertiaTensor(ixx=1.0, iyy=1.0, izz=1.0)
6969
)
7070
)
7171
]

0 commit comments

Comments
 (0)