KdlTreeIkTest.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include "robodyn_controllers/KdlTreeIk.h"
#include <ros/package.h>
#include "robodyn_controllers/MinJerkTrajectoryFactory.h"

bool outputFlag = false;

class KdlTreeIkTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        std::string packagePath = ros::package::getPath("robodyn_controllers");
        ik.loadFromFile(packagePath + "/test/urdf/r2c_legs_control.urdf.xml");
        fk.loadFromFile(packagePath + "/test/urdf/r2c_legs_control.urdf.xml");

        const double TORAD = 3.14159 / 180.;
        jointsInMap.insert(std::make_pair("/r2/left_leg/joint3", 147 * TORAD));
        jointsInMap.insert(std::make_pair("/r2/left_leg/joint5", 147 * TORAD));
        jointsInMap.insert(std::make_pair("/r2/right_leg/joint3", 147 * TORAD));
        jointsInMap.insert(std::make_pair("/r2/right_leg/joint5", 147 * TORAD));

        jointsOutMap.insert(std::make_pair("/r2/left_leg/joint3", 145 * TORAD));
        jointsOutMap.insert(std::make_pair("/r2/left_leg/joint5", 145 * TORAD));
        jointsOutMap.insert(std::make_pair("/r2/right_leg/joint3", 145 * TORAD));
        jointsOutMap.insert(std::make_pair("/r2/right_leg/joint5", 145 * TORAD));

        ik.getJointNames(jointNames);
        ik.setJointLimits(jointNames, std::vector<double>(jointNames.size(), -3.14), std::vector<double>(jointNames.size(), 3.14));
        joints_in.resize(jointNames.size());
        joints_out.resize(jointNames.size());
        std::map<std::string, double>::const_iterator it;

        for (unsigned int i = 0; i < jointNames.size(); ++i)
        {
            it = jointsInMap.find(jointNames[i]);

            if (it == jointsInMap.end())
            {
                joints_in.q(i) = 0.;
            }
            else
            {
                joints_in.q(i) = it->second;
            }

            joints_in.qdot(i) = 0.;

            it = jointsOutMap.find(jointNames[i]);

            if (it == jointsOutMap.end())
            {
                joints_out(i) = 0.;
            }
            else
            {
                joints_out(i) = it->second;
            }
        }

        nodeNames.push_back("/r2/left_leg/gripper/tip");
        nodeNames.push_back("/r2/right_leg/gripper/tip");

        for (unsigned int i = 0; i < nodeNames.size(); ++i)
        {
            frames_out.insert(std::make_pair(nodeNames[i], KDL::Frame()));
        }

        fk.getPoses(joints_out, frames_out);
    }

    virtual void TearDown()
    {
    }

    KdlTreeIk                         ik;
    KdlTreeFk                         fk;
    std::map<std::string, double>     jointsInMap;
    std::map<std::string, double>     jointsOutMap;
    std::vector<std::string>          jointNames;
    KDL::JntArrayVel                  joints_in;
    KDL::JntArray                     joints_out;
    std::vector<std::string>          nodeNames;
    std::map<std::string, KDL::Frame> frames_out;
};

TEST_F(KdlTreeIkTest, jointPosTest)
{
    std::vector<KDL::Frame> nodeFrames(nodeNames.size());

    for (unsigned int i = 0; i < nodeNames.size(); ++i)
    {
        nodeFrames[i] =  frames_out[nodeNames[i]];
    }

    std::vector<KdlTreeIk::NodePriority> priorities(nodeNames.size() * 6, KdlTreeIk::CRITICAL);
    KDL::JntArray joints_test;
    EXPECT_NO_THROW(ik.getJointPositions(joints_in.q, nodeNames, nodeFrames, joints_test, priorities));

    std::map<std::string, KDL::Frame> frames_test;
    fk.getPoses(joints_test, frames_test);

    EXPECT_TRUE(KDL::Equal(frames_out["/r2/left_leg/gripper/tip"], frames_test["/r2/left_leg/gripper/tip"], 1E-3));
    EXPECT_TRUE(KDL::Equal(frames_out["/r2/right_leg/gripper/tip"], frames_test["/r2/right_leg/gripper/tip"], 1E-3));
}

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

    for (int i = 0; i < argc; ++i)
    {
        std::cout << argv[i] << std::endl;

        if (strcmp(argv[i], "--outputTraj") == 0)
        {
            outputFlag = true;
        }
    }

    return RUN_ALL_TESTS();
}