Add NaN check for quaternions

ami-iit · Feb 14, 2025 · 938d9ce · 938d9ce
1 parent 57fae8b
commit 938d9ce
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/src/jaxsim/rbda/utils.py b/src/jaxsim/rbda/utils.py
@@ -132,6 +132,12 @@ def process_inputs(
     if W_Q_B.shape != (4,):
         raise ValueError(W_Q_B.shape, (4,))
 
+    # Check that the quaternion does not contain NaNs.
+    exceptions.raise_value_error_if(
+        condition=jnp.isnan(W_Q_B).any(),
+        msg="A RBDA received a quaternion that contains NaNs.",
+    )
+
     # Check that the quaternion is unary since our RBDAs make this assumption in order
     # to prevent introducing additional normalizations that would affect AD.
     exceptions.raise_value_error_if(