From b6d7673c2f5cdc2897a0ad4270637ac697db71eb Mon Sep 17 00:00:00 2001 From: sdhar04 Date: Thu, 8 Feb 2024 04:42:43 +0530 Subject: [PATCH 01/11] 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 69929e61..b48c8bb0 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 00000000..92863da6 --- /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 02/11] 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 92863da6..00000000 --- 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 03/11] 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 b48c8bb0..e9eb8baf 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 04/11] 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 e9eb8baf..f1d4486f 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); } } From 76c57b45d2eab342e6fbed6057e006e0fbdc1a53 Mon Sep 17 00:00:00 2001 From: Sammit Dhar Date: Fri, 10 Jan 2025 11:08:24 +0530 Subject: [PATCH 05/11] Update RotationSpline.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Sammit Dhar --- src/RotationSpline.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/RotationSpline.cc b/src/RotationSpline.cc index f1d4486f..fb46dda7 100644 --- a/src/RotationSpline.cc +++ b/src/RotationSpline.cc @@ -14,9 +14,10 @@ * limitations under the License. * */ +#include + #include "gz/math/Quaternion.hh" #include "gz/math/RotationSpline.hh" -#include "cmath" using namespace gz; using namespace math; From 79fda43a35e6f66460a40decd337ce469dc51328 Mon Sep 17 00:00:00 2001 From: Sammit Dhar Date: Thu, 31 Jul 2025 04:15:05 +0530 Subject: [PATCH 06/11] Update RotationSpline.cc According to review Signed-off-by: Sammit Dhar --- src/RotationSpline.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/RotationSpline.cc b/src/RotationSpline.cc index fb46dda7..0a6642cf 100644 --- a/src/RotationSpline.cc +++ b/src/RotationSpline.cc @@ -22,7 +22,6 @@ using namespace gz; using namespace math; -using namespace std; /// \internal /// \brief Private data for RotationSpline class RotationSpline::Implementation From cc558a51fcfcb61a1fac0d5a6fc621985364b44b Mon Sep 17 00:00:00 2001 From: Sammit Dhar Date: Thu, 31 Jul 2025 04:15:59 +0530 Subject: [PATCH 07/11] Update RotationSpline_TEST.cc Test for Slerp Signed-off-by: Sammit Dhar --- src/RotationSpline_TEST.cc | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/RotationSpline_TEST.cc b/src/RotationSpline_TEST.cc index d01270b6..b7171012 100644 --- a/src/RotationSpline_TEST.cc +++ b/src/RotationSpline_TEST.cc @@ -65,6 +65,12 @@ TEST(RotationSplineTest, RotationSpline) math::Quaterniond(0.978787, 0.107618, 0.137159, 0.107618)); EXPECT_EQ(s.Interpolate(1, 0.0), s.Point(1)); EXPECT_EQ(s.Interpolate(1, 1.0), s.Point(2)); + + // ::Interpolate + s.Clear(); + s.AddPoint(math::Quaterniond(0.1, 0, 0)); + s.AddPoint(math::Quaterniond(0.2, 0, 0)); + EXPECT_EQ(s.Interpolate(0.5), math::Quaterniond(0.15, 0, 0)); } ///////////////////////////////////////////////// From 1399fd2fbbf813cda9fef543734a170e6d270c80 Mon Sep 17 00:00:00 2001 From: Sammit Dhar Date: Thu, 31 Jul 2025 05:29:16 +0530 Subject: [PATCH 08/11] Update RotationSpline_TEST.cc Correctly changed the checks where Slerp is now called Signed-off-by: Sammit Dhar --- src/RotationSpline_TEST.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RotationSpline_TEST.cc b/src/RotationSpline_TEST.cc index b7171012..67a68449 100644 --- a/src/RotationSpline_TEST.cc +++ b/src/RotationSpline_TEST.cc @@ -52,7 +52,7 @@ TEST(RotationSplineTest, RotationSpline) // ::Interpolate EXPECT_TRUE(s.Interpolate(0.5) == - math::Quaterniond(0.998089, 0.0315333, 0.0427683, 0.0315333)); + math::Quaterniond(0.999181, 0.0184352, 0.030948, 0.0184352)); // ::Interpolate s.AddPoint(math::Quaterniond(.4, .4, .4)); @@ -62,7 +62,7 @@ TEST(RotationSplineTest, RotationSpline) EXPECT_EQ(s.Interpolate(s.PointCount()-1, 0.2), s.Point(s.PointCount()-1)); EXPECT_TRUE(s.Interpolate(1, 0.2) == - math::Quaterniond(0.978787, 0.107618, 0.137159, 0.107618)); + math::Quaterniond(0.980579, 0.101786, 0.133208, 0.101786)); EXPECT_EQ(s.Interpolate(1, 0.0), s.Point(1)); EXPECT_EQ(s.Interpolate(1, 1.0), s.Point(2)); From d854a852d344abdd72909d401558289d8ac76c4a Mon Sep 17 00:00:00 2001 From: Sammit Dhar Date: Thu, 31 Jul 2025 06:09:08 +0530 Subject: [PATCH 09/11] Update RotationSpline_TEST.cc Changed one test as it was too brittle Signed-off-by: Sammit Dhar --- src/RotationSpline_TEST.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RotationSpline_TEST.cc b/src/RotationSpline_TEST.cc index 67a68449..953c629c 100644 --- a/src/RotationSpline_TEST.cc +++ b/src/RotationSpline_TEST.cc @@ -51,8 +51,8 @@ TEST(RotationSplineTest, RotationSpline) s.AutoCalculate(true); // ::Interpolate - EXPECT_TRUE(s.Interpolate(0.5) == - math::Quaterniond(0.999181, 0.0184352, 0.030948, 0.0184352)); + math::Quaterniond expected(0.999181, 0.0184352, 0.030948, 0.0184352); + EXPECT_TRUE(s.Interpolate(0.5).Equal(expected, 1e-6)); // ::Interpolate s.AddPoint(math::Quaterniond(.4, .4, .4)); From 28b4321d2a2faf3e7580c59285ce18a5cfcc165f Mon Sep 17 00:00:00 2001 From: sdhar04 Date: Sat, 23 Aug 2025 16:16:43 +0530 Subject: [PATCH 10/11] Changed according to review Signed-off-by: sdhar04 --- src/RotationSpline.cc | 10 +++------- src/RotationSpline_TEST.cc | 4 ++-- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/RotationSpline.cc b/src/RotationSpline.cc index 0a6642cf..f9d69ea5 100644 --- a/src/RotationSpline.cc +++ b/src/RotationSpline.cc @@ -91,15 +91,11 @@ Quaterniond RotationSpline::Interpolate(const unsigned int _fromIndex, Quaterniond &p = this->dataPtr->points[_fromIndex]; Quaterniond &q = this->dataPtr->points[_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()); + auto diffQ = p.Inverse() * q; + const double diff = 2 * acos(diffQ.W()); // NB interpolate to nearest rotation - if ((diffX < 0.16) || (diffY < 0.16) || (diffZ < 0.16)) + if (diff < 0.16) { return Quaterniond::Slerp(_t, p, q, _useShortestPath); } diff --git a/src/RotationSpline_TEST.cc b/src/RotationSpline_TEST.cc index 953c629c..5abe1137 100644 --- a/src/RotationSpline_TEST.cc +++ b/src/RotationSpline_TEST.cc @@ -51,8 +51,8 @@ TEST(RotationSplineTest, RotationSpline) s.AutoCalculate(true); // ::Interpolate - math::Quaterniond expected(0.999181, 0.0184352, 0.030948, 0.0184352); - EXPECT_TRUE(s.Interpolate(0.5).Equal(expected, 1e-6)); + EXPECT_TRUE(s.Interpolate(0.5) == + math::Quaterniond(0.998089, 0.0315333, 0.0427683, 0.0315333)); // ::Interpolate s.AddPoint(math::Quaterniond(.4, .4, .4)); From 0b0685f5b6a101ac95100047848f73f9c63d8a61 Mon Sep 17 00:00:00 2001 From: sdhar04 Date: Sat, 23 Aug 2025 16:34:44 +0530 Subject: [PATCH 11/11] Changes for review Signed-off-by: sdhar04 --- src/RotationSpline_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RotationSpline_TEST.cc b/src/RotationSpline_TEST.cc index 5abe1137..b7171012 100644 --- a/src/RotationSpline_TEST.cc +++ b/src/RotationSpline_TEST.cc @@ -62,7 +62,7 @@ TEST(RotationSplineTest, RotationSpline) EXPECT_EQ(s.Interpolate(s.PointCount()-1, 0.2), s.Point(s.PointCount()-1)); EXPECT_TRUE(s.Interpolate(1, 0.2) == - math::Quaterniond(0.980579, 0.101786, 0.133208, 0.101786)); + math::Quaterniond(0.978787, 0.107618, 0.137159, 0.107618)); EXPECT_EQ(s.Interpolate(1, 0.0), s.Point(1)); EXPECT_EQ(s.Interpolate(1, 1.0), s.Point(2));