From 50a02967c45d8654d85371c81d14ca04eca7272e Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Fri, 10 May 2024 15:00:47 +0200 Subject: [PATCH 1/2] Refactor and fix forward kinematics --- src/imu/imu.cpp | 31 ++++++++++++++--------- src/imu/imu.h | 5 +--- suites/contexts/icub/test_imu.ini | 4 +-- suites/contexts/icub/test_imu_no_legs.ini | 7 +++++ 4 files changed, 29 insertions(+), 18 deletions(-) create mode 100644 suites/contexts/icub/test_imu_no_legs.ini diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index d4758f4..ff3f760 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -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 index 912668b..8c61473 100644 --- 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 partsList; @@ -69,7 +67,6 @@ class Imu : public yarp::robottestingframework::TestCase { yarp::sig::Vector velocities; int axes; - double timestamp; std::vector 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 I_R_I_IMU; robometry::BufferManager bufferManager; diff --git a/suites/contexts/icub/test_imu.ini b/suites/contexts/icub/test_imu.ini index fe00d7d..914686a 100644 --- 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 new file mode 100644 index 0000000..fe00d7d --- /dev/null +++ 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 From a0ac21c8e42a6268076caa29d0fef2f3288438f4 Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Fri, 10 May 2024 15:15:08 +0200 Subject: [PATCH 2/2] Delete plot.xml leftover --- src/imu/plot.xml | 66 ------------------------------------------------ 1 file changed, 66 deletions(-) delete mode 100644 src/imu/plot.xml diff --git a/src/imu/plot.xml b/src/imu/plot.xml deleted file mode 100644 index 1cc3411..0000000 --- a/src/imu/plot.xml +++ /dev/null @@ -1,66 +0,0 @@ - - - - - - - - - - - - - - -