Use JAX NumPy to build contact frame

Author: flferretti

src/jaxsim/rbda/contacts/detection.py

@@ -17,9 +17,13 @@ def _contact_frame(normal: jtp.Vector, position: jtp.Vector) -> jtp.Matrix:
 
     R = jnp.stack([t1, t2, n], axis=1)
 
-    return jaxsim.math.Transform.from_rotation_and_translation(
-        rotation=R,
-        translation=position,
+    return jnp.block(
+        [
+            [R[0, 0], R[0, 1], R[0, 2], position[0]],
+            [R[1, 0], R[1, 1], R[1, 2], position[1]],
+            [R[2, 0], R[2, 1], R[2, 2], position[2]],
+            [0.0, 0.0, 0.0, 1.0],
+        ]
     )
 
 

src/jaxsim/rbda/contacts/soft.py

@@ -5,6 +5,7 @@
 import jax
 import jax.numpy as jnp
 import jax_dataclasses
+import numpy as np
 
 import jaxsim.api as js
 import jaxsim.math
@@ -429,10 +430,10 @@ def compute_contact_forces(
             model.kin_dyn_parameters.contact_parameters.shape_type,
             model.kin_dyn_parameters.contact_parameters.shape_size,
             data._link_transforms[
-                jnp.array(model.kin_dyn_parameters.contact_parameters.body)
+                np.array(model.kin_dyn_parameters.contact_parameters.body)
             ],
             data._link_velocities[
-                jnp.array(model.kin_dyn_parameters.contact_parameters.body)
+                np.array(model.kin_dyn_parameters.contact_parameters.body)
             ],
         )