From 495841025eb95494ab9b026c981ddbe5afbfe1cf Mon Sep 17 00:00:00 2001 From: ctingley Date: Tue, 14 Jan 2025 22:47:47 -0800 Subject: [PATCH 1/4] Angle Axis Gradient Fix --- src/jaxsim/math/rotation.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/jaxsim/math/rotation.py b/src/jaxsim/math/rotation.py index 58d730ee7..741a436b6 100644 --- a/src/jaxsim/math/rotation.py +++ b/src/jaxsim/math/rotation.py @@ -78,7 +78,8 @@ def theta_is_not_zero(axis: jtp.Vector) -> jtp.Matrix: c1 = 2 * jnp.sin(theta / 2.0) ** 2 - u = v / theta + safe_theta = jnp.where(theta == 0, 1.0, theta) + u = v / safe_theta u = jnp.vstack(u.squeeze()) R = c * jnp.eye(3) - s * Skew.wedge(u) + c1 * u @ u.T From c017596ad8d8566e41f151ea93f5c8203839c8b7 Mon Sep 17 00:00:00 2001 From: ctingley Date: Wed, 15 Jan 2025 08:43:37 -0800 Subject: [PATCH 2/4] Removed theta_is_not_zero function --- src/jaxsim/math/rotation.py | 29 ++++++++++------------------- 1 file changed, 10 insertions(+), 19 deletions(-) diff --git a/src/jaxsim/math/rotation.py b/src/jaxsim/math/rotation.py index 741a436b6..ededbfa08 100644 --- a/src/jaxsim/math/rotation.py +++ b/src/jaxsim/math/rotation.py @@ -68,27 +68,18 @@ def from_axis_angle(vector: jtp.Vector) -> jtp.Matrix: vector = vector.squeeze() - def theta_is_not_zero(axis: jtp.Vector) -> jtp.Matrix: + theta = safe_norm(vector) - v = axis - theta = safe_norm(v) + s = jnp.sin(theta) + c = jnp.cos(theta) - s = jnp.sin(theta) - c = jnp.cos(theta) + c1 = 2 * jnp.sin(theta / 2.0) ** 2 - c1 = 2 * jnp.sin(theta / 2.0) ** 2 + safe_theta = jnp.where(theta == 0, 1.0, theta) + u = vector / safe_theta + u = jnp.vstack(u.squeeze()) - safe_theta = jnp.where(theta == 0, 1.0, theta) - u = v / safe_theta - u = jnp.vstack(u.squeeze()) + R = c * jnp.eye(3) - s * Skew.wedge(u) + c1 * u @ u.T + - R = c * jnp.eye(3) - s * Skew.wedge(u) + c1 * u @ u.T - - return R.transpose() - - return jnp.where( - 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), - ) + return R.transpose() From b07a472f34e67b1e28773def7127127a646ffa93 Mon Sep 17 00:00:00 2001 From: ctingley Date: Wed, 15 Jan 2025 08:53:56 -0800 Subject: [PATCH 3/4] Removed whitespace --- src/jaxsim/math/rotation.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/jaxsim/math/rotation.py b/src/jaxsim/math/rotation.py index ededbfa08..7c305dc8e 100644 --- a/src/jaxsim/math/rotation.py +++ b/src/jaxsim/math/rotation.py @@ -81,5 +81,4 @@ def from_axis_angle(vector: jtp.Vector) -> jtp.Matrix: R = c * jnp.eye(3) - s * Skew.wedge(u) + c1 * u @ u.T - - return R.transpose() + return R.transpose() \ No newline at end of file From a5485226417d01ab5ead9f9446c1f1f01e4803ec Mon Sep 17 00:00:00 2001 From: ctingley Date: Wed, 15 Jan 2025 09:13:19 -0800 Subject: [PATCH 4/4] Line Endings --- src/jaxsim/math/rotation.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/jaxsim/math/rotation.py b/src/jaxsim/math/rotation.py index 7c305dc8e..bcbe98a5f 100644 --- a/src/jaxsim/math/rotation.py +++ b/src/jaxsim/math/rotation.py @@ -80,5 +80,5 @@ def from_axis_angle(vector: jtp.Vector) -> jtp.Matrix: u = jnp.vstack(u.squeeze()) R = c * jnp.eye(3) - s * Skew.wedge(u) + c1 * u @ u.T - - return R.transpose() \ No newline at end of file + + return R.transpose()