Merge pull request #356 from ami-iit/sprint/fix-relaxed-contacts

[Sprint] Fix missing switch in RelaxedRigidContacts `compute_contact_forces`
ami-iit · Jan 30, 2025 · 060f14b · 060f14b
2 parents ccb7f27 + 17264a8
commit 060f14b
Showing 1 changed file with 23 additions and 19 deletions.
diff --git a/src/jaxsim/rbda/contacts/relaxed_rigid.py b/src/jaxsim/rbda/contacts/relaxed_rigid.py
@@ -307,30 +307,34 @@ 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(