From 5f84b945ed0a9d7023a729060f0c239f2721e848 Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Thu, 18 Jan 2024 16:45:52 +0100 Subject: [PATCH 1/4] Make remoteControlBoards and controlled joints parametrized lists --- src/imu/CMakeLists.txt | 4 - src/imu/imu.cpp | 117 ++++++++++++++++++++---------- src/imu/imu.h | 3 + suites/contexts/icub/test_imu.ini | 13 ++-- 4 files changed, 90 insertions(+), 47 deletions(-) diff --git a/src/imu/CMakeLists.txt b/src/imu/CMakeLists.txt index 6f6f394..9968f12 100644 --- a/src/imu/CMakeLists.txt +++ b/src/imu/CMakeLists.txt @@ -1,7 +1,3 @@ -if(NOT DEFINED CMAKE_MINIMUM_REQUIRED_VERSION) - cmake_minimum_required(VERSION 3.5) -endif() - add_definitions(-D_USE_MATH_DEFINES) robottestingframework_add_plugin(imu HEADERS imu.h diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index 3cc3140..f7b341c 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -18,18 +18,23 @@ Imu::Imu() : TestCase("Imu") { } Imu::~Imu() { } -bool Imu::setup(yarp::os::Property& property) { - +bool Imu::setup(yarp::os::Property& property) +{ ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!"); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("port"), "The port name must be given as the test parameter!"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("remote_controlboards"), "Please, provide the controlboards name."); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("model"), "Please, provide the urdf model path."); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("frame"), "Please, provide the frame name."); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("sensor"), "Please, provide the sensor name."); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("mean_error"), "Please, provide the threshold error."); robotName = property.find("robot").asString(); // robot name portName = property.find("port").asString(); // name of the port from which the data are streamed + partName = property.find("part").asString(); // name of the part of the robot on which the sensor is mounted frameName = property.find("frame").asString(); // frame on which the sensor is attached sensorName = property.find("sensor").asString(); // sensor name within urdf + errorMean = property.find("mean_error").asFloat64(); //error mean modelName = property.find("model").asString(); // urdf model path yarp::os::ResourceFinder &rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); std::string modelAbsolutePath = rf.findFileByName(modelName); @@ -45,34 +50,53 @@ bool Imu::setup(yarp::os::Property& property) { yarp::os::Property controlBoardOptions; controlBoardOptions.put("device", "remotecontrolboardremapper"); + + yarp::os::Bottle remoteControlBoards; + yarp::os::Bottle & remoteControlBoardsList = remoteControlBoards.addList(); + yarp::os::Bottle *inputControlBoards; + inputControlBoards = property.find("remote_controlboards").asList(); + for (int i = 0; i < inputControlBoards->size(); i++) + { + remoteControlBoardsList.addString("/"+robotName+"/"+inputControlBoards->get(i).asString()); + std::cout << inputControlBoards->get(i).asString() << std::endl; + } + yarp::os::Bottle axesNames; yarp::os::Bottle & axesList = axesNames.addList(); - axesList.addString("neck_pitch"); - axesList.addString("neck_roll"); - axesList.addString("neck_yaw"); - axesList.addString("torso_pitch"); - axesList.addString("torso_roll"); - axesList.addString("torso_yaw"); - axesList.addString("l_shoulder_pitch"); - axesList.addString("l_shoulder_roll"); - axesList.addString("l_shoulder_yaw"); - axesList.addString("l_elbow"); - axesList.addString("r_shoulder_pitch"); - axesList.addString("r_shoulder_roll"); - axesList.addString("r_shoulder_yaw"); - axesList.addString("r_elbow"); - controlBoardOptions.put("axesNames", axesNames.get(0)); + yarp::dev::PolyDriver tmpDriver; + yarp::dev::IEncoders* tmpEncoders; + yarp::dev::IAxisInfo* tmpAxis; + + yarp::os::Property tmpOptions; + + int tmpAxisNum; + std::string tmpString; + + tmpOptions.put("device", "remote_controlboard"); + tmpOptions.put("local", "/tmp"); + + for(int rcb = 0; rcb < remoteControlBoardsList.size(); rcb++) + { + tmpOptions.put("remote", remoteControlBoardsList.get(rcb)); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpDriver.open(tmpOptions), "Unable to open the tmp driver"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpDriver.isValid(), "Device driver cannot be opened"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpDriver.view(tmpEncoders), "Unable to open encoder interface"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpDriver.view(tmpAxis), "Unable to open axes interface"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpEncoders->getAxes(&tmpAxisNum), "Cannot get the name of controlled axes"); + + for(int i = 0; i < tmpAxisNum; i++) + { + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpAxis->getAxisName(i, tmpString), "Cannot get name of controlled axes"); + axesList.addString(tmpString); + } + + tmpDriver.close(); + } - yarp::os::Bottle remoteControlBoards; - yarp::os::Bottle & remoteControlBoardsList = remoteControlBoards.addList(); - remoteControlBoardsList.addString("/"+robotName+"/torso"); - remoteControlBoardsList.addString("/"+robotName+"/head"); - remoteControlBoardsList.addString("/"+robotName+"/left_arm"); - remoteControlBoardsList.addString("/"+robotName+"/right_arm"); controlBoardOptions.put("remoteControlBoards", remoteControlBoards.get(0)); controlBoardOptions.put("localPortPrefix", "/test"); - + controlBoardOptions.put("axesNames", axesNames.get(0)); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.open(controlBoardOptions), "Unable to open the controlBoard driver"); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver.isValid(), "Device driver cannot be opened"); @@ -88,7 +112,6 @@ bool Imu::setup(yarp::os::Property& property) { ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ienc->getAxes(&axes), "Cannot get number of controlled axes"); std::string axisName; - std::vector axis; for (int i = 0; i < axes; i++) { @@ -113,7 +136,7 @@ bool Imu::setup(yarp::os::Property& property) { positions.resize(axes); velocities.resize(axes); 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 positions"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ienc->getEncoderSpeeds(velocities.data()), "Cannot get joint velocities"); for(auto i = 0; i < axes; i++) { @@ -135,7 +158,8 @@ bool Imu::setup(yarp::os::Property& property) { return true; } -void Imu::tearDown() { +void Imu::tearDown() +{ outputPort.interrupt(); outputPort.close(); @@ -143,7 +167,8 @@ void Imu::tearDown() { MASclientDriver.close(); } -void Imu::run() { +void Imu::run() +{ ROBOTTESTINGFRAMEWORK_TEST_REPORT("Starting reading IMU orientation values..."); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(iorientation->getOrientationSensorMeasureAsRollPitchYaw(0, rpyValues, timestamp), "Unable to obtain rpy measurements."); @@ -153,15 +178,35 @@ void Imu::run() { double minLim; double maxLim; - for (int i = kinDynComp.model().getJointIndex("neck_pitch"); i <= kinDynComp.model().getJointIndex("neck_yaw"); i++) + yarp::dev::PolyDriver tmpDriver; + yarp::dev::IEncoders* tmpEncoders; + yarp::dev::IAxisInfo* tmpAxis; + yarp::os::Property tmpOptions; + + int tmpAxisNum; + std::string tmpString; + + tmpOptions.put("device", "remote_controlboard"); + tmpOptions.put("local", "/tmp"); + tmpOptions.put("remote", "/"+robotName+"/"+partName); + + tmpDriver.open(tmpOptions); + tmpDriver.view(tmpEncoders); + tmpDriver.view(tmpAxis); + tmpEncoders->getAxes(&tmpAxisNum); + + for (int i = 0; i < tmpAxisNum; i++) { - ilim->getLimits(i, &minLim, &maxLim); + tmpAxis->getAxisName(i, tmpString); + ilim->getLimits(model.model().getJointIndex(tmpString), &minLim, &maxLim); - moveJoint(i, minLim + 5); + moveJoint(model.model().getJointIndex(tmpString), minLim + 5); yarp::os::Time::delay(1.); - moveJoint(i, maxLim - 5); + moveJoint(model.model().getJointIndex(tmpString), maxLim - 5); yarp::os::Time::delay(1.); } + + tmpDriver.close(); } bool Imu::sendData(iDynTree::Vector3 expectedValues, iDynTree::Vector3 imuSignal) @@ -191,19 +236,16 @@ bool Imu::sendData(iDynTree::Vector3 expectedValues, iDynTree::Vector3 imuSignal bool Imu::moveJoint(int ax, double pos) { bool done = false; - double refPos; - yarp::os::Time::delay(.1); ipos->positionMove(ax, pos); - ipos->getTargetPosition(ax, &refPos); while(!done) { iorientation->getOrientationSensorMeasureAsRollPitchYaw(0, rpyValues, timestamp); 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 positions"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ienc->getEncoderSpeeds(velocities.data()), "Cannot get joint velocities"); for (auto i = 0; i < axes; i++) { @@ -223,11 +265,10 @@ bool Imu::moveJoint(int ax, double pos) auto error = (expectedImuSignal * imuSignal.inverse()).log(); double err_mean = (std::accumulate(error.begin(), error.end(), 0)) / error.size(); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(err_mean < 0.1, "Error > 0.1! Aborting ..."); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(err_mean < errorMean, "Error > 0.1! Aborting ..."); sendData(expectedImuSignal.asRPY(), imuSignal.asRPY()); ipos->checkMotionDone(&done); - } ipos->positionMove(ax, 0.0); diff --git a/src/imu/imu.h b/src/imu/imu.h index e1d98c7..cab8334 100644 --- a/src/imu/imu.h +++ b/src/imu/imu.h @@ -24,9 +24,11 @@ class Imu : public yarp::robottestingframework::TestCase { private: std::string robotName; std::string portName; + std::string partName; std::string modelName; std::string frameName; std::string sensorName; + double errorMean; yarp::dev::PolyDriver MASclientDriver; yarp::dev::PolyDriver controlBoardDriver; @@ -43,6 +45,7 @@ class Imu : public yarp::robottestingframework::TestCase { int axes; double timestamp; + std::vector axis; iDynTree::ModelLoader model; iDynTree::KinDynComputations kinDynComp; diff --git a/suites/contexts/icub/test_imu.ini b/suites/contexts/icub/test_imu.ini index f21d60a..052e023 100644 --- a/suites/contexts/icub/test_imu.ini +++ b/suites/contexts/icub/test_imu.ini @@ -1,5 +1,8 @@ -robot ${robotname} -model model.urdf -port /${robotname}/head/inertials -sensor head_imu_0 -frame head_imu_0 +robot ${robotname} +model model.urdf +port /${robotname}/head/inertials +part head +remote_controlboards ("torso", "head") +sensor head_imu_0 +frame head_imu_0 +mean_error 0.1 \ No newline at end of file From ad31d2eb0f0c0b6bda4cd57feb47bf31c0631dc7 Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Wed, 24 Jan 2024 14:41:52 +0100 Subject: [PATCH 2/4] Add doxygen documentation --- src/imu/imu.cpp | 4 ++-- src/imu/imu.h | 29 ++++++++++++++++++++++++++++- suites/contexts/icub/test_imu.ini | 2 +- 3 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index f7b341c..1205d46 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -23,7 +23,7 @@ bool Imu::setup(yarp::os::Property& property) ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!"); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("port"), "The port name must be given as the test parameter!"); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("remote_controlboards"), "Please, provide the controlboards name."); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("controlboards"), "Please, provide the controlboards name."); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("model"), "Please, provide the urdf model path."); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("frame"), "Please, provide the frame name."); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("sensor"), "Please, provide the sensor name."); @@ -54,7 +54,7 @@ bool Imu::setup(yarp::os::Property& property) yarp::os::Bottle remoteControlBoards; yarp::os::Bottle & remoteControlBoardsList = remoteControlBoards.addList(); yarp::os::Bottle *inputControlBoards; - inputControlBoards = property.find("remote_controlboards").asList(); + inputControlBoards = property.find("controlboards").asList(); for (int i = 0; i < inputControlBoards->size(); i++) { remoteControlBoardsList.addString("/"+robotName+"/"+inputControlBoards->get(i).asString()); diff --git a/src/imu/imu.h b/src/imu/imu.h index cab8334..5f7eb2a 100644 --- a/src/imu/imu.h +++ b/src/imu/imu.h @@ -13,6 +13,34 @@ #include #include +/** +* \ingroup icub-tests +* +* The purpose of this test is to evaluate the accuracy of the IMU Euler angles measurements. +* It takes as input the urdf of the robot and make a comparison between the expected values retrieved from the forward kinematics and the ones read from the IMU itself. +* The test involves the movements of the joints belonging to the part on which the sensor is mounted. The movements are executed sequentially, traversing from the home position to the lower limit, upper limit and back to the home position for each joint. +* +* Example: robottestingframework-testrunner --test plugins/imu.so --param "--robot icub --model model.urdf --port /icub/head/inertials --part head --controlboards ("torso", "head") --sensor head_imu_0 --frame head_imu_0 --mean_error 0.1" +* +* Moreover, you can manually modify the suites/contexts/icub/test_imu.ini file depending on the parameters of the test. In this case, after compiling, you can run: +* +* robottestingframework-testrunner --suite ../suites/imu.xml +* +* This will launch the test and open a yarpscope with the plots of the IMU traces. +* +* Accepts the following parameters: +* | Parameter name | Type | Units | Default Value | Required | Description | Notes | +* |:------------------:|:------------------:|:-----:|:-------------:|:--------:|:-----------:|:-----:| +* | robot | string | - | - | Yes | The name of the robot. | e.g. icub | +* | model | string | - | - | Yes | The name of the robot model. | e.g. model.urdf | +* | port | string | - | - | Yes | The name of the port streaming IMU data. | e.g. /icub/head/inertials | +* | part | string | - | - | Yes | The name of the robot part on which the sensor is mounted. | e.g. head | +* | controlboards | vector of string | - | - | Yes | The list of the controlboards to open. | e.g. ("torso", "head") | +* | sensor | string | - | - | Yes | The name of the sensor to be tested. | e.g. head_imu_0 | +* | frame | string | - | - | Yes | The name of the frame on which the sensor is attached. | e.g. head_imu_0| +* | mean_error | double | - | - | Yes | The tolerance on the mean of the error. | | +*/ + class Imu : public yarp::robottestingframework::TestCase { public: Imu(); @@ -49,7 +77,6 @@ class Imu : public yarp::robottestingframework::TestCase { iDynTree::ModelLoader model; iDynTree::KinDynComputations kinDynComp; - iDynTree::VectorDynSize s; iDynTree::VectorDynSize ds; iDynTree::Vector3 gravity; diff --git a/suites/contexts/icub/test_imu.ini b/suites/contexts/icub/test_imu.ini index 052e023..ba17807 100644 --- a/suites/contexts/icub/test_imu.ini +++ b/suites/contexts/icub/test_imu.ini @@ -2,7 +2,7 @@ robot ${robotname} model model.urdf port /${robotname}/head/inertials part head -remote_controlboards ("torso", "head") +controlboards ("torso", "head") sensor head_imu_0 frame head_imu_0 mean_error 0.1 \ No newline at end of file From a9b26df350e0da3053233faf06e0e2e2232f3543 Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Wed, 24 Jan 2024 16:18:24 +0100 Subject: [PATCH 3/4] Make tmp driver and interfaces members of the class and fix names --- src/imu/imu.cpp | 11 ++--------- src/imu/imu.h | 6 +++++- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index 1205d46..3391980 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -64,10 +64,6 @@ bool Imu::setup(yarp::os::Property& property) yarp::os::Bottle axesNames; yarp::os::Bottle & axesList = axesNames.addList(); - yarp::dev::PolyDriver tmpDriver; - yarp::dev::IEncoders* tmpEncoders; - yarp::dev::IAxisInfo* tmpAxis; - yarp::os::Property tmpOptions; int tmpAxisNum; @@ -116,10 +112,10 @@ bool Imu::setup(yarp::os::Property& property) for (int i = 0; i < axes; i++) { ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(iaxes->getAxisName(i, axisName), "Cannot get the name of controlled axes"); - axis.push_back(axisName); + axesVec.push_back(axisName); } - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(model.loadReducedModelFromFile(modelAbsolutePath.c_str(), axis), Asserter::format("Cannot load model from %s", modelAbsolutePath.c_str())); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(model.loadReducedModelFromFile(modelAbsolutePath.c_str(), axesVec), Asserter::format("Cannot load model from %s", modelAbsolutePath.c_str())); kinDynComp.loadRobotModel(model.model()); iDynTree::Vector3 baseLinkOrientationRad; @@ -178,9 +174,6 @@ void Imu::run() double minLim; double maxLim; - yarp::dev::PolyDriver tmpDriver; - yarp::dev::IEncoders* tmpEncoders; - yarp::dev::IAxisInfo* tmpAxis; yarp::os::Property tmpOptions; int tmpAxisNum; diff --git a/src/imu/imu.h b/src/imu/imu.h index 5f7eb2a..aa9b4af 100644 --- a/src/imu/imu.h +++ b/src/imu/imu.h @@ -66,6 +66,10 @@ class Imu : public yarp::robottestingframework::TestCase { yarp::dev::IAxisInfo* iaxes; yarp::dev::IControlLimits* ilim; + yarp::dev::PolyDriver tmpDriver; + yarp::dev::IEncoders* tmpEncoders; + yarp::dev::IAxisInfo* tmpAxis; + yarp::os::BufferedPort outputPort; yarp::sig::Vector rpyValues; yarp::sig::Vector positions; @@ -73,7 +77,7 @@ class Imu : public yarp::robottestingframework::TestCase { int axes; double timestamp; - std::vector axis; + std::vector axesVec; iDynTree::ModelLoader model; iDynTree::KinDynComputations kinDynComp; From b21417d1f0028afcd589152dcb3d9c06ce4b325a Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Wed, 31 Jan 2024 12:06:03 +0100 Subject: [PATCH 4/4] Set the movable joints a parameter and further improvements --- src/imu/imu.cpp | 76 ++++++++++--------------------- src/imu/imu.h | 5 +- src/imu/plot.xml | 12 ++--- suites/contexts/icub/test_imu.ini | 2 + 4 files changed, 33 insertions(+), 62 deletions(-) diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index 3391980..51155ce 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -24,6 +24,7 @@ bool Imu::setup(yarp::os::Property& property) ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("port"), "The port name must be given as the test parameter!"); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!"); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("controlboards"), "Please, provide the controlboards name."); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("controlled_joints"), "Please, provide the controlled joints name."); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("model"), "Please, provide the urdf model path."); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("frame"), "Please, provide the frame name."); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("sensor"), "Please, provide the sensor name."); @@ -39,6 +40,13 @@ bool Imu::setup(yarp::os::Property& property) yarp::os::ResourceFinder &rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); std::string modelAbsolutePath = rf.findFileByName(modelName); + yarp::os::Bottle *inputMoveJoints; + inputMoveJoints = property.find("move_joints").asList(); + for (int i = 0; i < inputMoveJoints->size(); i++) + { + movJointsList.addString(inputMoveJoints->get(i).asString()); + } + ROBOTTESTINGFRAMEWORK_TEST_REPORT("Running IMU test on "+robotName+"..."); yarp::os::Property MASclientOptions; @@ -58,36 +66,15 @@ bool Imu::setup(yarp::os::Property& property) for (int i = 0; i < inputControlBoards->size(); i++) { remoteControlBoardsList.addString("/"+robotName+"/"+inputControlBoards->get(i).asString()); - std::cout << inputControlBoards->get(i).asString() << std::endl; } yarp::os::Bottle axesNames; yarp::os::Bottle & axesList = axesNames.addList(); - - yarp::os::Property tmpOptions; - - int tmpAxisNum; - std::string tmpString; - - tmpOptions.put("device", "remote_controlboard"); - tmpOptions.put("local", "/tmp"); - - for(int rcb = 0; rcb < remoteControlBoardsList.size(); rcb++) + yarp::os::Bottle *inputJoints; + inputJoints = property.find("controlled_joints").asList(); + for(int i = 0; i < inputJoints->size(); i++) { - tmpOptions.put("remote", remoteControlBoardsList.get(rcb)); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpDriver.open(tmpOptions), "Unable to open the tmp driver"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpDriver.isValid(), "Device driver cannot be opened"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpDriver.view(tmpEncoders), "Unable to open encoder interface"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpDriver.view(tmpAxis), "Unable to open axes interface"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpEncoders->getAxes(&tmpAxisNum), "Cannot get the name of controlled axes"); - - for(int i = 0; i < tmpAxisNum; i++) - { - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tmpAxis->getAxisName(i, tmpString), "Cannot get name of controlled axes"); - axesList.addString(tmpString); - } - - tmpDriver.close(); + axesList.addString(inputJoints->get(i).asString()); } controlBoardOptions.put("remoteControlBoards", remoteControlBoards.get(0)); @@ -174,32 +161,14 @@ void Imu::run() double minLim; double maxLim; - yarp::os::Property tmpOptions; - - int tmpAxisNum; - std::string tmpString; - - tmpOptions.put("device", "remote_controlboard"); - tmpOptions.put("local", "/tmp"); - tmpOptions.put("remote", "/"+robotName+"/"+partName); - - tmpDriver.open(tmpOptions); - tmpDriver.view(tmpEncoders); - tmpDriver.view(tmpAxis); - tmpEncoders->getAxes(&tmpAxisNum); - - for (int i = 0; i < tmpAxisNum; i++) + for (int i = 0; i < movJointsList.size(); i++) { - tmpAxis->getAxisName(i, tmpString); - ilim->getLimits(model.model().getJointIndex(tmpString), &minLim, &maxLim); - - moveJoint(model.model().getJointIndex(tmpString), minLim + 5); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ilim->getLimits(model.model().getJointIndex(movJointsList.get(i).toString()), &minLim, &maxLim), Asserter::format("Unable to get limits for joint %s", movJointsList.get(i).toString())); + moveJoint(model.model().getJointIndex(movJointsList.get(i).toString()), minLim + 5); yarp::os::Time::delay(1.); - moveJoint(model.model().getJointIndex(tmpString), maxLim - 5); + moveJoint(model.model().getJointIndex(movJointsList.get(i).toString()), maxLim - 5); yarp::os::Time::delay(1.); } - - tmpDriver.close(); } bool Imu::sendData(iDynTree::Vector3 expectedValues, iDynTree::Vector3 imuSignal) @@ -229,6 +198,8 @@ 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; yarp::os::Time::delay(.1); ipos->positionMove(ax, pos); @@ -256,15 +227,16 @@ 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]))); - auto error = (expectedImuSignal * imuSignal.inverse()).log(); - double err_mean = (std::accumulate(error.begin(), error.end(), 0)) / error.size(); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(err_mean < errorMean, "Error > 0.1! Aborting ..."); - + error = error + (expectedImuSignal * imuSignal.inverse()).log(); + count++; + 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)); ipos->positionMove(ax, 0.0); - + return true; } \ No newline at end of file diff --git a/src/imu/imu.h b/src/imu/imu.h index aa9b4af..22f8595 100644 --- a/src/imu/imu.h +++ b/src/imu/imu.h @@ -57,6 +57,7 @@ class Imu : public yarp::robottestingframework::TestCase { std::string frameName; std::string sensorName; double errorMean; + yarp::os::Bottle movJointsList; yarp::dev::PolyDriver MASclientDriver; yarp::dev::PolyDriver controlBoardDriver; @@ -66,10 +67,6 @@ class Imu : public yarp::robottestingframework::TestCase { yarp::dev::IAxisInfo* iaxes; yarp::dev::IControlLimits* ilim; - yarp::dev::PolyDriver tmpDriver; - yarp::dev::IEncoders* tmpEncoders; - yarp::dev::IAxisInfo* tmpAxis; - yarp::os::BufferedPort outputPort; yarp::sig::Vector rpyValues; yarp::sig::Vector positions; diff --git a/src/imu/plot.xml b/src/imu/plot.xml index 21f97e9..1cc3411 100644 --- a/src/imu/plot.xml +++ b/src/imu/plot.xml @@ -5,8 +5,8 @@ hspan="1" vspan="1" title="Roll" - minval="-180" - maxval="180" + minval="-200" + maxval="200" bgcolor="LightSlateGrey">