From 5342ff23bcafee0b34be237090469dc783a1c31b Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 21 Feb 2025 17:32:50 +0100 Subject: [PATCH] Implement Baumgarte stabilization for kinematic constraints in RelaxedRigidContacts --- src/jaxsim/rbda/contacts/relaxed_rigid.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/jaxsim/rbda/contacts/relaxed_rigid.py b/src/jaxsim/rbda/contacts/relaxed_rigid.py index 4cd2d3a8b..91556d0f9 100644 --- a/src/jaxsim/rbda/contacts/relaxed_rigid.py +++ b/src/jaxsim/rbda/contacts/relaxed_rigid.py @@ -344,6 +344,10 @@ def compute_contact_forces( W_H_F1 = js.frame.transform(model=model, data=data, frame_index=F1_idx) W_H_F2 = js.frame.transform(model=model, data=data, frame_index=F2_idx) + W_p_F1 = W_H_F1[:3, 3] + W_p_F2 = W_H_F2[:3, 3] + W_R_F1 = W_H_F1[:3, :3] + W_R_F2 = W_H_F2[:3, :3] J_WF1 = js.frame.jacobian(model=model, data=data, frame_index=F1_idx) J_WF2 = js.frame.jacobian(model=model, data=data, frame_index=F2_idx) @@ -370,12 +374,21 @@ def compute_contact_forces( # jax.debug.print("R.shape: \n{}", R.shape) num_zeros_kin_constr = J_WF1.shape[0] - zeros_array_a_ref = jnp.zeros((num_zeros_kin_constr,)) + + # Compute Baumgarte stabilization terms for the kinematic constraints + + K_P = 1.0e2 + K_D = 2 * jnp.sqrt(K_P) + vel_error = (J_WF1 - J_WF2) @ BW_ν + pos_error = W_p_F1 - W_p_F2 + R_error = W_R_F1.T @ W_R_F2 + omega_error = jaxsim.math.rotation.Rotation.log_SO3(R_error) + baumgarte_term = K_P * jnp.hstack([pos_error, omega_error]) + K_D * vel_error R_ext = jnp.pad( R, ((0, num_zeros_kin_constr), (0, num_zeros_kin_constr)), mode="constant" ) - a_ref_ext = jnp.hstack([a_ref, zeros_array_a_ref]) + a_ref_ext = jnp.hstack([a_ref, -baumgarte_term]) # Compute the Delassus matrix and the free mixed linear acceleration of # the collidable points.