From 854aa684e27a9036997d64fae51b8b922d4e9ece Mon Sep 17 00:00:00 2001
From: Filippo Luca Ferretti <filippoluca.ferretti@outlook.com>
Date: Thu, 30 Nov 2023 10:17:19 +0100
Subject: [PATCH] [wip]

---
 src/jaxsim/high_level/model.py       |  9 +++++++++
 src/jaxsim/physics/algos/coriolis.py | 16 ++++++++++++++++
 2 files changed, 25 insertions(+)

diff --git a/src/jaxsim/high_level/model.py b/src/jaxsim/high_level/model.py
index b7131da2d..2582d7df1 100644
--- a/src/jaxsim/high_level/model.py
+++ b/src/jaxsim/high_level/model.py
@@ -945,6 +945,15 @@ def com_position(self) -> jtp.Vector:
     # Algorithms
     # ==========
 
+    def coriolis_matrix(self) -> jtp.Matrix:
+        H, H_dot, C = jaxsim.physics.algos.coriolis.coriolis(
+            model=self.physics_model,
+            q=self.data.model_state.joint_positions,
+            qd=self.data.model_state.joint_velocities,
+        )
+
+        return H, H_dot, C
+
     @functools.partial(oop.jax_tf.method_ro)
     def forward_kinematics(self) -> jtp.Array:
         """"""
diff --git a/src/jaxsim/physics/algos/coriolis.py b/src/jaxsim/physics/algos/coriolis.py
index 2d853b77c..167d42ec0 100644
--- a/src/jaxsim/physics/algos/coriolis.py
+++ b/src/jaxsim/physics/algos/coriolis.py
@@ -118,3 +118,19 @@ def inner_loop_body(
     )
 
     return Ic, Bc
+
+
+# if __name__ == "__main__":
+#     import jax.numpy as jnp
+#     import jaxsim
+#     from jaxsim.high_level.model import Model
+#     from pathlib import Path
+
+#     urdf_path = Path(
+#         "/home/flferretti/git/element_rl-for-codesign/assets/model/Hopper.sdf"
+#     )
+
+#     model = Model.build_from_model_description(model_description=urdf_path)
+
+#     with jax.disable_jit():
+#         H, H_dot, C = model.coriolis_matrix()