Skip to content

Commit

Permalink
Set the movable joints a parameter and further improvements
Browse files Browse the repository at this point in the history
  • Loading branch information
@martinaxgloria
martinaxgloria committed Jan 31, 2024
1 parent a9b26df commit b21417d
Show file tree
Hide file tree
Showing 4 changed files with 33 additions and 62 deletions.
76 changes: 24 additions & 52 deletions src/imu/imu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.");
Expand All @@ -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;
Expand All @@ -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));
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
}
5 changes: 1 addition & 4 deletions src/imu/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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 <yarp::os::Bottle> outputPort;
yarp::sig::Vector rpyValues;
yarp::sig::Vector positions;
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
2 changes: 2 additions & 0 deletions suites/contexts/icub/test_imu.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@ 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 b21417d

Please sign in to comment.