Fixed logic

gazebosim · Feb 7, 2024 · 138a25c · 138a25c
1 parent a10e7ca
commit 138a25c
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 8 deletions.
diff --git a/src/RotationSpline.cc b/src/RotationSpline.cc
@@ -21,6 +21,7 @@
 using namespace gz;
 using namespace math;
 
+using namespace std;
 /// \internal
 /// \brief Private data for RotationSpline
 class RotationSpline::Implementation
@@ -92,17 +93,21 @@ Quaterniond RotationSpline::Interpolate(const unsigned int _fromIndex,
   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());
+  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
-  if ((dist2>64) || (diffw>1.57))
+  if ((diffX<0.16) || (diffY<0.16) || (diffZ<0.16))
   {
-    return Quaterniond::Squad(_t, p, a, b, q, _useShortestPath);
+    return Quaterniond::Slerp(_t, p, q, _useShortestPath);
   }
   else 
   {
-    return Quaterniond::Slerp(_t, p, q, _useShortestPath);
+    return Quaterniond::Squad(_t, p, a, b, q, _useShortestPath);
   }
 }
 

diff --git a/test/interpolation.sh b/test/interpolation.sh
@@ -5,10 +5,10 @@ gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Bo
 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:-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}}'