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