From 8cf5fb2f6019768a68dccf93db4a781fd5d4f2af Mon Sep 17 00:00:00 2001
From: Alessandro Croci <alessandro.croci@iit.it>
Date: Tue, 28 Jan 2025 14:59:40 +0100
Subject: [PATCH 1/2] Fix missing switch to mixed in RelaxedRigidContacts
 `compute_contact_forces`

---
 src/jaxsim/rbda/contacts/relaxed_rigid.py | 56 ++++++++++++-----------
 1 file changed, 30 insertions(+), 26 deletions(-)

diff --git a/src/jaxsim/rbda/contacts/relaxed_rigid.py b/src/jaxsim/rbda/contacts/relaxed_rigid.py
index 0f9d90730..ee09d43e7 100644
--- a/src/jaxsim/rbda/contacts/relaxed_rigid.py
+++ b/src/jaxsim/rbda/contacts/relaxed_rigid.py
@@ -307,38 +307,42 @@ def compute_contact_forces(
         # collidable points.
         W_H_C = js.contact.transforms(model=model, data=data)
 
-        BW_ν = data.generalized_velocity()
-
-        BW_ν̇_free = jnp.hstack(
-            js.ode.system_acceleration(
-                model=model,
-                data=data,
-                link_forces=references.link_forces(model=model, data=data),
-                joint_torques=references.joint_force_references(model=model),
+        with (
+            data.switch_velocity_representation(VelRepr.Mixed),
+            references.switch_velocity_representation(VelRepr.Mixed),
+        ):
+            BW_ν = data.generalized_velocity()
+
+            BW_ν̇_free = jnp.hstack(
+                js.ode.system_acceleration(
+                    model=model,
+                    data=data,
+                    link_forces=references.link_forces(model=model, data=data),
+                    joint_torques=references.joint_force_references(model=model),
+                )
             )
-        )
 
-        M = js.model.free_floating_mass_matrix(model=model, data=data)
+            M = js.model.free_floating_mass_matrix(model=model, data=data)
 
-        Jl_WC = jnp.vstack(
-            jax.vmap(lambda J, δ: J * (δ > 0))(
-                js.contact.jacobian(model=model, data=data)[:, :3, :], δ
+            Jl_WC = jnp.vstack(
+                jax.vmap(lambda J, δ: J * (δ > 0))(
+                    js.contact.jacobian(model=model, data=data)[:, :3, :], δ
+                )
             )
-        )
 
-        J̇_WC = jnp.vstack(
-            jax.vmap(lambda J̇, δ: J̇ * (δ > 0))(
-                js.contact.jacobian_derivative(model=model, data=data)[:, :3], δ
-            ),
-        )
+            J̇_WC = jnp.vstack(
+                jax.vmap(lambda J̇, δ: J̇ * (δ > 0))(
+                    js.contact.jacobian_derivative(model=model, data=data)[:, :3], δ
+                ),
+            )
 
-        # Compute the regularization terms.
-        a_ref, R, *_ = self._regularizers(
-            model=model,
-            position_constraint=position_constraint,
-            velocity_constraint=velocity,
-            parameters=model.contacts_params,
-        )
+            # Compute the regularization terms.
+            a_ref, R, *_ = self._regularizers(
+                model=model,
+                position_constraint=position_constraint,
+                velocity_constraint=velocity,
+                parameters=model.contacts_params,
+            )
 
         # Compute the Delassus matrix and the free mixed linear acceleration of
         # the collidable points.

From 17264a88129b5890bea474537113f3bff7e8d401 Mon Sep 17 00:00:00 2001
From: Alessandro Croci <57228872+xela-95@users.noreply.github.com>
Date: Tue, 28 Jan 2025 17:24:44 +0100
Subject: [PATCH 2/2] Update src/jaxsim/rbda/contacts/relaxed_rigid.py

Co-authored-by: Filippo Luca Ferretti <102977828+flferretti@users.noreply.github.com>
---
 src/jaxsim/rbda/contacts/relaxed_rigid.py | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/jaxsim/rbda/contacts/relaxed_rigid.py b/src/jaxsim/rbda/contacts/relaxed_rigid.py
index ee09d43e7..7388a232d 100644
--- a/src/jaxsim/rbda/contacts/relaxed_rigid.py
+++ b/src/jaxsim/rbda/contacts/relaxed_rigid.py
@@ -336,13 +336,13 @@ def compute_contact_forces(
                 ),
             )
 
-            # Compute the regularization terms.
-            a_ref, R, *_ = self._regularizers(
-                model=model,
-                position_constraint=position_constraint,
-                velocity_constraint=velocity,
-                parameters=model.contacts_params,
-            )
+        # Compute the regularization terms.
+        a_ref, R, *_ = self._regularizers(
+            model=model,
+            position_constraint=position_constraint,
+            velocity_constraint=velocity,
+            parameters=model.contacts_params,
+        )
 
         # Compute the Delassus matrix and the free mixed linear acceleration of
         # the collidable points.