Refactor and fix forward kinematics

robotology · May 10, 2024 · 50a0296 · 50a0296
1 parent 28aefae
commit 50a0296
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 18 deletions.
diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp
@@ -8,6 +8,7 @@
 #include <yarp/os/Property.h>
 #include <yarp/os/Stamp.h>
 #include <yarp/os/ResourceFinder.h>
+#include <yarp/os/LogStream.h>
 
 #include <robometry/BufferConfig.h>
 #include <robometry/BufferManager.h>
@@ -99,7 +100,7 @@ bool Imu::setup(yarp::os::Property& property)
     if(inputSensorsList->size() == 0 || inputSensorsList->get(0).asString() == "all")
     {
         ROBOTTESTINGFRAMEWORK_TEST_REPORT("Testing all the IMUs available...");
-
+        std::string sensorName{""};
         yarp::dev::IOrientationSensors* ior;
         ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver.view(ior), "Unable to open the orientation interface");
         for(int sensorIndex = 0; sensorIndex < ior->getNrOfOrientationSensors(); sensorIndex++)
@@ -130,9 +131,6 @@ bool Imu::setup(yarp::os::Property& property)
     driverList.push(&MASclientDriver, "alljoints_inertials");
     ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(imultiwrap->attachAll(driverList), "Unable to do the attach");
     ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASremapperDriver.view(iorientation), "Unable to open orientation interface");
-
-    outputPort.open("/test-imu/out");
-    ROBOTTESTINGFRAMEWORK_TEST_REPORT("Opening port "+outputPort.getName()+"...");
 
     iDynTree::Vector3 baseLinkOrientationRad;
     baseLinkOrientationRad.zero();
@@ -180,6 +178,7 @@ void Imu::tearDown()
 
     controlBoardDriver.close();
     MASclientDriver.close();
+    MASremapperDriver.close();
 
     for(int i = 0; i < localBroker.size(); i++)
     {
@@ -193,14 +192,18 @@ void Imu::run()
 {
     ROBOTTESTINGFRAMEWORK_TEST_REPORT("Starting reading IMU orientation values...");
     rpyValues.resize(sensorsList.get(0).asList()->size());
+    I_R_I_IMU.resize(sensorsList.get(0).asList()->size());
 
     for (int sensorIndex = 0; sensorIndex < sensorsList.get(0).asList()->size(); sensorIndex++)
     {
-        ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(iorientation->getOrientationSensorName(sensorIndex, sensorName), "Unable to obtain rpy measurements.");
+        std::string sensorName{""};
+        std::string frameName{""};
+        double timestamp;
+        ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(iorientation->getOrientationSensorName(sensorIndex, sensorName), "Unable to obtain sensor name.");
         ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(iorientation->getOrientationSensorMeasureAsRollPitchYaw(sensorIndex, rpyValues[sensorIndex], timestamp), "Unable to obtain rpy measurements.");
-        iorientation->getOrientationSensorFrameName(sensorIndex, frameName);
+        ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(iorientation->getOrientationSensorFrameName(sensorIndex, frameName), "Unable to obtain frame name.");
         iDynTree::Rotation I_R_FK = kinDynComp.getWorldTransform(frameName).getRotation(); 
-        I_R_I_IMU = (I_R_FK * ((iDynTree::Rotation::RPY(iDynTree::deg2rad(rpyValues[sensorIndex][0]), iDynTree::deg2rad(rpyValues[sensorIndex][1]), iDynTree::deg2rad(rpyValues[sensorIndex][2]))).inverse()));    
+        I_R_I_IMU[sensorIndex] = (I_R_FK * ((iDynTree::Rotation::RPY(iDynTree::deg2rad(rpyValues[sensorIndex][0]), iDynTree::deg2rad(rpyValues[sensorIndex][1]), iDynTree::deg2rad(rpyValues[sensorIndex][2]))).inverse()));    
     }
 
     setupBrokers();
@@ -219,9 +222,13 @@ bool Imu::startMove()
     {
         for (int sensorIndex = 0; sensorIndex < sensorsList.get(0).asList()->size(); sensorIndex++)
         {
-            sensorName = sensorsList.get(0).asList()->get(sensorIndex).asString();
-            iorientation->getOrientationSensorMeasureAsRollPitchYaw(sensorIndex, rpyValues[sensorIndex], timestamp);
-            iorientation->getOrientationSensorFrameName(sensorIndex, frameName);
+            std::string frameName{""};
+            std::string sensorName{""};
+
+            double timestamp;
+            ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(iorientation->getOrientationSensorName(sensorIndex, sensorName), "Unable to obtain sensor name.");
+            ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(iorientation->getOrientationSensorMeasureAsRollPitchYaw(sensorIndex, rpyValues[sensorIndex], timestamp), "Unable to obtain rpy measurements.");
+            ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(iorientation->getOrientationSensorFrameName(sensorIndex, frameName), "Unable to obtain frame name.");
 
             ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ienc->getEncoders(positions.data()), "Cannot get joint positions");
             ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ienc->getEncoderSpeeds(velocities.data()), "Cannot get joint velocities");
@@ -240,7 +247,7 @@ bool Imu::startMove()
             gravity);
 
             iDynTree::Rotation expectedImuSignal = kinDynComp.getWorldTransform(frameName).getRotation();
-            iDynTree::Rotation imuSignal = (I_R_I_IMU * iDynTree::Rotation::RPY(iDynTree::deg2rad(rpyValues[sensorIndex][0]), iDynTree::deg2rad(rpyValues[sensorIndex][1]), iDynTree::deg2rad(rpyValues[sensorIndex][2]))); 
+            iDynTree::Rotation imuSignal = (I_R_I_IMU[sensorIndex] * iDynTree::Rotation::RPY(iDynTree::deg2rad(rpyValues[sensorIndex][0]), iDynTree::deg2rad(rpyValues[sensorIndex][1]), iDynTree::deg2rad(rpyValues[sensorIndex][2]))); 
             error = (expectedImuSignal * imuSignal.inverse()).log();
 
             bufferManager.push_back(positions, "joints_state::positions");
@@ -264,7 +271,7 @@ bool Imu::startMove()
 
     for(int sensorIndex = 0; sensorIndex < sensorsList.get(0).asList()->size(); sensorIndex++)
     {
-        sensorName = sensorsList.get(0).asList()->get(sensorIndex).asString();
+        auto sensorName = sensorsList.get(0).asList()->get(sensorIndex).asString();
         auto maxError = std::max_element(errorTot[sensorIndex].begin(), errorTot[sensorIndex].end());
         ROBOTTESTINGFRAMEWORK_TEST_CHECK(*maxError < errorMax, Asserter::format("Testing sensor %s: the max rotation angle error is %f rad!", sensorName.c_str(), *maxError));
     }

diff --git a/src/imu/imu.h b/src/imu/imu.h
@@ -48,8 +48,6 @@ class Imu : public yarp::robottestingframework::TestCase {
         std::string robotName;
         std::string portName;
         std::string modelName;
-        std::string frameName;
-        std::string sensorName;
         double errorMax;
         yarp::os::Bottle sensorsList;
         std::vector<std::string> partsList;
@@ -69,7 +67,6 @@ class Imu : public yarp::robottestingframework::TestCase {
         yarp::sig::Vector velocities;
 
         int axes;
-        double timestamp;
         std::vector<std::string> axesVec;
 
         iDynTree::ModelLoader model;
@@ -80,7 +77,7 @@ class Imu : public yarp::robottestingframework::TestCase {
         iDynTree::Rotation baseLinkOrientation;
         iDynTree::Twist baseVelocity;
         iDynTree::Transform I_T_base;
-        iDynTree::Rotation I_R_I_IMU;
+        std::vector<iDynTree::Rotation> I_R_I_IMU;
 
         robometry::BufferManager bufferManager;
 

diff --git a/suites/contexts/icub/test_imu.ini b/suites/contexts/icub/test_imu.ini
@@ -1,7 +1,7 @@
 robot                   ${robotname}
 model                   model.urdf
 port                    /${robotname}/alljoints/inertials
-remoteControlBoards     ("torso", "head", "left_arm", "right_arm")
-axesNames               ("torso_pitch", "torso_roll", "torso_yaw", "neck_pitch", "neck_roll", "neck_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw")
+remoteControlBoards     ("torso", "head", "left_arm", "right_arm", "left_leg", "right_leg")
+axesNames               ("torso_pitch", "torso_roll", "torso_yaw", "neck_pitch", "neck_roll", "neck_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
 sensorsList             ("all")
 maxError                0.1
diff --git a/suites/contexts/icub/test_imu_no_legs.ini b/suites/contexts/icub/test_imu_no_legs.ini
@@ -0,0 +1,7 @@
+robot                   ${robotname}
+model                   model.urdf
+port                    /${robotname}/alljoints/inertials
+remoteControlBoards     ("torso", "head", "left_arm", "right_arm")
+axesNames               ("torso_pitch", "torso_roll", "torso_yaw", "neck_pitch", "neck_roll", "neck_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw")
+sensorsList             ("all")
+maxError                0.1