KdlTreeTrTest.cpp

Go to the documentation of this file.

/*
 * KdlTreeTrTest.cpp
 *
 *  Created on: Jan 13, 2015
 *      Author: Julia Badger
 *
 */

#include <iostream>
#include <gtest/gtest.h>
#include "robodyn_controllers/MotionLimiter.h"
#include "robodyn_controllers/KdlTreeTr.h"
#include <tf/tf.h>

class KdlTreeTrTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        posLimiter.reset(new JointPositionLimiter);
        namePosLimiter.reset(new JointNamePositionLimiter);
        namePosLimiter->setJointPositionLimiter(posLimiter);

        std::string packagePath = ros::package::getPath("robodyn_controllers");
        treeTr.loadFromFile(packagePath + "/test/urdf/BranchingTestRobot.xml");

        treeTr.initialize();
        treeTr.setPositionLimiter(namePosLimiter);
    }

    virtual void TearDown()
    {
    }

    boost::shared_ptr<JointPositionLimiter>     posLimiter;
    boost::shared_ptr<JointNamePositionLimiter> namePosLimiter;
    KdlTreeTr                                   treeTr;
};

TEST_F(KdlTreeTrTest, limitTest)
{
    std::vector<std::string> jointNames;
    jointNames.push_back("joint0");
    jointNames.push_back("joint1");

    std::vector<double> minPosLims(2), maxPosLims(2), velLims(2);
    minPosLims.at(0) = 0;
    minPosLims.at(1) = 0.5;
    maxPosLims.at(0) = 1.0;
    maxPosLims.at(1) = 2.0;
    velLims.at(0) = 2.5;
    velLims.at(1) = 1.0;
    namePosLimiter->setLimits(jointNames, minPosLims, maxPosLims, velLims);

    KDL::JntArray jIn(2), jOut(2), dJoint(2);
    jIn(0) = 0.9; jOut(0) = 0.92; dJoint(0) = 0.0;
    jIn(1) = 0.55; jOut(1) = 0.56; dJoint(1) = 0.0;

    EXPECT_TRUE(treeTr.limitJoints(jIn, jOut, dJoint));
    EXPECT_GT(0.000001, std::abs(dJoint(0)) - 0.004);
    EXPECT_GT(0.000001, std::abs(dJoint(1)) - 0.002);

    jIn(0) = 0.92; jOut(0) = 0.9; dJoint(0) = 0.0;
    dJoint(1) = 0.0;

    EXPECT_FALSE(treeTr.limitJoints(jIn, jOut, dJoint));
    EXPECT_GT(1e-06, std::abs(dJoint(0)) - 0.02);
    EXPECT_GT(1e-06, std::abs(dJoint(1)) - 0.01);

    jIn(0) = 0.5; jOut(0) = 0.53; dJoint(0) = 0.0;

    EXPECT_TRUE(treeTr.limitJoints(jIn, jOut, dJoint));
    EXPECT_GT(1e-06, std::abs(dJoint(0)) - 0.0235);
    EXPECT_GT(1e-06, std::abs(dJoint(1)) - 0.007833333);

    jIn(0) = 0.8; jOut(0) = 0.79; dJoint(0) = 0.0;
    jOut(1) = 0.57; dJoint(1) = 0.0;

    EXPECT_TRUE(treeTr.limitJoints(jIn, jOut, dJoint));
    EXPECT_GT(1e-06, std::abs(dJoint(0)) - 0.005);
    EXPECT_GT(1e-06, std::abs(dJoint(1)) - 0.01);

}

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