Add vx star bar in math

flferretti · Nov 30, 2023 · ab1b3b8 · ab1b3b8
1 parent e391475
commit ab1b3b8
Showing 1 changed file with 10 additions and 0 deletions.
diff --git a/src/jaxsim/math/cross.py b/src/jaxsim/math/cross.py
@@ -23,3 +23,13 @@ def vx(velocity_sixd: jtp.Vector) -> jtp.Matrix:
     def vx_star(velocity_sixd: jtp.Vector) -> jtp.Matrix:
         v_cross_star = -Cross.vx(velocity_sixd).T
         return v_cross_star
+
+    @staticmethod
+    def vx_star_bar(velocity_sixd: jtp.Vector) -> jtp.Matrix:
+        v_cross_star_bar = jnp.block(
+            [
+                [Skew.wedge(vector=velocity_sixd.squeeze()), jnp.zeros(shape=(3, 3))],
+                [jnp.zeros(shape=(3, 3)), Skew.wedge(vector=velocity_sixd.squeeze())],
+            ]
+        )
+        return v_cross_star_bar