Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added Conditions for Slerp vs Squad interpolation #577

Open
wants to merge 6 commits into
base: gz-math7
Choose a base branch
from
Open

Added Conditions for Slerp vs Squad interpolation #577

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
b6d7673
Added Conditions for Slerp vs Squad interpolation
sdhar04 Feb 7, 2024
9d504f3
Removed bash script
sdhar04 Mar 10, 2024
c9e01c2
Merge branch 'gz-math7' into gz-math7
mjcarroll Mar 28, 2024
039a64a
Addressed lint issues
sdhar04 Mar 28, 2024
7dfe977
Merge branch 'gz-math7' of https://github.com/sdhar04/gz-math into gz…
sdhar04 Mar 28, 2024
cf8b74c
Reduced scope of variables required only for squad interpolation
sdhar04 Mar 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 18 additions & 2 deletions src/RotationSpline.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,10 +16,12 @@
*/
#include "gz/math/Quaternion.hh"
#include "gz/math/RotationSpline.hh"
#include "cmath"
Comment on lines 16 to +19
Copy link
Contributor

Choose a reason for hiding this comment

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

Suggested change
*/
#include "gz/math/Quaternion.hh"
#include "gz/math/RotationSpline.hh"
#include "cmath"
*/
#include <cmath>
#include "gz/math/Quaternion.hh"
#include "gz/math/RotationSpline.hh"


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

/// \internal
/// \brief Private data for RotationSpline
class RotationSpline::Implementation
Expand Down Expand Up @@ -90,9 +92,23 @@ Quaterniond RotationSpline::Interpolate(const unsigned int _fromIndex,
Quaterniond &q = this->dataPtr->points[_fromIndex+1];
Quaterniond &a = this->dataPtr->tangents[_fromIndex];
Quaterniond &b = this->dataPtr->tangents[_fromIndex+1];


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))
{
return Quaterniond::Slerp(_t, p, q, _useShortestPath);
}
else
{
return Quaterniond::Squad(_t, p, a, b, q, _useShortestPath);
}
}

/////////////////////////////////////////////////
Expand Down
20 changes: 20 additions & 0 deletions test/interpolation.sh
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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.


#Movements in these cases are mostly smooth, and any abrupt movements caused by these cases are present in Slerp as well as Squad interpolation
gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean --timeout 500 --req 'pose: {position:{x: 27.2, y:-1.7215,z:2.2255}, orientation:{x:-0.1466788, y:0.0344671, z:0.9623708, w:0.2261413}}'
sleep 2
gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean --timeout 500 --req 'pose: {position:{x:24.081968307495117,y:1.0910710096359253,z:1.194319486618042},orientation:{x:0.10836052149534225,y:-0.0045970650389790535,z:-0.99320769309997559,w:-0.042135711759328842}}'
sleep 2
gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean --timeout 500 --req 'pose: {position:{x: 27.2, y:-1.7215,z:2.2255}, orientation:{x:-0.1466788, y:0.0344671, z:0.9623708, w:0.2261413}}'
sleep 2
gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean --timeout 500 --req 'pose: {position:{x:-22.081968307495117,y:80.0910710096359253,z:80.194319486618042},orientation:{x:-0.10836052149534225,y:-0.0045970650389790535,z:-0.99320769309997559,w:1.042135711759328842}}'
sleep 2
gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean --timeout 500 --req 'pose: {position:{x: 31.2, y:-6.7215,z:7.2255}, orientation:{x:-0.1466788, y:0.0344671, z:0.9623708, w:0.2261413}}'
sleep 2
gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean --timeout 500 --req 'pose: {position:{x: 27.2, y:-1.7215,z:2.2255}, orientation:{x:-23.1466788, y:-23.0344671, z:23.9623708, w:3.2261413}}'
sleep 2
gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean --timeout 500 --req 'pose: {position:{x: 27.2, y:-1.7215,z:2.2255}, orientation:{x:-21.1466788, y:-21.0344671, z:21.9623708, w:1.2261413}}'
sleep 2
gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean --timeout 500 --req 'pose: {position:{x:0.5,y:1.0,z:1.0},orientation:{x:0.15,y:-0.05,z:-1.0,w:-0.05}}'
sleep 2
gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean --timeout 500 --req 'pose: {position:{x:1.5,y:1.0,z:1.0},orientation:{x:0.15,y:-0.05,z:-1.0,w:-1.05}}'
Loading