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

ergoCubGazeboV1: make ft frame centered on z w/ the link frame #158

Merged
merged 1 commit into from
Aug 7, 2023
Merged

ergoCubGazeboV1: make ft frame centered on z w/ the link frame #158

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
173 changes: 170 additions & 3 deletions tests/ergocub-model-test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,11 @@
#include <iDynTree/Model/JointState.h>
#include <iDynTree/Model/Indices.h>
#include <iDynTree/Model/Model.h>
#include <iDynTree/Model/Traversal.h>
#include <iDynTree/Model/RevoluteJoint.h>

#include <iDynTree/ModelIO/ModelLoader.h>
#include <iDynTree/Sensors/Sensors.h>
#include <iDynTree/Sensors/SixAxisForceTorqueSensor.h>

#include <yarp/os/Property.h>
#include <cmath>
Expand Down Expand Up @@ -198,7 +200,7 @@ bool checkSolesAreParallel(iDynTree::KinDynComputations & comp)
double l_sole_x = root_H_l_sole.getPosition().getVal(0);
double r_sole_x = root_H_r_sole.getPosition().getVal(0);

// The increased threshold is a workaround for https://github.com/robotology/icub-model-generator/issues/125
// The increased threshold is a workaround for https://github.com/robotology/ergocub-model-generator/issues/125
if( !checkDoubleAreEqual(l_sole_x,r_sole_x, 2e-4) )
{
std::cerr << "ergocub-model-test error: l_sole_x is " << l_sole_x << ", while r_sole_x is " << r_sole_x << " (diff : " << std::fabs(l_sole_x-r_sole_x) << " )" << std::endl;
Expand All @@ -209,7 +211,7 @@ bool checkSolesAreParallel(iDynTree::KinDynComputations & comp)
double l_sole_y = root_H_l_sole.getPosition().getVal(1);
double r_sole_y = root_H_r_sole.getPosition().getVal(1);

// The increased threshold is a workaround for https://github.com/robotology/icub-model-generator/issues/125
// The increased threshold is a workaround for https://github.com/robotology/ergocub-model-generator/issues/125
if( !checkDoubleAreEqual(l_sole_y,-r_sole_y,1e-4) )
{
std::cerr << "ergocub-model-test error: l_sole_y is " << l_sole_y << ", while r_sole_y is " << r_sole_y << " while they should be simmetric (diff : " << std::fabs(l_sole_y+r_sole_y) << " )" << std::endl;
Expand Down Expand Up @@ -476,6 +478,166 @@ bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp)
return ok;
}


bool Model_isFrameNameUsed(const iDynTree::Model& model, const std::string frameName)
{
for(size_t i=0; i < model.getNrOfLinks(); i++ )
{
if( frameName == model.getLinkName(i) )
{
return true;
}
}

for(size_t i=model.getNrOfLinks(); i < model.getNrOfFrames(); i++ )
{

if( frameName == model.getFrameName(i) )
{
return true;
}
}

return false;
}

bool checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(const std::string& modelPath,
iDynTree::KinDynComputations& comp,
const iDynTree::SensorsList& sensors,
const std::string& sensorName)
{
// As of mid 2023, for iCub 3 models the frame name is <prefix>_ft, while the sensor name is <prefix>_ft,
// and the joint name is <prefix>_ft_sensor
std::string frameName = sensorName;

//std::cerr << comp.model().toString() << std::endl;

// Check frame exist
if (!comp.model().isFrameNameUsed(frameName))
{
std::cerr << "ergocub-model-test : model " << modelPath << " does not contain frame " << frameName << " as expected." << std::endl;
return false;
}

// Check sensors exists
std::ptrdiff_t sensorIdx;
if (!sensors.getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE, sensorName, sensorIdx))
{
std::cerr << "ergocub-model-test : model " << modelPath << " does not contain FT sensor " << sensorName << " as expected." << std::endl;
return false;
}

// Get root_H_link
iDynTree::Transform root_H_frame = comp.getRelativeTransform("root_link", frameName);

// Get root_H_sensor
iDynTree::SixAxisForceTorqueSensor * sens
= (::iDynTree::SixAxisForceTorqueSensor *) sensors.getSensor(::iDynTree::SIX_AXIS_FORCE_TORQUE, sensorIdx);
if (!sens)
{
std::cerr << "ergocub-model-test : model " << modelPath << " error in reading sensor " << sensorName << "." << std::endl;
return false;
}

std::string firstLinkName = sens->getFirstLinkName();
iDynTree::Transform root_H_firstLink = comp.getRelativeTransform("root_link", firstLinkName);
iDynTree::Transform firstLink_H_sensor;
bool ok = sens->getLinkSensorTransform(sens->getFirstLinkIndex(), firstLink_H_sensor);

if (!ok)
{
std::cerr << "ergocub-model-test : model " << modelPath << " error in reading transform of sensor " << sensorName << "." << std::endl;
return false;
}


iDynTree::Transform root_H_sensor = root_H_firstLink*firstLink_H_sensor;

// Check that the two transfom are equal equal
if (!checkTransformAreEqual(root_H_frame, root_H_sensor, 1e-6))
{
std::cerr << "ergocub-model-test : transform between root_H_frame and root_H_sensor for " << sensorName << " is not the expected one, test failed." << std::endl;
std::cerr << "ergocub-model-test : root_H_frame :" << root_H_frame.toString() << std::endl;
std::cerr << "ergocub-model-test : root_H_sensor :" << root_H_sensor.toString() << std::endl;
return false;
}

// Beside checking that root_H_frame and root_H_sensor, we should also check that the translation
// between the child link of FT joint and the FT frame is the zero vector, as as of mid 2023 the SDF
// standard always assume that the 6D FT measured by the sensor is expressed in the origin of the child link frame
// See https://github.com/gazebosim/sdformat/issues/130 for more details
iDynTree::Traversal traversalWithURDFBase;
comp.model().computeFullTreeTraversal(traversalWithURDFBase);

iDynTree::LinkIndex childLinkIdx = traversalWithURDFBase.getChildLinkIndexFromJointIndex(comp.model(), sens->getParentJointIndex());
std::string childLinkName = comp.model().getLinkName(childLinkIdx);

iDynTree::Transform childLink_H_sensorFrame = comp.getRelativeTransform(childLinkName, frameName);

iDynTree::Vector3 zeroVector;
zeroVector.zero();

if (!checkVectorAreEqual(childLink_H_sensorFrame.getPosition(), zeroVector, 1e-6))
{
std::cerr << "ergocub-model-test : translation between link " << childLinkName << " and sensor " << sensorName << " is non-zero, test failed, see https://github.com/gazebosim/sdformat/issues/130 for more details." << std::endl;
std::cerr << "ergocub-model-test : childLink_H_sensorFrame.getPosition(): " << childLink_H_sensorFrame.getPosition().toString() << std::endl;
return false;
}

return true;
}

// Check FT sensors
// This is only possible with V3 as V3 models have FT frame exported models
// However, as of mid 2023 the V2 models do not need this check as the link explicitly
// are using the FT frames as frames of the corresponding link
bool checkAllFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(const std::string& modelPath)
{
iDynTree::ModelLoader mdlLoader;

// Open the model
iDynTree::ModelParserOptions parserOptions;

// By default iDynTree creates an additional frame with the same name of the sensor,
// however in this case we have both the sensor and the urdf frame called <prefix>_ft,
// and for this test we want to make sure that the <prefix>_ft additional frame is the
// one in the URDF
parserOptions.addSensorFramesAsAdditionalFrames = false;
mdlLoader.setParsingOptions(parserOptions);

mdlLoader.loadModelFromFile(modelPath);

iDynTree::KinDynComputations comp;
const bool modelLoaded = comp.loadRobotModel(mdlLoader.model());

if (!modelLoaded)
{
std::cerr << "ergocub-model-test error: impossible to load model from " << modelLoaded << std::endl;
return false;
}

iDynTree::Vector3 grav;
grav.zero();
iDynTree::JointPosDoubleArray qj(comp.getRobotModel());
iDynTree::JointDOFsDoubleArray dqj(comp.getRobotModel());
qj.zero();
dqj.zero();

comp.setRobotState(qj,dqj,grav);
iDynTree::SensorsList sensors = mdlLoader.sensors();


bool ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_arm_ft");
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_arm_ft") && ok;
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_leg_ft") && ok;
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_leg_ft") && ok;
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_foot_rear_ft") && ok;
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_foot_rear_ft") && ok;
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_foot_front_ft") && ok;
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_foot_front_ft") && ok;
return ok;
}

int main(int argc, char ** argv)
{
yarp::os::Property prop;
Expand Down Expand Up @@ -548,6 +710,11 @@ int main(int argc, char ** argv)
return EXIT_FAILURE;
}

if (!checkAllFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath))
{
return EXIT_FAILURE;
}


std::cerr << "Check for model " << modelPath << " concluded correctly!" << std::endl;

Expand Down
Loading