From a48f7e106b46332fd226de8ac8d44bc70f522e1f Mon Sep 17 00:00:00 2001
From: Filippo Luca Ferretti <filippoluca.ferretti@outlook.com>
Date: Tue, 17 Dec 2024 17:18:21 +0100
Subject: [PATCH] Add converters for mass matrix, jacobian and jacobian
 derivative

---
 src/jaxsim/api/common.py | 66 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 66 insertions(+)

diff --git a/src/jaxsim/api/common.py b/src/jaxsim/api/common.py
index 7d723120e..4965be7cf 100644
--- a/src/jaxsim/api/common.py
+++ b/src/jaxsim/api/common.py
@@ -232,3 +232,69 @@ def other_representation_to_inertial(
 
             case _:
                 raise ValueError(other_representation)
+
+
+def convert_mass_matrix(
+    M: jtp.Matrix,
+    base_transform: jtp.Matrix,
+    dofs: jtp.Int,
+    velocity_representation: VelRepr,
+):
+
+    return M
+
+    match velocity_representation:
+        case VelRepr.Body:
+            return M
+
+        case VelRepr.Inertial:
+
+            B_X_W = Adjoint.from_transform(transform=base_transform, inverse=True)
+            invT = jax.scipy.linalg.block_diag(B_X_W, jnp.eye(dofs))
+
+            return invT.T @ M @ invT
+
+        case VelRepr.Mixed:
+
+            BW_H_B = base_transform.at[0:3, 3].set(jnp.zeros(3))
+            B_X_BW = Adjoint.from_transform(transform=BW_H_B, inverse=True)
+            invT = jax.scipy.linalg.block_diag(B_X_BW, jnp.eye(dofs))
+
+            return invT.T @ M @ invT
+
+
+def convert_jacobian(
+    J: jtp.Matrix,
+    base_transform: jtp.Matrix,
+    dofs: jtp.Int,
+    velocity_representation: VelRepr,
+):
+    return J
+
+    match velocity_representation:
+
+        case VelRepr.Inertial:
+
+            W_H_B = base_transform
+            B_X_W = Adjoint.from_transform(transform=W_H_B, inverse=True)
+
+            return J @ jax.scipy.linalg.block_diag(B_X_W, jnp.eye(dofs))
+
+        case VelRepr.Body:
+
+            return J
+
+        case VelRepr.Mixed:
+
+            W_R_B = base_transform[:3, :3]
+            BW_H_B = jnp.eye(4).at[0:3, 0:3].set(W_R_B)
+            B_X_BW = Adjoint.from_transform(transform=BW_H_B, inverse=True)
+
+            return J @ jax.scipy.linalg.block_diag(B_X_BW, jnp.eye(dofs))
+
+        case _:
+            raise ValueError(velocity_representation)
+
+
+def convert_jacobian_derivative(J: jtp.Matrix, velocity_representation: VelRepr):
+    return J