From 49919b614a7b1490e1facf7f38af8ba820fb9ccc Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Thu, 8 Feb 2024 09:46:46 +0100 Subject: [PATCH 1/3] Improvements in computing error rotation angle --- src/imu/imu.cpp | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index 51155ce..efbfe7e 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include @@ -200,6 +201,7 @@ bool Imu::moveJoint(int ax, double pos) bool done = false; int count = 0; iDynTree::GeomVector3 error; + std::vector errorTot; yarp::os::Time::delay(.1); ipos->positionMove(ax, pos); @@ -226,17 +228,27 @@ bool Imu::moveJoint(int ax, double pos) iDynTree::Rotation expectedImuSignal = kinDynComp.getWorldTransform(frameName).getRotation(); iDynTree::Rotation imuSignal = (I_R_I_IMU * iDynTree::Rotation::RPY(iDynTree::deg2rad(rpyValues[0]), iDynTree::deg2rad(rpyValues[1]), iDynTree::deg2rad(rpyValues[2]))); + error = (expectedImuSignal * imuSignal.inverse()).log(); - error = error + (expectedImuSignal * imuSignal.inverse()).log(); - count++; + double p = 0.0; + double mag = 0.0; + + for (auto i : error) + { + p += pow(i, 2); + mag = sqrt(p); + errorTot.push_back(mag); + } sendData(expectedImuSignal.asRPY(), imuSignal.asRPY()); ipos->checkMotionDone(&done); } - double err_mean = fabs((std::accumulate(error.begin(), error.end(), 0.0)) / count); - ROBOTTESTINGFRAMEWORK_TEST_CHECK(err_mean < errorMean, Asserter::format("The error mean on axis %s is %f rad!", axesVec[ax].c_str(), err_mean)); + iDynTree::Rotation expectedImuSignal = kinDynComp.getWorldTransform(frameName).getRotation(); + iDynTree::Rotation imuSignal = (I_R_I_IMU * iDynTree::Rotation::RPY(iDynTree::deg2rad(rpyValues[0]), iDynTree::deg2rad(rpyValues[1]), iDynTree::deg2rad(rpyValues[2]))); + + ROBOTTESTINGFRAMEWORK_TEST_CHECK(*max_element(errorTot.begin(), errorTot.end()) < errorMean, Asserter::format("Testing %s: the max error of the rotation angle is %f rad!", axesVec[ax].c_str(), *max_element(errorTot.begin(), errorTot.end()))); ipos->positionMove(ax, 0.0); - + return true; } \ No newline at end of file From a8adb1ece5fc3ab43340382226042ae455196a10 Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Fri, 9 Feb 2024 10:17:58 +0100 Subject: [PATCH 2/3] Compute the max rotation angle error --- src/imu/imu.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index efbfe7e..1956796 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -1,7 +1,6 @@ #include #include #include -#include #include #include @@ -199,7 +198,6 @@ bool Imu::sendData(iDynTree::Vector3 expectedValues, iDynTree::Vector3 imuSignal bool Imu::moveJoint(int ax, double pos) { bool done = false; - int count = 0; iDynTree::GeomVector3 error; std::vector errorTot; yarp::os::Time::delay(.1); @@ -230,24 +228,23 @@ bool Imu::moveJoint(int ax, double pos) iDynTree::Rotation imuSignal = (I_R_I_IMU * iDynTree::Rotation::RPY(iDynTree::deg2rad(rpyValues[0]), iDynTree::deg2rad(rpyValues[1]), iDynTree::deg2rad(rpyValues[2]))); error = (expectedImuSignal * imuSignal.inverse()).log(); - double p = 0.0; - double mag = 0.0; + errorTot.reserve(error.size()); - for (auto i : error) - { - p += pow(i, 2); - mag = sqrt(p); - errorTot.push_back(mag); - } + double sumOfSquares = std::accumulate(error.begin(), error.end(), 0.0, + [](double accumulator, double element) { + return accumulator + element * element; + }); + + double rms = std::sqrt(sumOfSquares); + errorTot.push_back(rms); sendData(expectedImuSignal.asRPY(), imuSignal.asRPY()); ipos->checkMotionDone(&done); } - iDynTree::Rotation expectedImuSignal = kinDynComp.getWorldTransform(frameName).getRotation(); - iDynTree::Rotation imuSignal = (I_R_I_IMU * iDynTree::Rotation::RPY(iDynTree::deg2rad(rpyValues[0]), iDynTree::deg2rad(rpyValues[1]), iDynTree::deg2rad(rpyValues[2]))); + double maxError = *max_element(errorTot.begin(), errorTot.end()); - ROBOTTESTINGFRAMEWORK_TEST_CHECK(*max_element(errorTot.begin(), errorTot.end()) < errorMean, Asserter::format("Testing %s: the max error of the rotation angle is %f rad!", axesVec[ax].c_str(), *max_element(errorTot.begin(), errorTot.end()))); + ROBOTTESTINGFRAMEWORK_TEST_CHECK(maxError < errorMean, Asserter::format("Testing %s: the max rotation angle error is %f rad!", axesVec[ax].c_str(), maxError)); ipos->positionMove(ax, 0.0); return true; From 6efb5157cad3767c142ad6552e9eaebea1f7a020 Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Fri, 9 Feb 2024 10:19:28 +0100 Subject: [PATCH 3/3] Fix variable name --- src/imu/imu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index 1956796..be513fb 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -235,8 +235,8 @@ bool Imu::moveJoint(int ax, double pos) return accumulator + element * element; }); - double rms = std::sqrt(sumOfSquares); - errorTot.push_back(rms); + double mag = std::sqrt(sumOfSquares); + errorTot.push_back(mag); sendData(expectedImuSignal.asRPY(), imuSignal.asRPY()); ipos->checkMotionDone(&done);