iDynTree::SensorsList generateSensorsTree(const KDL::CoDyCo::UndirectedTree & undirected_tree,
const std::vector<std::string> & ft_names,
const std::vector<bool> & is_measure_direction_child_to_parent)
{
iDynTree::SensorsList sensors_tree;
for(size_t i=0; i < ft_names.size(); i++ )
{
//Creating a new ft sensor to be added in the ft sensors structure
iDynTree::SixAxisForceTorqueSensor new_sens;
if( undirected_tree.getJunction(ft_names[i]) != undirected_tree.getInvalidJunctionIterator() )
{
//Set the sensor name (for the time being equal to the junction name)
new_sens.setName(ft_names[i]);
//Set the junction name
new_sens.setParentJoint(ft_names[i]);
int junction_index = undirected_tree.getJunction(ft_names[i])->getJunctionIndex();
new_sens.setParentJointIndex(junction_index);
KDL::CoDyCo::JunctionMap::const_iterator junct_it = undirected_tree.getJunction(ft_names[i]);
int parent_index = junct_it->getParentLink()->getLinkIndex();
int child_index = junct_it->getChildLink()->getLinkIndex();
std::string parent_link_name = junct_it->getParentLink()->getName();
std::string child_link_name = junct_it->getChildLink()->getName();
if( is_measure_direction_child_to_parent[i] )
{
new_sens.setAppliedWrenchLink(parent_index);
}
else
{
new_sens.setAppliedWrenchLink(child_index);
}
// Currently we support only the case where the ft sensor frame is equal
// to the child link frame
new_sens.setSecondLinkSensorTransform(child_index,iDynTree::Transform::Identity());
new_sens.setSecondLinkName(child_link_name);
// Then, the parent_link_H_sensor transform is simply parent_link_H_child_link transform
KDL::Frame parent_link_H_sensor = junct_it->pose(0.0,false);
new_sens.setFirstLinkSensorTransform(parent_index,iDynTree::ToiDynTree(parent_link_H_sensor));
new_sens.setFirstLinkName(parent_link_name);
}
else
{
std::cerr << "[ERR] DynTree::generateSensorsTree: problem generating sensor for ft "
<< ft_names[i] << std::endl;
assert(false);
}
int ret = sensors_tree.addSensor(new_sens);
assert(ret == i);
}
return sensors_tree;
}
void assertConsistency(std::string modelName)
{
std::cerr << "DynamicsComputationsDynTreeConsistencyUnitTest running on model "
<< modelName << std::endl;
iDynTree::HighLevel::DynamicsComputations dynComp;
iCub::iDynTree::DynTree dynTree;
bool ok = dynComp.loadRobotModelFromFile(modelName);
ok = ok && dynTree.loadURDFModel(modelName);
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_DOUBLE(dynTree.getNrOfDOFs(),dynComp.getNrOfDegreesOfFreedom());
ASSERT_EQUAL_DOUBLE(dynTree.getNrOfFrames(),dynComp.getNrOfFrames());
// Load model with the same joint ordering of the DynTree
iDynTree::ModelLoader mdlLoader;
ok = mdlLoader.loadModelFromFile(modelName);
ASSERT_IS_TRUE(ok);
std::cerr << "Loaded model " << std::endl;
std::vector<std::string> consideredJoints;
KDL::CoDyCo::UndirectedTree undirectedTree = dynTree.getKDLUndirectedTree();
for(int i=0; i < undirectedTree.getNrOfJunctions(); i++)
{
std::string jointName = undirectedTree.getJunction(i)->getName();
// If the joint belong to the new model, include it in the considered joints
if( mdlLoader.model().isJointNameUsed(jointName) )
{
consideredJoints.push_back(jointName);
}
}
iDynTree::ModelLoader mdlLoaderReduced;
ok = mdlLoaderReduced.loadReducedModelFromFullModel(mdlLoader.model(),consideredJoints);
ASSERT_IS_TRUE(ok);
iDynTree::KinDynComputations kinDynComp;
ok = kinDynComp.loadRobotModel(mdlLoaderReduced.model());
ASSERT_IS_TRUE(ok);
std::cerr << "Loaded model in kinDynComp" << std::endl;
Vector6 baseAccKinDyn;
JointDOFsDoubleArray jntAccKinDyn;
setRandomState(dynComp,dynTree,kinDynComp,baseAccKinDyn,jntAccKinDyn);
std::cerr << "Test transforms " << std::endl;
testTransformsConsistency(dynComp,dynTree,kinDynComp);
std::cerr << "Test jacob " << std::endl;
testJacobianConsistency(dynComp,dynTree,kinDynComp);
testRegressorConsistency(dynComp,dynTree);
testCOMConsistency(dynComp,dynTree,kinDynComp);
testMassMatrixConsistency(dynComp,dynTree,kinDynComp);
testInverseDynamicsConsistency(dynComp,kinDynComp,baseAccKinDyn,jntAccKinDyn);
}
