From dcc52fd11b4b5d696cb29c9e6ab198c0bb1ff3e7 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Mon, 24 Feb 2025 17:42:08 +0100 Subject: [PATCH] Refactor link_contact_forces to separate kinematic constraint forces for F1 and F2 --- src/jaxsim/api/contact_model.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/jaxsim/api/contact_model.py b/src/jaxsim/api/contact_model.py index a30ae3f58..98ff34187 100644 --- a/src/jaxsim/api/contact_model.py +++ b/src/jaxsim/api/contact_model.py @@ -46,16 +46,16 @@ def link_contact_forces( # Add the forces coming from the kinematic constraints to the link on which the constraint is applied. - W_f_loop = aux_data["kin_constr_force"] + # W_f_loop = aux_data["kin_constr_force"] + W_f_loop_F1 = aux_data["kin_constr_force_F1"] + W_f_loop_f2 = aux_data["kin_constr_force_F2"] F1_idx = aux_data["F1_idx"] F2_idx = aux_data["F2_idx"] F1_parent_idx = js.frame.idx_of_parent_link(model, frame_index=F1_idx) F2_parent_idx = js.frame.idx_of_parent_link(model, frame_index=F2_idx) - jax.debug.print("F1_parent: {}", F1_parent_idx) - jax.debug.print("F2_parent: {}", F2_parent_idx) - W_f_L = W_f_L.at[F1_parent_idx].add(W_f_loop) - W_f_L = W_f_L.at[F2_parent_idx].add(-W_f_loop) + W_f_L = W_f_L.at[F1_parent_idx].add(W_f_loop_F1) + W_f_L = W_f_L.at[F2_parent_idx].add(W_f_loop_f2) jax.debug.print("W_f_L: \n{}", W_f_L) return W_f_L