Fixed fabs() use in quaternion interpolation #1479

werner291 · 2022-08-05T12:36:00Z

Description

See issue #1478, it appears that the formula got copied wrong from Wikipedia, as applying the absolute-value function in the place it was at prevented detection of the long-way-around case.

The new code seems to work for the case mentioned in the issue, I'll see if I can test it a bit more thoroughly.

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

… interpolation from taking the long path.

codecov · 2022-08-05T12:54:02Z

Codecov Report

Merging #1479 (70fa710) into main (0b5a7fa) will increase coverage by 0.12%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##             main    #1479      +/-   ##
==========================================
+ Coverage   50.99%   51.10%   +0.12%     
==========================================
  Files         380      380              
  Lines       31743    31741       -2     
==========================================
+ Hits        16184    16219      +35     
+ Misses      15559    15522      -37

Impacted Files	Coverage Δ
...veit_core/robot_model/src/floating_joint_model.cpp	`60.21% <100.00%> (+16.92%)`	⬆️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	`76.43% <0.00%> (+1.08%)`	⬆️
.../include/moveit/robot_model/floating_joint_model.h	`25.00% <0.00%> (+25.00%)`	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

…e new.

AndyZe

Some quick comments on the code. I'll come back to do my best to check the quaternion math

moveit_core/robot_model/test/test.cpp

AndyZe · 2022-08-05T14:55:03Z

moveit_core/robot_model/src/floating_joint_model.cpp

-  double dq = fabs(from[3] * to[3] + from[4] * to[4] + from[5] * to[5] + from[6] * to[6]);
-  double theta = (dq + std::numeric_limits<double>::epsilon() >= 1.0) ? 0.0 : acos(dq);
+  double dq = from[3] * to[3] + from[4] * to[4] + from[5] * to[5] + from[6] * to[6];
+  double theta = (fabs(dq) + std::numeric_limits<double>::epsilon() >= 1.0) ? 0.0 : acos(fabs(dq));


Isn't there a factor of 2 missing here?

This website gives this formula, note the 2:

cos(theta/2) = qa.w*qb.w + qa.x*qb.x + qa.y*qb.y+ qa.z*qb.z

I'm not sure if it's really missing, the implementation (almost) matches the example code on the website you linked. theta could probably be renamed to halfTheta, though.

The factor 2 comes from converting between axis-angle and quaternion. It's because quaternions are weird: you don't just multiply them with the thing you want to transform (though that's how most linalg libraries typically have their API set up) you pre-multiply with the quaternion (usually after converting your vector to a complex number), then post-multiply with the inverse quaternion. Somehow, that 4D weirdness spawns a factor 2.

Also, I don't see how the test could possible be passing if I was missing something as big as a factor 2 in there.

AndyZe · 2022-08-05T15:07:04Z

Somebody didn't drink their coffee before they coded this up ;)

There must be a SLERP function somewhere else in MoveIt you could use...

henningkayser

Thank you for the fix, it looks correct to me. However, I somewhat agree with Andy that it would be cleaner to use another library for that. How about simply calling Eigen::Quaternion::slerp()?

henningkayser · 2022-08-05T15:20:43Z

moveit_core/robot_model/src/floating_joint_model.cpp

-  double dq = fabs(from[3] * to[3] + from[4] * to[4] + from[5] * to[5] + from[6] * to[6]);
-  double theta = (dq + std::numeric_limits<double>::epsilon() >= 1.0) ? 0.0 : acos(dq);
+  double dq = from[3] * to[3] + from[4] * to[4] + from[5] * to[5] + from[6] * to[6];
+  double theta = (fabs(dq) + std::numeric_limits<double>::epsilon() >= 1.0) ? 0.0 : acos(fabs(dq));


I'm not sure if it's really missing, the implementation (almost) matches the example code on the website you linked. theta could probably be renamed to halfTheta, though.

henningkayser · 2022-08-05T15:22:12Z

moveit_core/robot_model/test/test.cpp

+  double jv1[7];
+  double jv2[7];
+  double intp[7];
+  random_numbers::RandomNumberGenerator rng;


There is currently some ongoing refactoring around RNG, see #1267. @sjahr any thoughts if we should merge this and refactor later?

It's not half-theta at all, that really is the angle between the two quaternions. The halving only shows up when you when you convert between Quaternions and axis-angle stuff to my knowledge

100% agree that just calling into Eigen would be way easier (and probably better/safer) than DIY-ing the math ourselves; I just didn't feel confident suggesting a larger rewrite than was necessary to fix the bug.

Do note that Eigen's Quaternion parameters are in a different order than MoveIt's.

I think we can merge this now b/c it's an obvious improvement. Thanks! especially for the new test

I'm still not sure I agree on the factor of 2 though. We can make an issue and figure it out later.

Oh I think I misread your website. Lemme take a look at what they're doing, Quaternions are tricky

Ok, found a source here that features the *2 as well https://nl.mathworks.com/matlabcentral/answers/415936-angle-between-2-quaternions

Here's a fantastic introduction to quaternions that only slightly breaks my brain: https://eater.net/quaternions

https://math.stackexchange.com/questions/4053346/the-angle-between-two-quaternion-vectors-is-equal-to-half-the-angle-between-thei

that given two quaternions q1,q2∈R4, the angle between the 4D quaternion vectors is equal to half the angle between their corresponding 3D orientations.

Oh my god the angle is half the angle... I think this is a definitions issue. Do we want the angle between rotations, or do we want the angle between the quaternions?

On the referenced Wikipedia article: "Compute Ω as the angle subtended by the arc, so that cos Ω = p0 ∙ p1, the n-dimensional dot product of the unit vectors from the origin to the ends."

So (if I read this correctly) it's the angle between the vectors (quaternions are basically 4D vectors), not the angle between the rotations that the quaternions represent. Therefore, it's theta and not half-theta.

This also reinforces my belief that we really should just let Eigen handle this...

In light of that explanation, this code here (https://github.com/ros-planning/moveit2/blob/main/moveit_core/robot_model/src/floating_joint_model.cpp#L122) is also wrong (the method name explicitly mentions "rotation" and not "quaternion angle"), and is missing the factor 2.

(Doesn't change anything for our case since the slerp operation explicitly requires the angle between quaternions, not rotations)

Co-authored-by: AndyZe <andyz@utexas.edu>

…ches and hopefully take advantage of Eigen probably having a better implementation than us.

werner291 · 2022-08-06T09:04:48Z

So I wrote a version that uses Eigen... Feel free to reject these if the changes are a bit too radical. Test shows no difference, though, and I'm a strong supporter of using Eigen instead of DIY math wherever possible.

werner291 · 2022-08-06T09:11:14Z

Oh, this line is a significant change if (abs(from[3] - to[3]) + abs(from[4] - to[4]) + abs(from[5] - to[5]) + abs(from[6] - to[6]) > std::numeric_limits<double>::epsilon())

Though really, all it does is skip the quaternion math if the rotation parts are not significantly different. The original code tested whether theta was significantly > 0, so I'd argue same difference.

AndyZe

Thank you! I have one small documentation suggest. LGTM besides that

moveit_core/robot_model/src/floating_joint_model.cpp

* Interpolate using Eigen::Quaternion::slerp() to (hopefully) save us further headaches and take advantage of Eigen probably having a better implementation than us. * Created a test case that fails for the old version, but passes for the new. Co-authored-by: AndyZe <zelenak@picknik.ai> (cherry picked from commit 59e69bb)

* Interpolate using Eigen::Quaternion::slerp() to (hopefully) save us further headaches and take advantage of Eigen probably having a better implementation than us. * Created a test case that fails for the old version, but passes for the new. (cherry picked from commit 59e69bb) Co-authored-by: werner291 <werner.kroneman@gmail.com> Co-authored-by: Henning Kayser <henningkayser@picknik.ai> Co-authored-by: AndyZe <zelenak@picknik.ai>

Corrected the placement of the fabs() operation to prevent quaternion…

7fc2104

… interpolation from taking the long path.

Created a test case that fails for the old version, but passes for th…

ced91bc

…e new.

werner291 marked this pull request as ready for review August 5, 2022 13:34

werner291 changed the title ~~Corrected the placement of the fabs() operation to prevent quaternion…~~ Fixed fabs() use in quaterion interpolation Aug 5, 2022

AndyZe reviewed Aug 5, 2022

View reviewed changes

moveit_core/robot_model/test/test.cpp Outdated Show resolved Hide resolved

moveit_core/robot_model/test/test.cpp Outdated Show resolved Hide resolved

AndyZe reviewed Aug 5, 2022

View reviewed changes

henningkayser reviewed Aug 5, 2022

View reviewed changes

werner291 and others added 3 commits August 5, 2022 17:54

Update moveit_core/robot_model/test/test.cpp

513868d

Co-authored-by: AndyZe <andyz@utexas.edu>

Moved joint bounds outside of loop

f2ef841

Merge branch 'main' into issue-1478

b27bd2d

AndyZe approved these changes Aug 5, 2022

View reviewed changes

werner291 added 2 commits August 6, 2022 11:03

Version of the code using Eigen, to (hopefully) save us further heada…

ea133d0

…ches and hopefully take advantage of Eigen probably having a better implementation than us.

Merge remote-tracking branch 'werner/issue-1478' into issue-1478

a5b9d3c

AndyZe approved these changes Aug 8, 2022

View reviewed changes

moveit_core/robot_model/src/floating_joint_model.cpp Show resolved Hide resolved

AndyZe added 2 commits August 8, 2022 08:18

Comment about quaternion difference

d3db1e6

Merge branch 'main' into issue-1478

70fa710

henningkayser changed the title ~~Fixed fabs() use in quaterion interpolation~~ Fixed fabs() use in quaternion interpolation Aug 9, 2022

henningkayser added backport-foxy Mergify label that triggers a PR backport to Foxy backport-galactic Mergify label that triggers a PR backport to Galactic backport-humble Mergify label that triggers a PR backport to Humble labels Aug 9, 2022

henningkayser approved these changes Aug 9, 2022

View reviewed changes

henningkayser merged commit 59e69bb into moveit:main Aug 9, 2022

mergify bot mentioned this pull request Aug 9, 2022

Fixed fabs() use in quaternion interpolation (backport #1479) #1494

Merged

mergify bot mentioned this pull request Aug 9, 2022

Fixed fabs() use in quaternion interpolation (backport #1479) #1495

Merged

mergify bot mentioned this pull request Aug 9, 2022

Fixed fabs() use in quaternion interpolation (backport #1479) #1496

Merged

henningkayser added a commit that referenced this pull request Aug 9, 2022

Fixed fabs() use in quaternion interpolation (backport #1479) (#1496)

3eef088

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fixed fabs() use in quaternion interpolation #1479

Fixed fabs() use in quaternion interpolation #1479

werner291 commented Aug 5, 2022 •

edited

Loading

codecov bot commented Aug 5, 2022 •

edited

Loading

AndyZe left a comment

AndyZe Aug 5, 2022

henningkayser Aug 5, 2022 •

edited

Loading

werner291 Aug 5, 2022 •

edited

Loading

AndyZe commented Aug 5, 2022

henningkayser left a comment

henningkayser Aug 5, 2022 •

edited

Loading

henningkayser Aug 5, 2022

werner291 Aug 5, 2022

werner291 Aug 5, 2022 •

edited

Loading

AndyZe Aug 5, 2022

werner291 Aug 6, 2022

werner291 Aug 6, 2022 •

edited

Loading

werner291 Aug 6, 2022

werner291 Aug 6, 2022 •

edited

Loading

werner291 Aug 6, 2022

werner291 commented Aug 6, 2022

werner291 commented Aug 6, 2022 •

edited

Loading

AndyZe left a comment

Fixed fabs() use in quaternion interpolation #1479

Fixed fabs() use in quaternion interpolation #1479

Conversation

werner291 commented Aug 5, 2022 • edited Loading

Description

Checklist

codecov bot commented Aug 5, 2022 • edited Loading

Codecov Report

AndyZe left a comment

Choose a reason for hiding this comment

AndyZe Aug 5, 2022

Choose a reason for hiding this comment

henningkayser Aug 5, 2022 • edited Loading

Choose a reason for hiding this comment

werner291 Aug 5, 2022 • edited Loading

Choose a reason for hiding this comment

AndyZe commented Aug 5, 2022

henningkayser left a comment

Choose a reason for hiding this comment

henningkayser Aug 5, 2022 • edited Loading

Choose a reason for hiding this comment

henningkayser Aug 5, 2022

Choose a reason for hiding this comment

werner291 Aug 5, 2022

Choose a reason for hiding this comment

werner291 Aug 5, 2022 • edited Loading

Choose a reason for hiding this comment

AndyZe Aug 5, 2022

Choose a reason for hiding this comment

werner291 Aug 6, 2022

Choose a reason for hiding this comment

werner291 Aug 6, 2022 • edited Loading

Choose a reason for hiding this comment

werner291 Aug 6, 2022

Choose a reason for hiding this comment

werner291 Aug 6, 2022 • edited Loading

Choose a reason for hiding this comment

werner291 Aug 6, 2022

Choose a reason for hiding this comment

werner291 commented Aug 6, 2022

werner291 commented Aug 6, 2022 • edited Loading

AndyZe left a comment

Choose a reason for hiding this comment

werner291 commented Aug 5, 2022 •

edited

Loading

codecov bot commented Aug 5, 2022 •

edited

Loading

henningkayser Aug 5, 2022 •

edited

Loading

werner291 Aug 5, 2022 •

edited

Loading

henningkayser Aug 5, 2022 •

edited

Loading

werner291 Aug 5, 2022 •

edited

Loading

werner291 Aug 6, 2022 •

edited

Loading

werner291 Aug 6, 2022 •

edited

Loading

werner291 commented Aug 6, 2022 •

edited

Loading