Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 5
0
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;
}