Skip to content

Conversation

sdhar04
Copy link

@sdhar04 sdhar04 commented Feb 7, 2024

🎉 New feature

Closes #568

Summary

I have added conditionals for using the Slerp algorithm when the difference in final and initial orientations falls within a threshold. Otherwise Squad is invoked.

Test it

You can open Gazebo and run the bash script I have added in gz-math/test which publishes a new position and orientation to the camera every 2 seconds.

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

Signed-off-by: sdhar04 <sammit.dhar.2004@gmail.com>
@github-actions github-actions bot added 🌱 garden Ignition Garden 🎵 harmonic Gazebo Harmonic labels Feb 7, 2024
@@ -0,0 +1,20 @@
#!/bin/bash
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why is this file required? It looks like an example, a test required some expectations or asserts

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The issue asks for smoother movements which are brought about by Slerp under the threshold, while Squad works just fine above it. I don’t know how to assert if a given interpolation is smooth or not, so I added this file for anyone to check the movements themselves.

@sdhar04 sdhar04 requested a review from ahcorde February 21, 2024 05:45
Signed-off-by: sdhar04 <sammit.dhar.2004@gmail.com>
@sdhar04
Copy link
Author

sdhar04 commented Mar 10, 2024

@ahcorde I have removed the bash script

@mjcarroll
Copy link
Contributor

@sdhar04 Can you address these lint issues?

  /github/workspace/src/RotationSpline.cc:95:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]
  /github/workspace/src/RotationSpline.cc:98:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]
  /github/workspace/src/RotationSpline.cc:102:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]
  /github/workspace/src/RotationSpline.cc:104:  Missing spaces around <  [whitespace/operators] [3]
  /github/workspace/src/RotationSpline.cc:108:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]

Copy link

codecov bot commented Mar 28, 2024

Codecov Report

Attention: Patch coverage is 87.50000% with 1 lines in your changes are missing coverage. Please review.

Project coverage is 94.11%. Comparing base (91c73b9) to head (7dfe977).

❗ Current head 7dfe977 differs from pull request most recent head cf8b74c. Consider uploading reports for the commit cf8b74c to get more accurate results

Files Patch % Lines
src/RotationSpline.cc 87.50% 1 Missing ⚠️
Additional details and impacted files
@@             Coverage Diff              @@
##           gz-math7     #577      +/-   ##
============================================
- Coverage     94.11%   94.11%   -0.01%     
============================================
  Files           146      146              
  Lines          9809     9816       +7     
============================================
+ Hits           9232     9238       +6     
- Misses          577      578       +1     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

Signed-off-by: sdhar04 <sammit.dhar.2004@gmail.com>
@sdhar04
Copy link
Author

sdhar04 commented Mar 28, 2024

@mjcarroll @azeey I have addressed the lint issues and reduced the scope of 2 variables only required for Squad interpolation. However, the tests don't seem to invoke the cases where Slerp is used, hence the codecov warnings. I have a bash script I used during testing to supply target orientations and both algorithms are used in my test cases (I checked by printing which one was invoked), with the interpolation being desirably smooth.
I have seen the tests and they seem to check for the midpoint of an interpolation. I don't know what the midpoint would be for a given Slerp interpolation, although I could try to figure things out by printing values while interpolating. Please guide me regarding what to do now. Do I need to change the RotationSpline_TEST.cc file only?

Copy link
Contributor

@ahcorde ahcorde left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@azeey or @scpeters what do you think about this ?

using namespace gz;
using namespace math;

using namespace std;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

remove this

@azeey azeey self-assigned this Jul 22, 2024
@azeey azeey added the beta Targeting beta release of upcoming collection label Jul 29, 2024
@arjo129
Copy link
Contributor

arjo129 commented Aug 12, 2024

Hi @sdhar04 , was wondering what the status on this was. Do we intend shipping this change with ionic (code freeze 26/8)? If we can't come to a consensus by then I will remove the beta label (I personally would prefer seeing it in this release).

@arjo129
Copy link
Contributor

arjo129 commented Aug 19, 2024

Hi @sdhar04 as we haven't heard back from you, I'm removing the beta label. @ahcorde's suggestions still apply.

@arjo129 arjo129 removed the beta Targeting beta release of upcoming collection label Aug 19, 2024
@azeey
Copy link
Contributor

azeey commented Jan 7, 2025

Hi @sdhar04, any updates on this PR?

@sdhar04
Copy link
Author

sdhar04 commented Jan 10, 2025

Hello @azeey. Sorry I hadn't seen the previous messages. This PR is currently having an issue with tests and codecov. Slerp interpolation follows a different path than Squad so it does not pass the RotationSpline test which specifically checks for the midpoint of the path followed according to Squad interpolation, hence I am looking into adding a suitable test condition for it, and have been studying up on the relevant concepts for quite a while. I have also tried changing the conditions so that Squad is used for all the tests but that is an obvious codecov issue.

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
Signed-off-by: Sammit Dhar <sammit.dhar.2004@gmail.com>
@azeey
Copy link
Contributor

azeey commented Jul 28, 2025

Hi @sdhar04, there hasn't been much progress in this PR for a while. Are you still planning to address the pending issues or should we go ahead and close it?

@sdhar04
Copy link
Author

sdhar04 commented Jul 28, 2025

Hi @azeey
Sorry I had totally forgotten about this. I have a possible solution in mind, and will get started with it right away.

@azeey azeey added this to the Jetty Release milestone Jul 28, 2025
@azeey
Copy link
Contributor

azeey commented Jul 28, 2025

Okay, great!

sdhar04 added 4 commits July 31, 2025 04:15
According to review

Signed-off-by: Sammit Dhar <sammit.dhar.2004@gmail.com>
Test for Slerp

Signed-off-by: Sammit Dhar <sammit.dhar.2004@gmail.com>
Correctly changed the checks where Slerp is now called

Signed-off-by: Sammit Dhar <sammit.dhar.2004@gmail.com>
Changed one test as it was too brittle

Signed-off-by: Sammit Dhar <sammit.dhar.2004@gmail.com>
@sdhar04 sdhar04 requested a review from ahcorde August 2, 2025 08:12
@sdhar04
Copy link
Author

sdhar04 commented Aug 2, 2025

Hello @azeey. I have made the requested changes, and changed the values in RotationSpline_TEST.cc so that it checks for the Slerp interpolation midpoints wherever it is invoked, and Squad midpoints in the other cases. This should solve the codecov issue. The repo builds on my system without any error, but it's failing here.
I suspect it is because the tests right now check for equality assertions to the 6th decimal place, and adding some tolerance could solve this. What are your thoughts on this approach?

Copy link
Contributor

@azeey azeey left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm concerned about how we're calculating the difference between the two quaternions. What do you think of my suggestion?

Also, could you also fix the failing python test?

Comment on lines 94 to 102
Vector3d peu(p.Euler());
Vector3d qeu(q.Euler());

double diffX = abs(peu.X()-qeu.X());
double diffY = abs(peu.Y()-qeu.Y());
double diffZ = abs(peu.Z()-qeu.Z());

// NB interpolate to nearest rotation
return Quaterniond::Squad(_t, p, a, b, q, _useShortestPath);
if ((diffX < 0.16) || (diffY < 0.16) || (diffZ < 0.16))
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Comparing euler angles like this doesn't seem like a safe thing to do. I would suggest calculating the auto diffQ = p.Inverse()*q and then doing const double diff = 2*acos(diffQ.W())) and check the threshold against that.

@azeey azeey removed this from the Jetty Release milestone Aug 11, 2025
Signed-off-by: sdhar04 <sammit.dhar.2004@gmail.com>
@sdhar04 sdhar04 requested a review from azeey August 23, 2025 10:49
Signed-off-by: sdhar04 <sammit.dhar.2004@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

🌱 garden Ignition Garden 🎵 harmonic Gazebo Harmonic

Projects

Status: In review

Development

Successfully merging this pull request may close these issues.

Consider Slerp instead of Squad for rotation interpolation

5 participants