Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Imu test - define and configure parameters of the test in the .ini file #67

Merged
merged 4 commits into from
Feb 1, 2024
Merged

Imu test - define and configure parameters of the test in the .ini file #67

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
5f84b94
Make remoteControlBoards and controlled joints parametrized lists
martinaxgloria Jan 18, 2024
ad31d2e
Add doxygen documentation
martinaxgloria Jan 24, 2024
a9b26df
Make tmp driver and interfaces members of the class and fix names
martinaxgloria Jan 24, 2024
b21417d
Set the movable joints a parameter and further improvements
martinaxgloria Jan 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 0 additions & 4 deletions src/imu/CMakeLists.txt
View file
Open in desktop
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
114 changes: 74 additions & 40 deletions src/imu/imu.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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("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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this used anywhere?

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);
Expand All @@ -45,34 +50,49 @@ 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());
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::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");
Expand All @@ -88,15 +108,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 +132,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 +154,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,15 +174,32 @@ void Imu::run() {
double minLim;
double maxLim;

for (int i = kinDynComp.model().getJointIndex("neck_pitch"); i <= kinDynComp.model().getJointIndex("neck_yaw"); i++)
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)
Expand Down Expand Up @@ -191,19 +229,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++)
{
Expand All @@ -223,11 +258,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);
Expand Down
36 changes: 35 additions & 1 deletion src/imu/imu.h
View file
Open in desktop
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,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;
Expand All @@ -36,17 +66,21 @@ 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 <yarp::os::Bottle> outputPort;
yarp::sig::Vector rpyValues;
yarp::sig::Vector positions;
yarp::sig::Vector velocities;

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
13 changes: 8 additions & 5 deletions suites/contexts/icub/test_imu.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
controlboards ("torso", "head")
sensor head_imu_0
frame head_imu_0
mean_error 0.1