Changes the quat_box_minus implementation and adds quat_box_plus and rigid_body_twist_transform #2217
Conversation
jtigue-bdai
requested review from
jsmith-bdai,
Mayankm96 and
kellyguo11
as code owners
| quat_diff = quat_mul(q1, quat_conjugate(q2)) # q1 * q2^-1 | ||
| return axis_angle_from_quat(quat_diff) # log(qd) |
There was a problem hiding this comment.
Hmm was the previous implementation wrong? I remember checking that the above doesn't work well for small angles so made a better implementation.
There was a problem hiding this comment.
Hey @Mayankm96 I added some explanation to the PR description and on the removed portions of the code. Let me know if you have any questions.
| def rigid_body_twist_transform( | ||
| v0: torch.Tensor, w0: torch.Tensor, t01: torch.Tensor, q01: torch.Tensor | ||
| ) -> tuple[torch.Tensor, torch.Tensor]: | ||
| r"""Transform the linear and angular velocity of a rigid body between reference frames. |
There was a problem hiding this comment.
Might need to fix the docstring enters to make sure it is rendered properly.
| def quat_box_minus(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: | ||
| """The box-minus operator (quaternion difference) between two quaternions. | ||
|
|
||
| Args: | ||
| q1: The first quaternion in (w, x, y, z). Shape is (N, 4). | ||
| q2: The second quaternion in (w, x, y, z). Shape is (N, 4). | ||
|
|
||
| Returns: | ||
| The difference between the two quaternions. Shape is (N, 3). | ||
| """ | ||
| quat_diff = quat_mul(q1, quat_conjugate(q2)) # q1 * q2^-1 | ||
| re = quat_diff[:, 0] # real part, q = [w, x, y, z] = [re, im] | ||
| im = quat_diff[:, 1:] # imaginary part | ||
| norm_im = torch.norm(im, dim=1) | ||
| scale = 2.0 * torch.where(norm_im > 1.0e-7, torch.atan2(norm_im, re) / norm_im, torch.sign(re)) | ||
| return scale.unsqueeze(-1) * im |
There was a problem hiding this comment.
-
When atan2 is being used in this function, it doesn’t account for the sign of re . Notice how the approximation takes the sign of re to compute the scale value when norm_im is sufficiently small but doesn’t take that into account for the non-approximated value.
-
This causes that whenever there is a negative real part of the quaternion difference
re, the scale will overestimate the rotation and will take the longest path between orientations instead of the shortest. E.g. whenever we go from -0.1 rad to 0.1 rad in a single axis this will compute a -(2pi-0.2) rad rotation instead of a 0.2 rad rotation. -
This will happen whenever
re = quat_diff[:,0]is negative.
Description
Reason for Change
While using
quat_box_minusto get angular velocities through finite differences we encountered that the angular velocity contained some unreasonably large values at given points along the trajectory. After some investigation we observed:quat_box_minusyielding unreasonably large axis-angle differences at some points (”outliers”) of the trajectory.quat_diff = quat_mul(q1, quat_conjugate(q2))) becomes negative (Type of change
Checklist
pre-commitchecks with./isaaclab.sh --formatconfig/extension.tomlfileCONTRIBUTORS.mdor my name already exists there