Skip to content

Commit

Permalink
Make remoteControlBoards and controlled joints parametrized lists
Browse files Browse the repository at this point in the history
  • Loading branch information
@martinaxgloria
martinaxgloria committed Jan 18, 2024
1 parent b338b10 commit 5f84b94
Show file tree
Hide file tree
Showing 4 changed files with 90 additions and 47 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
117 changes: 79 additions & 38 deletions src/imu/imu.cpp
View file
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("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);
Expand All @@ -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");
Expand All @@ -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<std::string> axis;

for (int i = 0; i < axes; i++)
{
Expand All @@ -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++)
{
Expand All @@ -135,15 +158,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 +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)
Expand Down Expand Up @@ -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++)
{
Expand All @@ -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);
Expand Down
3 changes: 3 additions & 0 deletions src/imu/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -43,6 +45,7 @@ class Imu : public yarp::robottestingframework::TestCase {

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

iDynTree::ModelLoader model;
iDynTree::KinDynComputations kinDynComp;
Expand Down
13 changes: 8 additions & 5 deletions suites/contexts/icub/test_imu.ini
View file
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
remote_controlboards ("torso", "head")
sensor head_imu_0
frame head_imu_0
mean_error 0.1

0 comments on commit 5f84b94

Please sign in to comment.