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;
}
예제 #2
0
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);
}