Skip to content

Commit

Permalink
Merge pull request #67 from martinaxgloria/imu-test-stint1
Browse files Browse the repository at this point in the history 
Imu test - define and configure parameters of the test in the .ini file
  • Loading branch information
@Nicogene
Nicogene authored Feb 1, 2024
2 parents b338b10 + b21417d commit e579b72
Show file tree
Hide file tree
Showing 5 changed files with 100 additions and 62 deletions.
4 changes: 0 additions & 4 deletions src/imu/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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
Expand Down
98 changes: 52 additions & 46 deletions src/imu/imu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,22 +18,35 @@ 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("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.");
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);

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;
Expand All @@ -45,34 +58,28 @@ 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("controlboards").asList();
for (int i = 0; i < inputControlBoards->size(); i++)
{
remoteControlBoardsList.addString("/"+robotName+"/"+inputControlBoards->get(i).asString());
}

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::os::Bottle *inputJoints;
inputJoints = property.find("controlled_joints").asList();
for(int i = 0; i < inputJoints->size(); i++)
{
axesList.addString(inputJoints->get(i).asString());
}

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");
Expand All @@ -88,15 +95,14 @@ 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<std::string> axis;

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;
Expand All @@ -113,7 +119,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++)
{
Expand All @@ -135,15 +141,17 @@ bool Imu::setup(yarp::os::Property& property) {
return true;
}

void Imu::tearDown() {
void Imu::tearDown()
{
outputPort.interrupt();
outputPort.close();

controlBoardDriver.close();
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.");

Expand All @@ -153,13 +161,12 @@ void Imu::run() {
double minLim;
double maxLim;

for (int i = kinDynComp.model().getJointIndex("neck_pitch"); i <= kinDynComp.model().getJointIndex("neck_yaw"); i++)
for (int i = 0; i < movJointsList.size(); i++)
{
ilim->getLimits(i, &minLim, &maxLim);

moveJoint(i, 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(i, maxLim - 5);
moveJoint(model.model().getJointIndex(movJointsList.get(i).toString()), maxLim - 5);
yarp::os::Time::delay(1.);
}
}
Expand Down Expand Up @@ -191,19 +198,18 @@ bool Imu::sendData(iDynTree::Vector3 expectedValues, iDynTree::Vector3 imuSignal
bool Imu::moveJoint(int ax, double pos)
{
bool done = false;
double refPos;

int count = 0;
iDynTree::GeomVector3 error;
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++)
{
Expand All @@ -221,16 +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 < 0.1, "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;
}
33 changes: 32 additions & 1 deletion src/imu/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,34 @@
#include <iDynTree/ModelLoader.h>
#include <iDynTree/Model.h>

/**
* \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();
Expand All @@ -24,9 +52,12 @@ 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::os::Bottle movJointsList;

yarp::dev::PolyDriver MASclientDriver;
yarp::dev::PolyDriver controlBoardDriver;
Expand All @@ -43,10 +74,10 @@ class Imu : public yarp::robottestingframework::TestCase {

int axes;
double timestamp;
std::vector<std::string> axesVec;

iDynTree::ModelLoader model;
iDynTree::KinDynComputations kinDynComp;

iDynTree::VectorDynSize s;
iDynTree::VectorDynSize ds;
iDynTree::Vector3 gravity;
Expand Down
12 changes: 6 additions & 6 deletions src/imu/plot.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@
hspan="1"
vspan="1"
title="Roll"
minval="-180"
maxval="180"
minval="-200"
maxval="200"
bgcolor="LightSlateGrey">
<graph remote="/test-imu/out"
index="0"
Expand All @@ -26,8 +26,8 @@
hspan="1"
vspan="1"
title="Pitch"
minval="-180"
maxval="180"
minval="-200"
maxval="200"
bgcolor="LightSlateGrey">
<graph remote="/test-imu/out"
index="1"
Expand All @@ -47,8 +47,8 @@
hspan="1"
vspan="1"
title="Yaw"
minval="-180"
maxval="180"
minval="-200"
maxval="200"
bgcolor="LightSlateGrey">
<graph remote="/test-imu/out"
index="2"
Expand Down
15 changes: 10 additions & 5 deletions suites/contexts/icub/test_imu.ini
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,10 @@
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
controlboards ("torso", "head")
controlled_joints ("torso_pitch", "torso_roll", "torso_yaw", "neck_pitch", "neck_roll", "neck_yaw")
move_joints ("neck_pitch", "neck_roll", "neck_yaw")
sensor head_imu_0
frame head_imu_0
mean_error 0.1

0 comments on commit e579b72

Please sign in to comment.