bool solidShapesFromURDFString(const std::string & urdf_string,
const std::string & urdf_filename,
const Model & model,
const std::string urdfGeometryType,
iDynTree::ModelSolidShapes & output)
{
if (urdfGeometryType != "visual" && urdfGeometryType != "collision")
{
std::cerr << "[ERROR] unknown urdfGeometryType " << urdfGeometryType << std::endl;
return false;
}
ModelLoader loader;
ModelParserOptions options;
options.originalFilename = urdf_filename;
loader.setParsingOptions(options);
bool ok = loader.loadModelFromString(urdf_string);
if (ok && urdfGeometryType == "visual")
{
output = loader.model().visualSolidShapes();
}
if (ok && urdfGeometryType == "collision")
{
output = loader.model().collisionSolidShapes();
}
return ok;
}
bool solidShapesFromURDF(const std::string & urdf_filename,
const Model & model,
const std::string urdfGeometryType,
ModelSolidShapes & output)
{
if (urdfGeometryType != "visual" && urdfGeometryType != "collision")
{
std::cerr << "[ERROR] unknown urdfGeometryType " << urdfGeometryType << std::endl;
return false;
}
ModelLoader loader;
bool ok = loader.loadModelFromFile(urdf_filename);
if (ok && urdfGeometryType == "visual")
{
output = loader.model().visualSolidShapes();
}
if (ok && urdfGeometryType == "collision")
{
output = loader.model().collisionSolidShapes();
}
return ok;
}
Model getModel(const std::string& fileName)
{
ModelLoader loader;
bool ok = loader.loadModelFromFile(fileName);
// Load model
ASSERT_IS_TRUE(ok);
Model model = loader.model();
return model;
}
void testFwdKinConsistency(std::string modelFilePath)
{
std::cout << "Testing " << modelFilePath << std::endl;
// Load iDynTree model and compute a default traversal
ModelLoader loader;
loader.loadModelFromFile(modelFilePath);
iDynTree::Model model = loader.model();
iDynTree::Traversal traversal;
model.computeFullTreeTraversal(traversal);
// Load KDL tree
KDL::Tree tree;
treeFromUrdfFile(modelFilePath,tree);
KDL::CoDyCo::UndirectedTree undirected_tree(tree);
KDL::CoDyCo::Traversal kdl_traversal;
undirected_tree.compute_traversal(kdl_traversal);
// A KDL::Tree only supports 0 and 1 DOFs joints, and
// KDL::Tree::getNrOfJoints does not count the 0 dof joints, so
// it is actually equivalent to iDynTree::Model::getNrOfDOFs
ASSERT_EQUAL_DOUBLE(tree.getNrOfJoints(),model.getNrOfDOFs());
// Input : joint positions and base position with respect to world
iDynTree::FreeFloatingPos baseJntPos(model);
iDynTree::FreeFloatingVel baseJntVel(model);
iDynTree::FreeFloatingAcc baseJntAcc(model);
baseJntPos.worldBasePos() = getRandomTransform();
baseJntVel.baseVel() = getRandomTwist();
baseJntAcc.baseAcc() = getRandomTwist();
for(unsigned int jnt=0; jnt < baseJntPos.getNrOfPosCoords(); jnt++)
{
baseJntPos.jointPos()(jnt) = getRandomDouble();
baseJntVel.jointVel()(jnt) = getRandomDouble();
baseJntAcc.jointAcc()(jnt) = getRandomDouble();
}
// Build a map between KDL joints and iDynTree joints because we are not sure
// that the joint indices will match
std::vector<int> kdl2idyntree_joints;
kdl2idyntree_joints.resize(undirected_tree.getNrOfDOFs());
for(unsigned int dofIndex=0; dofIndex < undirected_tree.getNrOfDOFs(); dofIndex++)
{
std::string dofName = undirected_tree.getJunction(dofIndex)->getName();
int idyntreeJointIndex = model.getJointIndex(dofName);
kdl2idyntree_joints[dofIndex] = idyntreeJointIndex;
}
KDL::JntArray jntKDL(undirected_tree.getNrOfDOFs());
KDL::JntArray jntVelKDL(undirected_tree.getNrOfDOFs());
KDL::JntArray jntAccKDL(undirected_tree.getNrOfDOFs());
KDL::Frame worldBaseKDL;
KDL::Twist baseVelKDL;
KDL::Twist baseAccKDL;
ToKDL(baseJntPos,worldBaseKDL,jntKDL,kdl2idyntree_joints);
ToKDL(baseJntVel,baseVelKDL,jntVelKDL,kdl2idyntree_joints);
ToKDL(baseJntAcc,baseAccKDL,jntAccKDL,kdl2idyntree_joints);
// Output : link (for iDynTree) or frame positions with respect to world
std::vector<KDL::Frame> framePositions(undirected_tree.getNrOfLinks());
iDynTree::LinkPositions linkPositions(model);
// Build a map between KDL links and iDynTree links because we are not sure
// that the link indices will match
std::vector<int> idynTree2KDL_links;
idynTree2KDL_links.resize(model.getNrOfLinks());
for(unsigned int linkIndex=0; linkIndex < model.getNrOfLinks(); linkIndex++)
{
std::string linkName = model.getLinkName(linkIndex);
int kdlLinkIndex = undirected_tree.getLink(linkName)->getLinkIndex();
idynTree2KDL_links[linkIndex] = kdlLinkIndex;
}
// Compute position forward kinematics with old KDL code and with the new iDynTree code
KDL::CoDyCo::getFramesLoop(undirected_tree,
jntKDL,
kdl_traversal,
framePositions,
worldBaseKDL);
ForwardPositionKinematics(model,traversal,baseJntPos,linkPositions);
// Check results
for(unsigned int travEl = 0; travEl < traversal.getNrOfVisitedLinks(); travEl++)
{
LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();
ASSERT_EQUAL_TRANSFORM_TOL(linkPositions(linkIndex),ToiDynTree(framePositions[idynTree2KDL_links[linkIndex]]),1e-1);
}
// Compution velocity & acceleration kinematics
std::vector<KDL::Twist> kdlLinkVel(undirected_tree.getNrOfLinks());
std::vector<KDL::Twist> kdlLinkAcc(undirected_tree.getNrOfLinks());
KDL::CoDyCo::rneaKinematicLoop(undirected_tree,jntKDL,jntVelKDL,jntAccKDL,kdl_traversal,
baseVelKDL,baseAccKDL,kdlLinkVel,kdlLinkAcc);
iDynTree::LinkVelArray linksVels(model);
iDynTree::LinkAccArray linksAcc(model);
ForwardVelAccKinematics(model,traversal,baseJntPos,baseJntVel,baseJntAcc,linksVels,linksAcc);
// Check results
for(unsigned int travEl = 0; travEl < traversal.getNrOfVisitedLinks(); travEl++)
{
LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();
ASSERT_EQUAL_VECTOR(linksVels(linkIndex).asVector(),
ToiDynTree(kdlLinkVel[idynTree2KDL_links[linkIndex]]).asVector());
ASSERT_EQUAL_VECTOR(linksAcc(linkIndex).asVector(),
ToiDynTree(kdlLinkAcc[idynTree2KDL_links[linkIndex]]).asVector());
}
// Compute position, velocity and acceleration
LinkPositions linksPos_check(model);
iDynTree::LinkVelArray linksVels_check(model);
iDynTree::LinkAccArray linksAcc_check(model);
ForwardPosVelAccKinematics(model,traversal,baseJntPos, baseJntVel, baseJntAcc,
linksPos_check,linksVels_check,linksAcc_check);
for(unsigned int travEl = 0; travEl < traversal.getNrOfVisitedLinks(); travEl++)
{
LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();
ASSERT_EQUAL_TRANSFORM(linkPositions(linkIndex),
linksPos_check(linkIndex));
ASSERT_EQUAL_VECTOR(linksVels(linkIndex).asVector(),
linksVels_check(linkIndex).asVector());
ASSERT_EQUAL_VECTOR(linksAcc(linkIndex).asVector(),
linksAcc(linkIndex).asVector());
}
// Compute torques (i.e. inverse dynamics)
LinkNetExternalWrenches fExt(model);
LinkInternalWrenches f(model);
FreeFloatingGeneralizedTorques baseWrenchJointTorques(model);
KDL::JntArray jntTorques(model.getNrOfDOFs());
KDL::Wrench baseWrench;
std::vector<KDL::Wrench> fExtKDL(undirected_tree.getNrOfLinks());
std::vector<KDL::Wrench> fKDL(undirected_tree.getNrOfLinks());
// First set to zero all the fExtKDL, fKDL wrenches : then
// the following loop will set the one that correspond to "real"
// iDynTree links to the same value we used in iDynTree
for(unsigned int linkKDL = 0; linkKDL < undirected_tree.getNrOfLinks(); linkKDL++)
{
fKDL[linkKDL] = KDL::Wrench::Zero();
fExtKDL[linkKDL] = KDL::Wrench::Zero();
}
for(unsigned int link = 0; link < model.getNrOfLinks(); link++ )
{
fExt(link) = getRandomWrench();
fExtKDL[idynTree2KDL_links[link]] = ToKDL(fExt(link));
fKDL[idynTree2KDL_links[link]] = KDL::Wrench::Zero();
}
bool ok = RNEADynamicPhase(model,traversal,
baseJntPos.jointPos(),
linksVels,linksAcc,fExt,f,baseWrenchJointTorques);
int retVal = KDL::CoDyCo::rneaDynamicLoop(undirected_tree,jntKDL,kdl_traversal,
kdlLinkVel,kdlLinkAcc,
fExtKDL,fKDL,jntTorques,baseWrench);
ASSERT_EQUAL_DOUBLE(ok,true);
ASSERT_EQUAL_DOUBLE(retVal,0);
/***
* This check is commented out because KDL::CoDyCo::rneaDynamicLoop returned
* a reverse baseWrench.. still need to be investigated.
* (The - is necessary for consistency with the mass matrix..)
*
*/
//ASSERT_EQUAL_VECTOR(baseWrenchJointTorques.baseWrench().asVector(),
// ToiDynTree(baseWrench).asVector());
for(int link = 0; link < model.getNrOfLinks(); link++ )
{
ASSERT_EQUAL_VECTOR(f(link).asVector(),ToiDynTree(fKDL[idynTree2KDL_links[link]]).asVector());
}
for(int dof = 0; dof < model.getNrOfDOFs(); dof++ )
{
ASSERT_EQUAL_DOUBLE(jntTorques(dof),baseWrenchJointTorques.jointTorques()(kdl2idyntree_joints[dof]));
}
// Check mass matrix
iDynTree::FreeFloatingMassMatrix massMatrix(model);
iDynTree::LinkInertias crbis(model);
ok = CompositeRigidBodyAlgorithm(model,traversal,
baseJntPos.jointPos(),
crbis,massMatrix);
KDL::CoDyCo::FloatingJntSpaceInertiaMatrix massMatrixKDL(undirected_tree.getNrOfDOFs()+6);
std::vector<KDL::RigidBodyInertia> Ic(undirected_tree.getNrOfLinks());
retVal = KDL::CoDyCo::crba_floating_base_loop(undirected_tree,kdl_traversal,jntKDL,Ic,massMatrixKDL);
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_DOUBLE_TOL(retVal,0,1e-8);
// Check composite rigid body inertias
for(int link = 0; link < model.getNrOfLinks(); link++ )
{
ASSERT_EQUAL_MATRIX(crbis(link).asMatrix(),ToiDynTree(Ic[idynTree2KDL_links[link]]).asMatrix());
}
std::cerr << "iDynTree " << massMatrix.toString() << std::endl;
std::cerr << "massMatrix " << massMatrixKDL.data << std::endl;
// Check CRBA algorithm
for(int ii=0; ii < model.getNrOfDOFs()+6; ii++ )
{
for( int jj=0; jj < model.getNrOfDOFs()+6; jj++ )
{
int idyntreeII = kdl2idyntreeFloatingDOFMapping(ii,kdl2idyntree_joints);
int idyntreeJJ = kdl2idyntreeFloatingDOFMapping(jj,kdl2idyntree_joints);
ASSERT_EQUAL_DOUBLE_TOL(massMatrix(idyntreeII,idyntreeJJ),massMatrixKDL(ii,jj),1e-8);
}
}
return;
}
void dynamicsBenchmark(std::string modelFilePath, unsigned int nrOfTrials)
{
std::cout << "Benchmarking dynamics algorithms for " << modelFilePath << std::endl;
// initialization variables
ModelLoader loader;
loader.loadModelFromFile(modelFilePath);
iDynTree::Model model = loader.model();
iDynTree::Traversal traversal;
model.computeFullTreeTraversal(traversal);
// Load KDL tree
KDL::Tree tree;
treeFromUrdfFile(modelFilePath,tree);
KDL::CoDyCo::UndirectedTree undirected_tree(tree);
KDL::CoDyCo::Traversal kdl_traversal;
undirected_tree.compute_traversal(kdl_traversal);
// Input : iDynTree data structures
iDynTree::FreeFloatingPos baseJntPos(model);
iDynTree::FreeFloatingVel baseJntVel(model);
iDynTree::FreeFloatingAcc baseJntAcc(model);
iDynTree::LinkVelArray linksVels(model);
iDynTree::LinkAccArray linksAcc(model);
iDynTree::LinkPositions linkPositions(model);
baseJntPos.worldBasePos() = getRandomTransform();
baseJntVel.baseVel() = getRandomTwist();
baseJntAcc.baseAcc() = getRandomTwist();
for(unsigned int jnt=0; jnt < baseJntPos.getNrOfPosCoords(); jnt++)
{
baseJntPos.jointPos()(jnt) = getRandomDouble();
baseJntVel.jointVel()(jnt) = getRandomDouble();
baseJntAcc.jointAcc()(jnt) = getRandomDouble();
}
KDL::JntArray jntKDL(undirected_tree.getNrOfDOFs());
KDL::JntArray jntVelKDL(undirected_tree.getNrOfDOFs());
KDL::JntArray jntAccKDL(undirected_tree.getNrOfDOFs());
KDL::Frame worldBaseKDL;
KDL::Twist baseVelKDL;
KDL::Twist baseAccKDL;
std::vector<KDL::Twist> kdlLinkVel(undirected_tree.getNrOfLinks());
std::vector<KDL::Twist> kdlLinkAcc(undirected_tree.getNrOfLinks());
LinkNetExternalWrenches fExt(model);
LinkInternalWrenches f(model);
FreeFloatingGeneralizedTorques baseWrenchJointTorques(model);
KDL::JntArray jntTorques(model.getNrOfDOFs());
KDL::Wrench baseWrench;
std::vector<KDL::Wrench> fExtKDL(undirected_tree.getNrOfLinks());
std::vector<KDL::Wrench> fKDL(undirected_tree.getNrOfLinks());
iDynTree::FreeFloatingMassMatrix massMatrix(model);
iDynTree::LinkInertias crbis(model);
KDL::CoDyCo::FloatingJntSpaceInertiaMatrix massMatrixKDL(undirected_tree.getNrOfDOFs()+6);
std::vector<KDL::RigidBodyInertia> Ic(undirected_tree.getNrOfLinks());
struct benchTime
{
double totalTimeRNEA;
double totalTimeCRBA;
};
benchTime KDLtimes;
KDLtimes.totalTimeCRBA = 0.0;
KDLtimes.totalTimeRNEA = 0.0;
benchTime iDynTreeTimes;
iDynTreeTimes.totalTimeCRBA = 0.0;
iDynTreeTimes.totalTimeRNEA = 0.0;
double tic,toc;
for(unsigned int trial=0; trial < nrOfTrials; trial++ )
{
tic = clockInSec();
KDL::CoDyCo::rneaKinematicLoop(undirected_tree,jntKDL,jntVelKDL,jntAccKDL,kdl_traversal,
baseVelKDL,baseAccKDL,kdlLinkVel,kdlLinkAcc);
KDL::CoDyCo::rneaDynamicLoop(undirected_tree,jntKDL,kdl_traversal,
kdlLinkVel,kdlLinkAcc,
fExtKDL,fKDL,jntTorques,baseWrench);
toc = clockInSec();
KDLtimes.totalTimeRNEA += (toc-tic);
tic = clockInSec();
ForwardVelAccKinematics(model,traversal,baseJntPos,baseJntVel,baseJntAcc,linksVels,linksAcc);
RNEADynamicPhase(model,traversal,
baseJntPos.jointPos(),
linksVels,linksAcc,fExt,f,baseWrenchJointTorques);
toc = clockInSec();
iDynTreeTimes.totalTimeRNEA += (toc-tic);
tic = clockInSec();
CompositeRigidBodyAlgorithm(model,traversal,baseJntPos.jointPos(),crbis,massMatrix);
toc = clockInSec();
iDynTreeTimes.totalTimeCRBA += (toc-tic);
tic = clockInSec();
KDL::CoDyCo::crba_floating_base_loop(undirected_tree,kdl_traversal,jntKDL,Ic,massMatrixKDL);
toc = clockInSec();
KDLtimes.totalTimeCRBA += (toc-tic);
}
std::cout << "KDL-based implementation : " << std::endl;
std::cout << "\tRNEA average time " << (KDLtimes.totalTimeRNEA/nrOfTrials)*1e6 << " microseconds" << std::endl;
std::cout << "\tCRBA average time " << (KDLtimes.totalTimeCRBA/nrOfTrials)*1e6 << " microseconds" << std::endl;
std::cout << "iDynTree-based implementation : " << std::endl;
std::cout << "\tRNEA average time " << (iDynTreeTimes.totalTimeRNEA/nrOfTrials)*1e6 << " microseconds" << std::endl;
std::cout << "\tCRBA average time " << (iDynTreeTimes.totalTimeCRBA/nrOfTrials)*1e6 << " microseconds" << std::endl;
std::cout << "iDynTree/KDL ratio : " << std::endl;
std::cout << "\tRNEA ratio " << iDynTreeTimes.totalTimeRNEA/KDLtimes.totalTimeRNEA << std::endl;
std::cout << "\tCRBA ratio " << iDynTreeTimes.totalTimeCRBA/KDLtimes.totalTimeCRBA << std::endl;
return;
}