Maintain collidable points ordering

ami-iit · Mar 4, 2025 · 67aa99f · 67aa99f
1 parent 36d8490
commit 67aa99f
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/src/jaxsim/api/kin_dyn_parameters.py b/src/jaxsim/api/kin_dyn_parameters.py
@@ -805,11 +805,14 @@ def build_from(model_description: ModelDescription) -> ContactParameters:
             "false",
             "0",
         }:
-            points, idxs = jnp.unique(all_points, axis=0, return_index=True)
-            selected_points = map(collidable_points.__getitem__, idxs)
+            _, idxs = jnp.unique(all_points, axis=0, return_index=True)
+            idxs = jnp.sort(idxs)
         else:
-            points = all_points
-            selected_points = iter(collidable_points)
+            idxs = jnp.arange(len(collidable_points))
+
+        # Select unique points and corresponding link indices.
+        points = all_points[idxs]
+        selected_points = map(collidable_points.__getitem__, idxs)
 
         # Extract the indices of the links to which the collidable points are rigidly attached.
         link_index_of_points = tuple(