MotionLimiter_Test.cpp

Go to the documentation of this file.

/*
 * JointMotionLimiter_Test.cpp
 *
 *  Created on: Sept 07, 2012
 *      Author: Ross Taylor
 *
 */

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

class JointMotionLimiterTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        limiter.reset(new JointMotionLimiter);
        nameLimiter.setJointMotionLimiter(limiter);
        posLimiter.reset(new JointPositionLimiter);
        namePosLimiter.setJointPositionLimiter(posLimiter);
    }

    virtual void TearDown()
    {
    }

    boost::shared_ptr<JointMotionLimiter>   limiter;
    JointNameMotionLimiter                  nameLimiter;
    boost::shared_ptr<JointPositionLimiter> posLimiter;
    JointNamePositionLimiter                namePosLimiter;
};

TEST_F(JointMotionLimiterTest, basicTest)
{
    std::vector<double> jLims1(3);
    std::vector<double> jLims2(3);
    jLims1.at(0) = 1.2;
    jLims1.at(1) = 2.7;
    jLims1.at(2) = 0.34;
    jLims2       = jLims1;
    jLims2.at(2) = 8.6;
    limiter->setLimits(jLims1, jLims2);
    EXPECT_EQ(limiter->getVelocityLimit(2), 0.34);
    EXPECT_EQ(limiter->getAccelerationLimit(2), 8.6);
}

TEST_F(JointMotionLimiterTest, nameTest)
{
    std::vector<std::string> jointNames;
    jointNames.push_back("test1");
    jointNames.push_back("test2");
    jointNames.push_back("test3");

    std::vector<double> jLims(3);
    jLims.at(0) = 1.2;
    jLims.at(1) = 2.7;
    jLims.at(2) = 0.34;
    // add acceleration
    std::vector<double> aLims(3);
    aLims.at(0) = 2.2;
    aLims.at(1) = 1.7;
    aLims.at(2) = 3.2;
    nameLimiter.setLimits(jointNames, jLims, aLims);

    EXPECT_EQ(nameLimiter.getVelocityLimit("test1"), jLims.at(0));
    EXPECT_EQ(nameLimiter.getVelocityLimit("test2"), jLims.at(1));
    EXPECT_EQ(nameLimiter.getVelocityLimit("test3"), jLims.at(2));
    EXPECT_ANY_THROW(nameLimiter.getVelocityLimit("test4"));

    EXPECT_EQ(nameLimiter.getAccelerationLimit("test1"), aLims.at(0));
    EXPECT_EQ(nameLimiter.getAccelerationLimit("test2"), aLims.at(1));
    EXPECT_EQ(nameLimiter.getAccelerationLimit("test3"), aLims.at(2));
    EXPECT_ANY_THROW(nameLimiter.getAccelerationLimit("test4"));

    // test reorder
    jointNames[0] = "test3";
    jointNames[2] = "test1";

    nameLimiter.reorderLimits(jointNames);
    std::vector<double> vLims = limiter->getVelocityLimits();
    EXPECT_EQ(jLims.at(0), vLims.at(2));
    EXPECT_EQ(jLims.at(1), vLims.at(1));
    EXPECT_EQ(jLims.at(2), vLims.at(0));

    jointNames[2] = "test5";
    EXPECT_ANY_THROW(nameLimiter.reorderLimits(jointNames));
}

TEST_F(JointMotionLimiterTest, posTest)
{
    std::vector<double> jLims1(3);
    std::vector<double> jLims2(3);
    jLims1.at(0) = 1.2;
    jLims1.at(1) = 2.7;
    jLims1.at(2) = 0.34;
    jLims2       = jLims1;
    jLims2.at(2) = 8.6;
    posLimiter->setLimits(jLims1, jLims2);
    EXPECT_EQ(posLimiter->getMinPosition(2), 0.34);
    EXPECT_EQ(posLimiter->getMaxPosition(2), 8.6);
}

TEST_F(JointMotionLimiterTest, namePosTest)
{
    std::vector<std::string> jointNames;
    jointNames.push_back("test1");
    jointNames.push_back("test2");
    jointNames.push_back("test3");

    std::vector<double> jLims(3);
    jLims.at(0) = 1.2;
    jLims.at(1) = 2.7;
    jLims.at(2) = 0.34;
    // add acceleration
    std::vector<double> aLims(3);
    aLims.at(0) = 2.2;
    aLims.at(1) = 1.7;
    aLims.at(2) = 3.2;
    namePosLimiter.setLimits(jointNames, jLims, aLims, jLims);

    EXPECT_EQ(namePosLimiter.getMinPosition("test1"), jLims.at(0));
    EXPECT_EQ(namePosLimiter.getMinPosition("test2"), jLims.at(1));
    EXPECT_EQ(namePosLimiter.getMinPosition("test3"), jLims.at(2));
    EXPECT_ANY_THROW(namePosLimiter.getMinPosition("test4"));

    EXPECT_EQ(namePosLimiter.getMaxPosition("test1"), aLims.at(0));
    EXPECT_EQ(namePosLimiter.getMaxPosition("test2"), aLims.at(1));
    EXPECT_EQ(namePosLimiter.getMaxPosition("test3"), aLims.at(2));
    EXPECT_ANY_THROW(namePosLimiter.getMaxPosition("test4"));

    double lim;
    EXPECT_TRUE(namePosLimiter.getMaxVelocity("test1", lim));
    EXPECT_EQ(lim, jLims.at(0));
    EXPECT_TRUE(namePosLimiter.getMaxVelocity("test2", lim));
    EXPECT_EQ(lim, jLims.at(1));
    EXPECT_TRUE(namePosLimiter.getMaxVelocity("test3", lim));
    EXPECT_EQ(lim, jLims.at(2));
    EXPECT_FALSE(namePosLimiter.getMaxVelocity("test4", lim));

    // test reorder
    jointNames[0] = "test3";
    jointNames[2] = "test1";

    namePosLimiter.reorderLimits(jointNames);
    std::vector<double> vLims = posLimiter->getMinPositions();
    EXPECT_EQ(jLims.at(0), vLims.at(2));
    EXPECT_EQ(jLims.at(1), vLims.at(1));
    EXPECT_EQ(jLims.at(2), vLims.at(0));

    jointNames[2] = "test5";
    EXPECT_ANY_THROW(namePosLimiter.reorderLimits(jointNames));
}

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