KdlTreeFkTest.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include "robodyn_controllers/KdlTreeFk.h"

#include <kdl/treefksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <ros/package.h>

class KdlTreeFkTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        std::string packagePath = ros::package::getPath("robodyn_controllers");
        fk.loadFromFile(packagePath + "/test/urdf/BranchingTestRobot.xml");
        tree = fk.getTree();
        srand(time(NULL));
    }

    virtual void TearDown()
    {
    }

    KdlTreeFk fk;
    KDL::Tree tree;
};

TEST_F(KdlTreeFkTest, poseTest)
{
    KDL::TreeFkSolverPos_recursive kdlSolver(tree);
    KDL::JntArray joints(tree.getNrOfJoints());

    for (unsigned int i = 0; i < joints.rows(); ++i)
    {
        joints(i) = ((double)(rand() % 314)) / 100. - 1.57;
    }

    std::map<std::string, KDL::Frame> frames;
    fk.getPoses(joints, frames);

    EXPECT_EQ(tree.getSegments().size(), frames.size());

    KDL::Frame testFrame;

    for (std::map<std::string, KDL::Frame>::const_iterator it = frames.begin(); it != frames.end(); ++it)
    {
        kdlSolver.JntToCart(joints, testFrame, it->first);
        EXPECT_TRUE(Equal(testFrame, it->second));
    }
}

TEST_F(KdlTreeFkTest, velTest)
{
    KDL::Chain               chain;
    KDL::JntArrayVel         joints(tree.getNrOfJoints());
    std::vector<std::string> jointNames;
    fk.getJointNames(jointNames);
    ASSERT_EQ(joints.q.rows(), jointNames.size());

    for (unsigned int i = 0; i < joints.q.rows(); ++i)
    {
        joints.q(i)    = ((double)(rand() % 314)) / 100. - 1.57;
        joints.qdot(i) = ((double)(rand() % 200)) / 2000. - .1;
    }

    std::map<std::string, KDL::FrameVel> frames;
    fk.getVelocities(joints, frames);

    EXPECT_EQ(tree.getSegments().size(), frames.size());

    KDL::FrameVel testFrame;

    for (std::map<std::string, KDL::FrameVel>::const_iterator it = frames.begin(); it != frames.end(); ++it)
    {
        // get chain
        fk.getChain(fk.getBaseName(), it->first, chain);
        // make solver
        KDL::ChainFkSolverVel_recursive kdlSolver(chain);
        // get joints
        KDL::JntArrayVel chainJoints(chain.getNrOfJoints());
        std::vector<std::string> chainJointNames;
        fk.getChainJointNames(it->first, chainJointNames);
        ASSERT_EQ(chainJoints.q.rows(), chainJointNames.size());

        for (unsigned int i = 0; i < chainJoints.q.rows(); ++i)
        {
            std::vector<std::string>::const_iterator nit = std::find(jointNames.begin(), jointNames.end(), chainJointNames[i]);

            if (nit != jointNames.end())
            {
                chainJoints.q(i)    = joints.q(nit - jointNames.begin());
                chainJoints.qdot(i) = joints.qdot(nit - jointNames.begin());
            }
        }

        kdlSolver.JntToCart(chainJoints, testFrame);
        EXPECT_TRUE(Equal(testFrame, it->second));
    }
}

int main(int argc, char** argv)
{
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}