Created thresholds for Squad and Slerp interpolation

Signed-off-by: sdhar04 <[email protected]>
gazebosim · Feb 6, 2024 · 3c4c086 · 3c4c086
1 parent af4bb2c
commit 3c4c086
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 4 deletions.
diff --git a/src/RotationSpline.cc b/src/RotationSpline.cc
@@ -16,6 +16,7 @@
 */
 #include "gz/math/Quaternion.hh"
 #include "gz/math/RotationSpline.hh"
+#include "cmath"
 
 using namespace gz;
 using namespace math;
@@ -88,11 +89,21 @@ Quaterniond RotationSpline::Interpolate(const unsigned int _fromIndex,
   // Use squad using tangents we've already set up
   Quaterniond &p = this->dataPtr->points[_fromIndex];
   Quaterniond &q = this->dataPtr->points[_fromIndex+1];
-  //Quaterniond &a = this->dataPtr->tangents[_fromIndex];
-  //Quaterniond &b = this->dataPtr->tangents[_fromIndex+1];
-
+  Quaterniond &a = this->dataPtr->tangents[_fromIndex];
+  Quaterniond &b = this->dataPtr->tangents[_fromIndex+1];
+
+  //See difference in distance and orientation 
+  double dist2 = pow(p.X()-q.X(),2)+pow(p.Y()-q.Y(),2)+pow(p.Z()-q.Z(),2);
+  double diffw = abs(p.W()-q.W());
   // NB interpolate to nearest rotation
-  return Quaterniond::Slerp(_t, p, q, _useShortestPath);
+  if ((dist2>64) || (diffw>1.57))
+  {
+    return Quaterniond::Squad(_t, p, a, b, q, _useShortestPath);
+  }
+  else 
+  {
+    return Quaterniond::Slerp(_t, p, 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:22.081968307495117,y:1.0910710096359253,z:1.194319486618042},orientation:{x:-1.10836052149534225,y:1.0045970650389790535,z:1.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: 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: 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}}'