Added Conditions for Slerp vs Squad interpolation

Signed-off-by: sdhar04 <[email protected]>
gazebosim · Feb 7, 2024 · b6d7673 · b6d7673
1 parent ec655a9
commit b6d7673
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 2 deletions.
diff --git a/src/RotationSpline.cc b/src/RotationSpline.cc
@@ -16,10 +16,12 @@
 */
 #include "gz/math/Quaternion.hh"
 #include "gz/math/RotationSpline.hh"
+#include "cmath"
 
 using namespace gz;
 using namespace math;
 
+using namespace std;
 /// \internal
 /// \brief Private data for RotationSpline
 class RotationSpline::Implementation
@@ -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);
+  }
 }
 
 /////////////////////////////////////////////////

diff --git a/test/interpolation.sh b/test/interpolation.sh
@@ -0,0 +1,20 @@
+#!/bin/bash
+
+#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}}'