jaxsim 0.2.1.dev74__py3-none-any.whl → 0.2.1.dev80__py3-none-any.whl

jaxsim/_version.py

@@ -12,5 +12,5 @@ __version__: str
 __version_tuple__: VERSION_TUPLE
 version_tuple: VERSION_TUPLE
 
-__version__ = version = '0.2.1.dev74'
-__version_tuple__ = version_tuple = (0, 2, 1, 'dev74')
+__version__ = version = '0.2.1.dev80'
+__version_tuple__ = version_tuple = (0, 2, 1, 'dev80')

jaxsim/api/contact.py

@@ -32,6 +32,7 @@ def collidable_point_kinematics(
 
     from jaxsim.rbda import collidable_points
 
+    # Switch to inertial-fixed since the RBDAs expect velocities in this representation.
     with data.switch_velocity_representation(VelRepr.Inertial):
         W_p_Ci, W_ṗ_Ci = collidable_points.collidable_points_pos_vel(
             model=model,
@@ -61,7 +62,9 @@ def collidable_point_positions(
         The position of the collidable points in the world frame.
     """
 
-    return collidable_point_kinematics(model=model, data=data)[0]
+    W_p_Ci, _ = collidable_point_kinematics(model=model, data=data)
+
+    return W_p_Ci
 
 
 @jax.jit
@@ -79,7 +82,9 @@ def collidable_point_velocities(
         The 3D velocity of the collidable points.
     """
 
-    return collidable_point_kinematics(model=model, data=data)[1]
+    _, W_ṗ_Ci = collidable_point_kinematics(model=model, data=data)
+
+    return W_ṗ_Ci
 
 
 @jax.jit
@@ -269,3 +274,124 @@ def estimate_good_soft_contacts_parameters(
     )
 
     return sc_parameters
+
+
+@jax.jit
+def transforms(model: js.model.JaxSimModel, data: js.data.JaxSimModelData) -> jtp.Array:
+    r"""
+    Return the pose of the collidable points.
+
+    Args:
+        model: The model to consider.
+        data: The data of the considered model.
+
+    Returns:
+        The stacked SE(3) matrices of all collidable points.
+
+    Note:
+        Each collidable point is implicitly associated with a frame
+        :math:`C = ({}^W p_C, [L])`, where :math:`{}^W p_C` is the position of the
+        collidable point and :math:`[L]` is the orientation frame of the link it is
+        rigidly attached to.
+    """
+
+    # Get the transforms of the parent link of all collidable points.
+    W_H_L = jax.vmap(
+        lambda parent_link_idx: js.link.transform(
+            model=model, data=data, link_index=parent_link_idx
+        )
+    )(jnp.array(model.kin_dyn_parameters.contact_parameters.body, dtype=int))
+
+    # Build the link-to-point transform from the displacement between the link frame L
+    # and the implicit contact frame C.
+    L_H_C = jax.vmap(lambda L_p_C: jnp.eye(4).at[0:3, 3].set(L_p_C))(
+        model.kin_dyn_parameters.contact_parameters.point
+    )
+
+    # Compose the work-to-link and link-to-point transforms.
+    return jax.vmap(lambda W_H_Li, L_H_Ci: W_H_Li @ L_H_Ci)(W_H_L, L_H_C)
+
+
+@functools.partial(jax.jit, static_argnames=["output_vel_repr"])
+def jacobian(
+    model: js.model.JaxSimModel,
+    data: js.data.JaxSimModelData,
+    *,
+    output_vel_repr: VelRepr | None = None,
+) -> jtp.Array:
+    r"""
+    Return the free-floating Jacobian of the collidable points.
+
+    Args:
+        model: The model to consider.
+        data: The data of the considered model.
+        output_vel_repr:
+            The output velocity representation of the free-floating jacobian.
+
+    Returns:
+        The stacked 6×(6+n) free-floating jacobians of the frames associated to the
+        collidable points.
+
+    Note:
+        Each collidable point is implicitly associated with a frame
+        :math:`C = ({}^W p_C, [L])`, where :math:`{}^W p_C` is the position of the
+        collidable point and :math:`[L]` is the orientation frame of the link it is
+        rigidly attached to.
+    """
+
+    output_vel_repr = (
+        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.
+    # 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))
+
+    # Adjust the output representation.
+    match output_vel_repr:
+
+        case VelRepr.Inertial:
+            O_J_WC = W_J_WC
+
+        case VelRepr.Body:
+
+            W_H_C = transforms(model=model, data=data)
+
+            def jacobian(W_H_C: jtp.Matrix, W_J_WC: jtp.Matrix) -> jtp.Matrix:
+                C_X_W = jaxsim.math.Adjoint.from_transform(
+                    transform=W_H_C, inverse=True
+                )
+                C_J_WC = C_X_W @ W_J_WC
+                return C_J_WC
+
+            O_J_WC = jax.vmap(jacobian)(W_H_C, W_J_WC)
+
+        case VelRepr.Mixed:
+
+            W_H_C = transforms(model=model, data=data)
+
+            def jacobian(W_H_C: jtp.Matrix, W_J_WC: jtp.Matrix) -> jtp.Matrix:
+
+                W_H_CW = W_H_C.at[0:3, 0:3].set(jnp.eye(3))
+
+                CW_X_W = jaxsim.math.Adjoint.from_transform(
+                    transform=W_H_CW, inverse=True
+                )
+
+                CW_J_WC = CW_X_W @ W_J_WC
+                return CW_J_WC
+
+            O_J_WC = jax.vmap(jacobian)(W_H_C, W_J_WC)
+
+        case _:
+            raise ValueError(output_vel_repr)
+
+    return O_J_WC

{jaxsim-0.2.1.dev74.dist-info → jaxsim-0.2.1.dev80.dist-info}/METADATA

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

{jaxsim-0.2.1.dev74.dist-info → jaxsim-0.2.1.dev80.dist-info}/RECORD

@@ -1,11 +1,11 @@
 jaxsim/__init__.py,sha256=OcrfoYS1DGcmAGqu2AqlCTiUVxcpi-IsVwcr_16x74Q,1789
-jaxsim/_version.py,sha256=4Oh93XR_460KA2Rq1fEo8S7jdrJW940Uh6izGU9tdL0,426
+jaxsim/_version.py,sha256=6bZIeO5gVEXbcUw14r0OEvahs0AZBEq5fAmJ16fr2fU,426
 jaxsim/logging.py,sha256=c4zhwBKf9eAYAHVp62kTEllqdsZgh0K-kPKVy8L3elU,1584
 jaxsim/typing.py,sha256=MeuOCQtLAr-sPkvB_sU8FtwGNRirz1auCwIgRC-QZl8,646
 jaxsim/api/__init__.py,sha256=8eV22t2S3UwNyCg8karPetG1dmX1VDBXkyv28_FwNQA,210
 jaxsim/api/com.py,sha256=Yof6otFi-mLWAs1rqjmeNJTOWIH9gn7BdU5EIjiL6Ts,13481
 jaxsim/api/common.py,sha256=DV-WZG28sikXopNv458aYvpLjmiAtFr5LRscOwXusuk,6640
-jaxsim/api/contact.py,sha256=Ve4ZOWkLEBRgK3KhtICxKY7YzsxYvc3lO-pPRBjqSnY,8659
+jaxsim/api/contact.py,sha256=Cvr-EfQtHP3nymtWdo-9WWU24Bkta-2Pp3nKsdjo6uc,12778
 jaxsim/api/data.py,sha256=GXSCfq4_PsRZuUwsmcl18e2Ppdtu4ykYs0oWih8J2ZI,26775
 jaxsim/api/frame.py,sha256=68liCmwFDikuP0GQOpo-nF7aVeHJ173e1USSOwskTP0,6246
 jaxsim/api/joint.py,sha256=-5DogPg4g4mmLckyVIVNjwv-Rxz0IWS7_md9nDlhPWA,4581
@@ -58,8 +58,8 @@ jaxsim/utils/__init__.py,sha256=tnQq1_CavdfeKaLYt3pmO7Jk4MU2RwwQU_qICkjyoTY,197
 jaxsim/utils/hashless.py,sha256=bFIwKeo9KiWwsY8QM55duEGGQOyyJ4jQyPcuqTLEp5k,297
 jaxsim/utils/jaxsim_dataclass.py,sha256=h26timZ_XrBL_Q_oymv-DkQd-EcUiHn8QexAaZXBY9c,11396
 jaxsim/utils/tracing.py,sha256=KDMoyVPlu2NJvFkhtZwq5AkqMMgajt3munvJom-vEjQ,650
-jaxsim-0.2.1.dev74.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
-jaxsim-0.2.1.dev74.dist-info/METADATA,sha256=rj10ROK1KcakStKl-rEOQcaxVHOJfga3OZPiSdOhiNo,9744
-jaxsim-0.2.1.dev74.dist-info/WHEEL,sha256=GJ7t_kWBFywbagK5eo9IoUwLW6oyOeTKmQ-9iHFVNxQ,92
-jaxsim-0.2.1.dev74.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
-jaxsim-0.2.1.dev74.dist-info/RECORD,,
+jaxsim-0.2.1.dev80.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
+jaxsim-0.2.1.dev80.dist-info/METADATA,sha256=yFYgYOpZwSa-j1g4zj-4mp2B3z4TkL_9kbF-Rd5Q3hI,9744
+jaxsim-0.2.1.dev80.dist-info/WHEEL,sha256=GJ7t_kWBFywbagK5eo9IoUwLW6oyOeTKmQ-9iHFVNxQ,92
+jaxsim-0.2.1.dev80.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
+jaxsim-0.2.1.dev80.dist-info/RECORD,,