jaxsim 0.3.1.dev54__tar.gz → 0.3.1.dev59__tar.gz

{jaxsim-0.3.1.dev54 → jaxsim-0.3.1.dev59}/PKG-INFO

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

{jaxsim-0.3.1.dev54 → jaxsim-0.3.1.dev59}/src/jaxsim/_version.py

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

{jaxsim-0.3.1.dev54 → jaxsim-0.3.1.dev59}/src/jaxsim/api/contact.py

@@ -351,17 +351,17 @@ def jacobian(
         output_vel_repr if output_vel_repr is not None else data.velocity_representation
     )
 
-    # For each collidable point, get the Jacobians of their parent link.
+    # Compute the Jacobians of all links.
+    W_J_WL = js.model.generalized_free_floating_jacobian(
+        model=model, data=data, output_vel_repr=VelRepr.Inertial
+    )
+
+    # Compute the contact Jacobian.
     # In inertial-fixed output representation, the Jacobian of the parent link is also
     # the Jacobian of the frame C implicitly associated with the collidable point.
-    W_J_WC = W_J_WL = jax.vmap(
-        lambda parent_link_idx: js.link.jacobian(
-            model=model,
-            data=data,
-            link_index=parent_link_idx,
-            output_vel_repr=VelRepr.Inertial,
-        )
-    )(jnp.array(model.kin_dyn_parameters.contact_parameters.body, dtype=int))
+    W_J_WC = jax.vmap(lambda parent_link_idx: W_J_WL[parent_link_idx])(
+        jnp.array(model.kin_dyn_parameters.contact_parameters.body, dtype=int)
+    )
 
     # Adjust the output representation.
     match output_vel_repr:

{jaxsim-0.3.1.dev54 → jaxsim-0.3.1.dev59}/src/jaxsim.egg-info/PKG-INFO

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

{jaxsim-0.3.1.dev54 → jaxsim-0.3.1.dev59}/src/jaxsim.egg-info/SOURCES.txt

@@ -108,6 +108,7 @@ src/jaxsim/utils/wrappers.py
 tests/__init__.py
 tests/conftest.py
 tests/test_api_com.py
+tests/test_api_contact.py
 tests/test_api_data.py
 tests/test_api_frame.py
 tests/test_api_joint.py

jaxsim-0.3.1.dev59/tests/test_api_contact.py

@@ -0,0 +1,58 @@
+import jax
+import jax.numpy as jnp
+import pytest
+
+import jaxsim.api as js
+from jaxsim import VelRepr
+
+
+def test_contact_kinematics(
+    jaxsim_models_types: js.model.JaxSimModel,
+    velocity_representation: VelRepr,
+    prng_key: jax.Array,
+):
+
+    model = jaxsim_models_types
+
+    _, subkey = jax.random.split(prng_key, num=2)
+    data = js.data.random_model_data(
+        model=model,
+        key=subkey,
+        velocity_representation=velocity_representation,
+    )
+
+    # =====
+    # Tests
+    # =====
+
+    # Compute the pose of the implicit contact frame associated to the collidable points
+    # and the transforms of all links.
+    W_H_C = js.contact.transforms(model=model, data=data)
+    W_H_L = js.model.forward_kinematics(model=model, data=data)
+
+    # Check that the orientation of the implicit contact frame matches with the
+    # orientation of the link to which the contact point is attached.
+    for contact_idx, index_of_parent_link in enumerate(
+        model.kin_dyn_parameters.contact_parameters.body
+    ):
+        assert W_H_C[contact_idx, 0:3, 0:3] == pytest.approx(
+            W_H_L[index_of_parent_link][0:3, 0:3]
+        )
+
+    # Check that the origin of the implicit contact frame is located over the
+    # collidable point.
+    W_p_C = js.contact.collidable_point_positions(model=model, data=data)
+    assert W_p_C == pytest.approx(W_H_C[:, 0:3, 3])
+
+    # Compute the velocity of the collidable point.
+    # This quantity always matches with the linear component of the mixed 6D velocity
+    # of the implicit frame associated to the collidable point.
+    W_ṗ_C = js.contact.collidable_point_velocities(model=model, data=data)
+
+    # Compute the velocity of the collidable point using the contact Jacobian.
+    ν = data.generalized_velocity()
+    CW_J_WC = js.contact.jacobian(model=model, data=data, output_vel_repr=VelRepr.Mixed)
+    CW_vl_WC = jnp.einsum("c6g,g->c6", CW_J_WC, ν)[:, 0:3]
+
+    # Compare the two velocities.
+    assert W_ṗ_C == pytest.approx(CW_vl_WC)

{jaxsim-0.3.1.dev54 → jaxsim-0.3.1.dev59}/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,