jaxsim 0.5.1.dev91__py3-none-any.whl → 0.5.1.dev95__py3-none-any.whl

jaxsim/_version.py

@@ -12,5 +12,5 @@ __version__: str
 __version_tuple__: VERSION_TUPLE
 version_tuple: VERSION_TUPLE
 
-__version__ = version = '0.5.1.dev91'
-__version_tuple__ = version_tuple = (0, 5, 1, 'dev91')
+__version__ = version = '0.5.1.dev95'
+__version_tuple__ = version_tuple = (0, 5, 1, 'dev95')

jaxsim/api/data.py

@@ -382,9 +382,8 @@ class JaxSimModelData(common.ModelDataWithVelocityRepresentation):
         # we introduce a Baumgarte stabilization to let the quaternion converge to
         # a unit quaternion. In this case, it is not guaranteed that the quaternion
         # stored in the state is a unit quaternion.
-        W_Q_B = jnp.where(
-            jnp.allclose(W_Q_B.dot(W_Q_B), 1.0), W_Q_B, W_Q_B / jnp.linalg.norm(W_Q_B)
-        )
+        norm = jaxsim.math.safe_norm(W_Q_B)
+        W_Q_B = W_Q_B / (norm + jnp.finfo(float).eps * (norm == 0))
 
         return (W_Q_B if not dcm else jaxsim.math.Quaternion.to_dcm(W_Q_B)).astype(
             float
@@ -611,11 +610,8 @@ class JaxSimModelData(common.ModelDataWithVelocityRepresentation):
 
         W_Q_B = jnp.array(base_quaternion, dtype=float)
 
-        W_Q_B = jax.lax.select(
-            pred=jnp.allclose(jnp.linalg.norm(W_Q_B), 1.0, atol=1e-6, rtol=0.0),
-            on_true=W_Q_B,
-            on_false=W_Q_B / jnp.linalg.norm(W_Q_B),
-        )
+        norm = jaxsim.math.safe_norm(W_Q_B)
+        W_Q_B = W_Q_B / (norm + jnp.finfo(float).eps * (norm == 0))
 
         return self.replace(
             validate=True,

jaxsim/math/__init__.py

@@ -8,5 +8,6 @@ from .quaternion import Quaternion
 from .rotation import Rotation
 from .skew import Skew
 from .transform import Transform
+from .utils import safe_norm
 
 from .joint_model import JointModel, supported_joint_motion  # isort:skip

jaxsim/math/quaternion.py

@@ -4,6 +4,8 @@ import jaxlie
 
 import jaxsim.typing as jtp
 
+from .utils import safe_norm
+
 
 class Quaternion:
     @staticmethod
@@ -111,18 +113,13 @@ class Quaternion:
             operand=quaternion,
         )
 
-        norm_ω = jax.lax.cond(
-            pred=ω.dot(ω) < (1e-6) ** 2,
-            true_fun=lambda _: 1e-6,
-            false_fun=lambda _: jnp.linalg.norm(ω),
-            operand=None,
-        )
+        norm_ω = safe_norm(ω)
 
         qd = 0.5 * (
             Q
             @ jnp.hstack(
                 [
-                    K * norm_ω * (1 - jnp.linalg.norm(quaternion)),
+                    K * norm_ω * (1 - safe_norm(quaternion)),
                     ω,
                 ]
             )

jaxsim/math/rotation.py

@@ -4,6 +4,7 @@ import jaxlie
 import jaxsim.typing as jtp
 
 from .skew import Skew
+from .utils import safe_norm
 
 
 class Rotation:
@@ -67,7 +68,7 @@ class Rotation:
         def theta_is_not_zero(axis: jtp.Vector) -> jtp.Matrix:
 
             v = axis
-            theta = jnp.linalg.norm(v)
+            theta = safe_norm(v)
 
             s = jnp.sin(theta)
             c = jnp.cos(theta)
@@ -81,19 +82,9 @@ class Rotation:
 
             return R.transpose()
 
-        # Use the double-where trick to prevent JAX problems when the
-        # jax.jit and jax.grad transforms are applied.
         return jnp.where(
-            jnp.linalg.norm(vector) > 0,
-            theta_is_not_zero(
-                axis=jnp.where(
-                    jnp.linalg.norm(vector) > 0,
-                    vector,
-                    # The following line is a workaround to prevent division by 0.
-                    # Considering the outer where, this branch is never executed.
-                    jnp.ones(3),
-                )
-            ),
+            jnp.allclose(vector, 0.0),
             # Return an identity rotation matrix when the input vector is zero.
             jnp.eye(3),
+            theta_is_not_zero(axis=vector),
         )

jaxsim/math/utils.py

@@ -0,0 +1,31 @@
+import jax.numpy as jnp
+
+import jaxsim.typing as jtp
+
+
+def safe_norm(array: jtp.ArrayLike, axis=None) -> jtp.Array:
+    """
+    Provides a calculation for an array norm so that it is safe
+    to compute the gradient and handle NaNs.
+
+    Args:
+        array: The array for which to compute the norm.
+        axis: The axis for which to compute the norm.
+
+    Returns:
+        The norm of the array with handling for zero arrays to avoid NaNs.
+    """
+
+    # Check if the entire array is composed of zeros.
+    is_zero = jnp.allclose(array, 0.0)
+
+    # Replace zeros with an array of ones temporarily to avoid division by zero.
+    # This ensures the computation of norm does not produce NaNs or Infs.
+    array = jnp.where(is_zero, jnp.ones_like(array), array)
+
+    # Compute the norm of the array along the specified axis.
+    norm = jnp.linalg.norm(array, axis=axis)
+
+    # Use `jnp.where` to set the norm to 0.0 where the input array was all zeros.
+    # This usage supports potential batch processing for future scalability.
+    return jnp.where(is_zero, 0.0, norm)

jaxsim/rbda/contacts/soft.py

@@ -309,19 +309,16 @@ class SoftContacts(common.ContactModel):
 
         # Compute the direction of the tangential force.
         # To prevent dividing by zero, we use a switch statement.
-        # The ε, instead, is needed to make AD happy.
-        f_tangential_direction = jnp.where(
-            f_tangential.dot(f_tangential) != 0,
-            f_tangential / jnp.linalg.norm(f_tangential + ε),
-            jnp.zeros(3),
+        norm = jaxsim.math.safe_norm(f_tangential)
+        f_tangential_direction = f_tangential / (
+            norm + jnp.finfo(float).eps * (norm == 0)
         )
 
         # Project the tangential force to the friction cone if slipping.
         f_tangential = jnp.where(
             sticking,
             f_tangential,
-            jnp.minimum(μ * force_normal_mag, jnp.linalg.norm(f_tangential + ε))
-            * f_tangential_direction,
+            jnp.minimum(μ * force_normal_mag, norm) * f_tangential_direction,
         )
 
         # Set the tangential force to zero if there is no contact.

jaxsim/terrain/terrain.py

@@ -7,6 +7,7 @@ import jax.numpy as jnp
 import jax_dataclasses
 import numpy as np
 
+import jaxsim.math
 import jaxsim.typing as jtp
 from jaxsim import exceptions
 
@@ -41,7 +42,7 @@ class Terrain(abc.ABC):
             [(h_xm - h_xp) / (2 * self.delta), (h_ym - h_yp) / (2 * self.delta), 1.0]
         )
 
-        return n / jnp.linalg.norm(n)
+        return n / jaxsim.math.safe_norm(n)
 
 
 @jax_dataclasses.pytree_dataclass

{jaxsim-0.5.1.dev91.dist-info → jaxsim-0.5.1.dev95.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: jaxsim
-Version: 0.5.1.dev91
+Version: 0.5.1.dev95
 Summary: A differentiable physics engine and multibody dynamics library for control and robot learning.
 Author-email: Diego Ferigo <dgferigo@gmail.com>, Filippo Luca Ferretti <filippoluca.ferretti@outlook.com>
 Maintainer-email: Filippo Luca Ferretti <filippo.ferretti@iit.it>, Alessandro Croci <alessandro.croci@iit.it>

{jaxsim-0.5.1.dev91.dist-info → jaxsim-0.5.1.dev95.dist-info}/RECORD

@@ -1,5 +1,5 @@
 jaxsim/__init__.py,sha256=opgtbhhd1kDsHI4H1vOd3loMPDRi884yQ3tohfFGfNc,3382
-jaxsim/_version.py,sha256=HHdXV3EXu0rha3QfUW2g4pSsGNWqjfhD2e1Qwed6NGk,426
+jaxsim/_version.py,sha256=uLmioR8f_16hLO76GPSpzQyclhE2S-4nH8pntMObYGY,426
 jaxsim/exceptions.py,sha256=Sq3qtqeiy-CK76new_W2KKQ-4MAzyOUK5j5pBLr4RPQ,2250
 jaxsim/logging.py,sha256=STI-D_upXZYX-ZezLrlJJ0UlD5YspST0vZ_DcIwkzO4,1553
 jaxsim/typing.py,sha256=2HXy9hgazPXjofi1vLQ09ZubPtgVmg80U9NKmZ6NYiI,761
@@ -7,7 +7,7 @@ jaxsim/api/__init__.py,sha256=8eV22t2S3UwNyCg8karPetG1dmX1VDBXkyv28_FwNQA,210
 jaxsim/api/com.py,sha256=5fYNRUhKE5VGGdW88zY8mqqEy5VTWyaHu5k6MgW4Jt4,13826
 jaxsim/api/common.py,sha256=SvEOGxCKOxKLLVHaNp1sFkBX0sku3-wH0-HUlYVWCDk,7090
 jaxsim/api/contact.py,sha256=vfW-HEvQcAUHl7dOOwI-ndRxgMeAtkKT7tTaMDFlh7k,25421
-jaxsim/api/data.py,sha256=hz8g-P0o7XoDYyUFPx6yA8QlgHmjfFf2_OdiwcRV6W8,30292
+jaxsim/api/data.py,sha256=AFp1sDNRIkwpBom6ZlW6L7vJBtf4D9woVRJ8bGICr3s,30189
 jaxsim/api/frame.py,sha256=d6pa6vywGDqfaJU76F_-yjLJs6R3mrjZ6B-KXPu6f3Q,14595
 jaxsim/api/joint.py,sha256=AnqlNWmBOay-gsoo0y4AbfFQ2OCJm-8T1E0IMhZeLoY,7457
 jaxsim/api/kin_dyn_parameters.py,sha256=wnto0nzzEJ_M8tH2PUdldEyxQwQdsStYUoQFu696uuw,29897
@@ -20,15 +20,16 @@ jaxsim/integrators/__init__.py,sha256=hxvOD-VK_mmd6v31wtC-nb28AYve1gLuZCNLV9wS-K
 jaxsim/integrators/common.py,sha256=fnDqVIIXMYe2aiT_qnEhJSAeFuYRGhmElVCl7zPTrN8,18229
 jaxsim/integrators/fixed_step.py,sha256=KpjRd6hHtapxDoo6D1kyDrVDSHnke2TepI5grFH7_bM,2693
 jaxsim/integrators/variable_step.py,sha256=HuUKudeFj0W7dvVATVNZK3uk1Nh_qKlGO_CDqXJFV14,22166
-jaxsim/math/__init__.py,sha256=8oPITEoGwgRcOeG8KxtqxPQ8b5uku1HNRMokpCoi9Tc,352
+jaxsim/math/__init__.py,sha256=2T1WUU_chNBCvyvkKSdiesPlckbo-gXVbCZEGoF-W0I,381
 jaxsim/math/adjoint.py,sha256=V7r5VrTCKPLEL5gavNSx9U7xSsrb11a5e4gWqJ2MuRo,4375
 jaxsim/math/cross.py,sha256=U7yEx_l75mSy5g6O-jsjBztApvxC3WaV4MpkS5tThu4,1330
 jaxsim/math/inertia.py,sha256=01hz6wMFreN2jBA0rVoBS1YMVh77KvwuzXSOpI3pxNk,1614
 jaxsim/math/joint_model.py,sha256=EzAveaG5B6ZnCFNUzN30KEQUVesd83lfWXJarYR-kUw,9989
-jaxsim/math/quaternion.py,sha256=_WA7W3iv7px83sWO1V1n0-J78hqAlO4SL1-jofE-UZ4,4754
-jaxsim/math/rotation.py,sha256=P34sx28Rh1MhNwxUxqXjxP-ZN1_5tvoflMoAIpy2LbE,2586
+jaxsim/math/quaternion.py,sha256=vrPkdSUPfv1RZOULY9uG_bxmqgOARdvMxKtb6QixHuY,4609
+jaxsim/math/rotation.py,sha256=W6vaIWmoBBuPgNJKey1vFpl2a1IxXTToCTaPfs-kd9I,2155
 jaxsim/math/skew.py,sha256=oOGSSR8PUGROl6IJFlrmu6K3gPH-u16hUPfKIkcVv9o,1177
 jaxsim/math/transform.py,sha256=KXzQgOnCfAtbXCwxhplpJ3F0JT3oEyeLVby1_uRAryQ,2892
+jaxsim/math/utils.py,sha256=C5kP11KWsIacWtzouaI5tNH8BHjZ-ZgZ67U9wzjz7jw,1070
 jaxsim/mujoco/__init__.py,sha256=fZyRWre49pIhOrYdf6yJk_hOax8qWGe8OCmoq-dMVq8,201
 jaxsim/mujoco/__main__.py,sha256=GBmB7J-zj75ZnFyuAAmpSOpbxi_HhHhWJeot3ljGDJY,5291
 jaxsim/mujoco/loaders.py,sha256=_CZekIqZNe8oFeH7zSv4gGZAZENRISwMd8dt640zjRI,20860
@@ -58,16 +59,16 @@ jaxsim/rbda/contacts/__init__.py,sha256=L5MM-2pv76YPGzxExdz2EErgGBATuAjYnNHlq5QO
 jaxsim/rbda/contacts/common.py,sha256=BjwZMCkzd1ZOdZW7_Zt09Cl5j2JUHXM5Q8ao_qS6e64,10406
 jaxsim/rbda/contacts/relaxed_rigid.py,sha256=PgwKfProN5sLXJsSov3nIidHHMVpJqIp7eIv6_bPGjs,20345
 jaxsim/rbda/contacts/rigid.py,sha256=X-PE6PmZqlKoZTY6JhYBSW-vom-rq2uBKmBUNQeQHCg,15991
-jaxsim/rbda/contacts/soft.py,sha256=sIWT4NUJmoVR5T1Fo0ExdPfzL_gPfiPiB-9CFuotE_s,15567
+jaxsim/rbda/contacts/soft.py,sha256=MhqHThn3XVyy8lRg6QVYzcJF4vVRKwnMvOMEyyF96OE,15443
 jaxsim/rbda/contacts/visco_elastic.py,sha256=QhyJHjDowyBTAhoSdZcCIkOqzp__gMXhLON-qYyMgQc,39886
 jaxsim/terrain/__init__.py,sha256=f7lVX-iNpH_wkkjef9Qpjh19TTAUOQw76EiLYJDVizc,78
-jaxsim/terrain/terrain.py,sha256=_G1QS3zWycj089R8fTP5s2VjcZpEdJxREjXZJ-oXIvc,5248
+jaxsim/terrain/terrain.py,sha256=bv-YAwG06EydHnB6bcNtl7xIyB3LSl0vVXSVLFC4JpQ,5273
 jaxsim/utils/__init__.py,sha256=Y5zyoRevl3EMVQadhZ4EtSwTEkDt2vcnFoRhPJjKTZ0,215
 jaxsim/utils/jaxsim_dataclass.py,sha256=TGmTQV2Lq7Q-2nLoAEaeNtkPa_qj0IKkdBm4COj46Os,11312
 jaxsim/utils/tracing.py,sha256=eEY28MZW0Lm_jJNt1NkFqZz0ek01tvhR46OXZYCo7tc,532
 jaxsim/utils/wrappers.py,sha256=ZY7olSORzZRvSzkdeNLj8yjwUIAt9L0Douwl7wItjpk,4008
-jaxsim-0.5.1.dev91.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
-jaxsim-0.5.1.dev91.dist-info/METADATA,sha256=PPDDHpeFVoVidQMnmZcNT_Spo8ryctjuFISZWYM_ZgI,17937
-jaxsim-0.5.1.dev91.dist-info/WHEEL,sha256=PZUExdf71Ui_so67QXpySuHtCi3-J3wvF4ORK6k_S8U,91
-jaxsim-0.5.1.dev91.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
-jaxsim-0.5.1.dev91.dist-info/RECORD,,
+jaxsim-0.5.1.dev95.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
+jaxsim-0.5.1.dev95.dist-info/METADATA,sha256=xOMthPKBfJqlJrzyVADzPjB5b-h8MKI3K8CGRdcKZy4,17937
+jaxsim-0.5.1.dev95.dist-info/WHEEL,sha256=A3WOREP4zgxI0fKrHUG8DC8013e3dK3n7a6HDbcEIwE,91
+jaxsim-0.5.1.dev95.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
+jaxsim-0.5.1.dev95.dist-info/RECORD,,

{jaxsim-0.5.1.dev91.dist-info → jaxsim-0.5.1.dev95.dist-info}/WHEEL

@@ -1,5 +1,5 @@
 Wheel-Version: 1.0
-Generator: setuptools (75.6.0)
+Generator: setuptools (75.7.0)
 Root-Is-Purelib: true
 Tag: py3-none-any