From b6d7673c2f5cdc2897a0ad4270637ac697db71eb Mon Sep 17 00:00:00 2001 From: sdhar04 Date: Thu, 8 Feb 2024 04:42:43 +0530 Subject: [PATCH 1/4] Added Conditions for Slerp vs Squad interpolation Signed-off-by: sdhar04 --- src/RotationSpline.cc | 20 ++++++++++++++++++-- test/interpolation.sh | 20 ++++++++++++++++++++ 2 files changed, 38 insertions(+), 2 deletions(-) create mode 100755 test/interpolation.sh diff --git a/src/RotationSpline.cc b/src/RotationSpline.cc index 69929e618..b48c8bb04 100644 --- 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 new file mode 100755 index 000000000..92863da68 --- /dev/null +++ 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}}' From 9d504f305f744ab05297c931a17ff7c83977d2f6 Mon Sep 17 00:00:00 2001 From: sdhar04 Date: Sun, 10 Mar 2024 22:29:52 +0530 Subject: [PATCH 2/4] Removed bash script Signed-off-by: sdhar04 --- test/interpolation.sh | 20 -------------------- 1 file changed, 20 deletions(-) delete mode 100755 test/interpolation.sh diff --git a/test/interpolation.sh b/test/interpolation.sh deleted file mode 100755 index 92863da68..000000000 --- a/test/interpolation.sh +++ /dev/null @@ -1,20 +0,0 @@ -#!/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}}' From 039a64a6e3bca55cce4c35aaeb81a9afdc7c762d Mon Sep 17 00:00:00 2001 From: sdhar04 Date: Fri, 29 Mar 2024 01:49:35 +0530 Subject: [PATCH 3/4] Addressed lint issues Signed-off-by: sdhar04 --- src/RotationSpline.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/RotationSpline.cc b/src/RotationSpline.cc index b48c8bb04..e9eb8bafc 100644 --- a/src/RotationSpline.cc +++ b/src/RotationSpline.cc @@ -92,20 +92,20 @@ 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 - if ((diffX<0.16) || (diffY<0.16) || (diffZ<0.16)) + if ((diffX < 0.16) || (diffY < 0.16) || (diffZ < 0.16)) { return Quaterniond::Slerp(_t, p, q, _useShortestPath); } - else + else { return Quaterniond::Squad(_t, p, a, b, q, _useShortestPath); } From cf8b74c47ba983ca6d55eed5f757a8ac0c94c374 Mon Sep 17 00:00:00 2001 From: sdhar04 Date: Fri, 29 Mar 2024 03:50:20 +0530 Subject: [PATCH 4/4] Reduced scope of variables required only for squad interpolation Signed-off-by: sdhar04 --- src/RotationSpline.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RotationSpline.cc b/src/RotationSpline.cc index e9eb8bafc..f1d4486f8 100644 --- a/src/RotationSpline.cc +++ b/src/RotationSpline.cc @@ -90,8 +90,6 @@ 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]; Vector3d peu(p.Euler()); Vector3d qeu(q.Euler()); @@ -107,6 +105,8 @@ Quaterniond RotationSpline::Interpolate(const unsigned int _fromIndex, } else { + Quaterniond &a = this->dataPtr->tangents[_fromIndex]; + Quaterniond &b = this->dataPtr->tangents[_fromIndex+1]; return Quaterniond::Squad(_t, p, a, b, q, _useShortestPath); } }