jaxsim 0.3.1.dev51__tar.gz → 0.3.1.dev56__tar.gz

{jaxsim-0.3.1.dev51 → jaxsim-0.3.1.dev56}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: jaxsim
-Version: 0.3.1.dev51
+Version: 0.3.1.dev56
 Home-page: https://github.com/ami-iit/jaxsim
 Author: Diego Ferigo
 Author-email: diego.ferigo@iit.it

{jaxsim-0.3.1.dev51 → jaxsim-0.3.1.dev56}/src/jaxsim/_version.py

@@ -12,5 +12,5 @@ __version__: str
 __version_tuple__: VERSION_TUPLE
 version_tuple: VERSION_TUPLE
 
-__version__ = version = '0.3.1.dev51'
-__version_tuple__ = version_tuple = (0, 3, 1, 'dev51')
+__version__ = version = '0.3.1.dev56'
+__version_tuple__ = version_tuple = (0, 3, 1, 'dev56')

{jaxsim-0.3.1.dev51 → jaxsim-0.3.1.dev56}/src/jaxsim/api/ode.py

@@ -150,14 +150,11 @@ def system_velocity_dynamics(
         # Sum the forces of all collidable points rigidly attached to a body.
         # Since the contact forces W_f_Ci are expressed in the world frame,
         # we don't need any coordinate transformation.
-        W_f_Li_terrain = jax.vmap(
-            lambda nc: (
-                jnp.vstack(
-                    jnp.equal(parent_link_index_of_collidable_points, nc).astype(int)
-                )
-                * W_f_Ci
-            ).sum(axis=0)
-        )(jnp.arange(model.number_of_links()))
+        mask = parent_link_index_of_collidable_points[:, jnp.newaxis] == jnp.arange(
+            model.number_of_links()
+        )
+
+        W_f_Li_terrain = mask.T @ W_f_Ci
 
     # ====================
     # Enforce joint limits

{jaxsim-0.3.1.dev51 → jaxsim-0.3.1.dev56}/src/jaxsim.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: jaxsim
-Version: 0.3.1.dev51
+Version: 0.3.1.dev56
 Home-page: https://github.com/ami-iit/jaxsim
 Author: Diego Ferigo
 Author-email: diego.ferigo@iit.it

{jaxsim-0.3.1.dev51 → jaxsim-0.3.1.dev56}/tests/test_api_link.py

@@ -223,6 +223,74 @@ def test_link_bias_acceleration(
         Jν_js = js.link.bias_acceleration(model=model, data=data, link_index=index)
         assert pytest.approx(Jν_idt) == Jν_js
 
+    # Test that the conversion of the link bias acceleration works as expected.
+    match data.velocity_representation:
+
+        # We exclude the mixed representation because converting the acceleration is
+        # more complex than using the plain 6D transform matrix.
+        case VelRepr.Mixed:
+            pass
+
+        # Inertial-fixed to body-fixed conversion.
+        case VelRepr.Inertial:
+
+            W_H_L = js.model.forward_kinematics(model=model, data=data)
+
+            W_a_bias_WL = jax.vmap(
+                lambda index: js.link.bias_acceleration(
+                    model=model, data=data, link_index=index
+                )
+            )(jnp.arange(model.number_of_links()))
+
+            with data.switch_velocity_representation(VelRepr.Body):
+
+                W_X_L = jax.vmap(
+                    lambda W_H_L: jaxsim.math.Adjoint.from_transform(transform=W_H_L)
+                )(W_H_L)
+
+                L_a_bias_WL = jax.vmap(
+                    lambda index: js.link.bias_acceleration(
+                        model=model, data=data, link_index=index
+                    )
+                )(jnp.arange(model.number_of_links()))
+
+                W_a_bias_WL_converted = jax.vmap(
+                    lambda W_X_L, L_a_bias_WL: W_X_L @ L_a_bias_WL
+                )(W_X_L, L_a_bias_WL)
+
+            assert W_a_bias_WL == pytest.approx(W_a_bias_WL_converted)
+
+        # Body-fixed to inertial-fixed conversion.
+        case VelRepr.Body:
+
+            W_H_L = js.model.forward_kinematics(model=model, data=data)
+
+            L_a_bias_WL = jax.vmap(
+                lambda index: js.link.bias_acceleration(
+                    model=model, data=data, link_index=index
+                )
+            )(jnp.arange(model.number_of_links()))
+
+            with data.switch_velocity_representation(VelRepr.Inertial):
+
+                L_X_W = jax.vmap(
+                    lambda W_H_L: jaxsim.math.Adjoint.from_transform(
+                        transform=W_H_L, inverse=True
+                    )
+                )(W_H_L)
+
+                W_a_bias_WL = jax.vmap(
+                    lambda index: js.link.bias_acceleration(
+                        model=model, data=data, link_index=index
+                    )
+                )(jnp.arange(model.number_of_links()))
+
+                L_a_bias_WL_converted = jax.vmap(
+                    lambda L_X_W, W_a_bias_WL: L_X_W @ W_a_bias_WL
+                )(L_X_W, W_a_bias_WL)
+
+            assert L_a_bias_WL == pytest.approx(L_a_bias_WL_converted)
+
 
 def test_link_jacobian_derivative(
     jaxsim_models_types: js.model.JaxSimModel,