C++ ModelLoader::model方法代码示例

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);
    }

//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........