[Bug]: Sensor frames are reordered after buildModel() instead of preserving insertion order

[Ipuch]

🐞 Bug

This is very minor.

When building a Pinocchio model from a ModelGraph, sensor frames are not preserved in their insertion order. Instead, they appear to be reorganized in a weird way.

As a biomechanist, this creates problems when trying to match sensor frames ordering with external data (e.g., marker trajectories from motion capture files) where the order is significant.

Reproduction steps

Pinocchio version: 3.8
Python version: 3.14
Operating System: ubuntu

import numpy as np
import pinocchio as pin

# Create model graph
model_graph = pin.graph.ModelGraph()

# Add root and two segments
model_graph.addBody("/ground", pin.Inertia(0.0, np.zeros(3), np.zeros((3, 3))))
model_graph.addBody("/segment1", pin.Inertia(1.0, np.zeros(3), np.eye(3)))
model_graph.addBody("/segment2", pin.Inertia(1.0, np.zeros(3), np.eye(3)))

# Connect segments with joints
model_graph.addJoint(
    "joint1",
    pin.graph.JointRevolute(np.array([0, 0, 1])),
    "/ground",
    pin.SE3.Identity(),
    "/segment1",
    pin.SE3.Identity()
)

model_graph.addJoint(
    "joint2",
    pin.graph.JointRevolute(np.array([0, 0, 1])),
    "/segment1",
    pin.SE3.Identity(),
    "/segment2",
    pin.SE3.Identity()
)

# Add three sensor frames on segment1
for i in range(3):
    frame_name = f"marker_seg1_{i}"
    model_graph.addFrame(frame_name, pin.graph.SensorFrame())
    model_graph.addJoint(
        f"{frame_name}_link",
        pin.graph.JointFixed(),
        "/segment1",
        pin.SE3.Identity(),
        frame_name,
        pin.SE3(np.eye(3), np.array([0.1 * i, 0, 0]))
    )

# Add one sensor frame on segment2
frame_name = "marker_seg2_0"
model_graph.addFrame(frame_name, pin.graph.SensorFrame())
model_graph.addJoint(
    f"{frame_name}_link",
    pin.graph.JointFixed(),
    "/segment2",
    pin.SE3.Identity(),
    frame_name,
    pin.SE3(np.eye(3), np.array([0.1, 0, 0]))
)

# Build the model
model = pin.graph.buildModel(model_graph, "/ground", pin.SE3.Identity())

# Print actual sensor frame order
print("Expected order: marker_seg1_0, marker_seg1_1, marker_seg1_2, marker_seg2_0")
print("\nActual order in the model:")
for frame_id, frame in enumerate(model.frames):
    if frame.type == pin.SENSOR:
        print(f"  Frame {frame_id}: {frame.name}")

Expected behavior: sensor frames should appear in the model in the same order they were added to the ModelGraph:

  Frame 6: marker_seg1_0
  Frame 7: marker_seg1_1
  Frame 8: marker_seg1_2
  Frame 9: marker_seg2_0

Actual Behavior: sensor frames are reordered.

  Frame 6: marker_seg2_0
  Frame 7: marker_seg1_0
  Frame 8: marker_seg1_1
  Frame 9: marker_seg1_2

Workaround: Currently, we need to manually create a mapping from frame names to indices after model construction, but it's not satisfactory.

Relevant log output / Error message

System Info

Pinocchio version: 3.8
Python version: 3.14
Operating System: ubuntu

Checklist

• I have checked that all my packages are installed from the same package manager (conda-forge, Nix, PyPI, robotpkg, ROS, ...)
• I have checked that there is no similar issue/discussion in the repo
• I have provided a minimal and working example to reproduce the bug
• I have used Markdown code blocks for both code and stack traces/compiler errors.

[jorisv]

Hello @Ipuch,

Pinocchio joint graph should be created with a DFS (depth-first search) algorithm.
So we will proceed ground, segment1, segment2, marker_seg2_0, marker_seg1_[0-2].

The pinocchio requirement is only on joint, so it's not mandatory to proceed frame the same way.

We didn't plan to preserve construction order since some joint (mimic) could benefice from some internal graph reordering to work properly. Also, URDF parsing was not preserving order since it was sorting joint by alphabetic order.
But we can discuss about adding also this kind of constraint in ModelGraph::buildModel.

I'm curious why you judge the manual mapping no satisfactory. That some kind of thing we us to do when working with robot sensor feedback.