From f0d27269fde940952307320530f6fa4161dc309c Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Wed, 13 Nov 2024 16:52:28 +0100 Subject: [PATCH] Refactor `test_api_contact.py` to use indices of enabled collidable points --- tests/test_api_contact.py | 40 +++++++++++++++++++++++++++++---------- 1 file changed, 30 insertions(+), 10 deletions(-) diff --git a/tests/test_api_contact.py b/tests/test_api_contact.py index 6456f2645..4a0882737 100644 --- a/tests/test_api_contact.py +++ b/tests/test_api_contact.py @@ -22,6 +22,15 @@ def test_contact_kinematics( velocity_representation=velocity_representation, ) + # Get the indices of the enabled collidable points. + indices_of_enabled_collidable_points = ( + model.kin_dyn_parameters.contact_parameters.indices_of_enabled_collidable_points + ) + + parent_link_idx_of_enabled_collidable_points = jnp.array( + model.kin_dyn_parameters.contact_parameters.body, dtype=int + )[indices_of_enabled_collidable_points] + # ===== # Tests # ===== @@ -34,7 +43,7 @@ def test_contact_kinematics( # Check that the orientation of the implicit contact frame matches with the # orientation of the link to which the contact point is attached. for contact_idx, index_of_parent_link in enumerate( - model.kin_dyn_parameters.contact_parameters.body + parent_link_idx_of_enabled_collidable_points ): assert W_H_C[contact_idx, 0:3, 0:3] == pytest.approx( W_H_L[index_of_parent_link][0:3, 0:3] @@ -74,29 +83,40 @@ def test_contact_jacobian_derivative( velocity_representation=velocity_representation, ) - # ===== - # Tests - # ===== + # Get the indices of the enabled collidable points. + indices_of_enabled_collidable_points = ( + model.kin_dyn_parameters.contact_parameters.indices_of_enabled_collidable_points + ) # Extract the parent link names and the poses of the contact points. parent_link_names = js.link.idxs_to_names( - model=model, link_indices=model.kin_dyn_parameters.contact_parameters.body + model=model, + link_indices=jnp.array( + model.kin_dyn_parameters.contact_parameters.body, dtype=int + )[indices_of_enabled_collidable_points], ) - W_p_Ci = model.kin_dyn_parameters.contact_parameters.point + + L_p_Ci = model.kin_dyn_parameters.contact_parameters.point[ + indices_of_enabled_collidable_points + ] + + # ===== + # Tests + # ===== # Load the model in ROD. rod_model = rod.Sdf.load(sdf=model.built_from).model # Add dummy frames on the contact points. - for idx, (link_name, W_p_C) in enumerate( - zip(parent_link_names, W_p_Ci, strict=True) + for idx, link_name, L_p_C in zip( + indices_of_enabled_collidable_points, parent_link_names, L_p_Ci, strict=True ): rod_model.add_frame( frame=rod.Frame( name=f"contact_point_{idx}", attached_to=link_name, pose=rod.Pose( - relative_to=link_name, pose=jnp.zeros(shape=(6,)).at[0:3].set(W_p_C) + relative_to=link_name, pose=jnp.zeros(shape=(6,)).at[0:3].set(L_p_C) ), ), ) @@ -125,7 +145,7 @@ def test_contact_jacobian_derivative( frame_idxs = js.frame.names_to_idxs( model=model_with_frames, frame_names=( - f"contact_point_{idx}" for idx in list(range(len(parent_link_names))) + f"contact_point_{idx}" for idx in indices_of_enabled_collidable_points ), )