From f2016a8c69c6f382cc12b034f0d01cfc9ca6f0fe Mon Sep 17 00:00:00 2001
From: Filippo Luca Ferretti <102977828+flferretti@users.noreply.github.com>
Date: Fri, 22 Nov 2024 13:08:30 +0100
Subject: [PATCH] Use a `linalg.pinv` to invert the mass matrix in
 RelaxedRigidContacts

---
 src/jaxsim/rbda/contacts/relaxed_rigid.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/jaxsim/rbda/contacts/relaxed_rigid.py b/src/jaxsim/rbda/contacts/relaxed_rigid.py
index 0922f4f45..6941adfa9 100644
--- a/src/jaxsim/rbda/contacts/relaxed_rigid.py
+++ b/src/jaxsim/rbda/contacts/relaxed_rigid.py
@@ -376,7 +376,7 @@ def compute_contact_forces(
 
         # Compute the Delassus matrix and the free mixed linear acceleration of
         # the collidable points.
-        G = Jl_WC @ jnp.linalg.lstsq(M, Jl_WC.T)[0]
+        G = Jl_WC @ jnp.linalg.pinv(M) @ Jl_WC.T
         CW_al_free_WC = Jl_WC @ BW_ν̇_free + J̇_WC @ BW_ν
 
         # Calculate quantities for the linear optimization problem.