From 67aa99f469838a42359e6c7825bf8291469fa4eb Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Tue, 4 Mar 2025 09:23:00 +0000 Subject: [PATCH] Maintain collidable points ordering --- src/jaxsim/api/kin_dyn_parameters.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/jaxsim/api/kin_dyn_parameters.py b/src/jaxsim/api/kin_dyn_parameters.py index 41764823a..6055558d3 100644 --- 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(