Skip to content

Commit

Permalink
Merge pull request #243 from robotology/ftorigintesticub3
Browse files Browse the repository at this point in the history 
Fix location of *_foot_* links in iCub3 models and add tests on the location of FT sensors
  • Loading branch information
@traversaro
traversaro authored Aug 7, 2023
2 parents 55ad5ce + 168ce45 commit 2f94125
Show file tree
Hide file tree
Showing 3 changed files with 184 additions and 10 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/ci.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ jobs:
## idyntree
git clone https://github.com/robotology/idyntree.git
cd idyntree
git checkout v1.2.0
git checkout v2.0.0
export IDYNTREE_COMMIT=`git rev-parse HEAD`
mkdir build
cd build
Expand Down
8 changes: 4 additions & 4 deletions simmechanics/data/icub3/ICUB3_ALL_SIM_MODEL.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1877,7 +1877,7 @@
<name>"CS3"</name>
<displayName>"SCSYS_R_FOOT_FRONT"</displayName>
<nodeID>"221(USERADDED)"</nodeID>
<position>64.85,108.5,-946.447</position>
<position>64.85,108.5,-924.147</position>
<positionOrigin>"WORLD"</positionOrigin>
<positionReferenceFrame>"WORLD"</positionReferenceFrame>
<positionUnits>"mm"</positionUnits>
Expand Down Expand Up @@ -1971,7 +1971,7 @@
<name>"CS4"</name>
<displayName>"SCSYS_R_FOOT_REAR"</displayName>
<nodeID>"229(USERADDED)"</nodeID>
<position>64.85,-10.75,-946.447</position>
<position>64.85,-10.75,-924.147</position>
<positionOrigin>"WORLD"</positionOrigin>
<positionReferenceFrame>"WORLD"</positionReferenceFrame>
<positionUnits>"mm"</positionUnits>
Expand Down Expand Up @@ -2671,7 +2671,7 @@
<name>"CS2"</name>
<displayName>"SCSYS_L_FOOT_FRONT"</displayName>
<nodeID>"228(USERADDED)"</nodeID>
<position>-64.75,108.55,-946.447</position>
<position>-64.75,108.55,-924.147</position>
<positionOrigin>"WORLD"</positionOrigin>
<positionReferenceFrame>"WORLD"</positionReferenceFrame>
<positionUnits>"mm"</positionUnits>
Expand Down Expand Up @@ -2752,7 +2752,7 @@
<name>"CS2"</name>
<displayName>"SCSYS_L_FOOT_REAR"</displayName>
<nodeID>"219(USERADDED)"</nodeID>
<position>-64.75,-10.7,-946.447</position>
<position>-64.75,-10.7,-924.147</position>
<positionOrigin>"WORLD"</positionOrigin>
<positionReferenceFrame>"WORLD"</positionReferenceFrame>
<positionUnits>"mm"</positionUnits>
Expand Down
184 changes: 179 additions & 5 deletions tests/icub-model-test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,12 @@
#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 @@ -503,6 +506,172 @@ bool checkFTSensorsAreCorrectlyOrientedV3(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 << "icub-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 << "icub-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 << "icub-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 << "icub-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 << "icub-model-test : transform between root_H_frame and root_H_sensor for " << sensorName << " is not the expected one, test failed." << std::endl;
std::cerr << "icub-model-test : root_H_frame :" << root_H_frame.toString() << std::endl;
std::cerr << "icub-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 << "icub-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 << "icub-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 << "icub-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;
}


bool isiCub3Model(const std::string& modelPath)
{
return (modelPath.find("Genova09") != std::string::npos ||
modelPath.find("GazeboV3") != std::string::npos);
}

int main(int argc, char ** argv)
{
yarp::os::Property prop;
Expand Down Expand Up @@ -545,8 +714,7 @@ int main(int argc, char ** argv)
comp.setRobotState(qj,dqj,grav);

// Check axis
if (modelPath.find("Genova09") != std::string::npos ||
modelPath.find("GazeboV3") != std::string::npos) {
if (isiCub3Model(modelPath)) {
if( !checkAxisDirectionsV3(comp) )
{
return EXIT_FAILURE;
Expand Down Expand Up @@ -575,8 +743,7 @@ int main(int argc, char ** argv)

// Now some test that test the sensors
// The ft sensors orientation respect to the root_link are different to iCubV2 and they are under investigation.
if (modelPath.find("Genova09") != std::string::npos ||
modelPath.find("GazeboV3") != std::string::npos) {
if (isiCub3Model(modelPath)) {

if( !checkFTSensorsAreEvenAndNotNull(mdlLoader) )
{
Expand All @@ -601,6 +768,13 @@ int main(int argc, char ** argv)
}
}

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



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

0 comments on commit 2f94125

Please sign in to comment.