DynamicsUtilitiesTest.cpp

Go to the documentation of this file.

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

class DynamicsUtilitiesTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
    }

    virtual void TearDown()
    {
    }

    DynamicsUtilities du;
};

TEST_F(DynamicsUtilitiesTest, SumWrenchesTest)
{
    std::vector<KDL::Wrench> wrenchVec;

    EXPECT_EQ(KDL::Wrench::Zero(), du.SumWrenches(wrenchVec));

    KDL::Wrench testWrench(KDL::Vector(1.0, 0.0, 2.0), KDL::Vector(0.0, 3.0, 0.0));
    wrenchVec.push_back(testWrench);

    EXPECT_EQ(testWrench, du.SumWrenches(wrenchVec));

    testWrench.force.x(3.0);
    testWrench.torque.z(5.0);
    wrenchVec.push_back(testWrench);

    EXPECT_EQ(KDL::Wrench(KDL::Vector(4.0, 0.0, 4.0), KDL::Vector(0.0, 6.0, 5.0)), du.SumWrenches(wrenchVec));

}

TEST_F(DynamicsUtilitiesTest, isBaseFrameTest)
{
    std::vector<std::string> baseFrames;
    std::string              frame = "link1";

    EXPECT_FALSE(du.isBaseFrame(baseFrames, frame));

    baseFrames.push_back("link2");

    EXPECT_FALSE(du.isBaseFrame(baseFrames, frame));

    baseFrames.push_back(frame);
    EXPECT_TRUE(du.isBaseFrame(baseFrames, frame));

}

TEST_F(DynamicsUtilitiesTest, magTest)
{
    KDL::Vector vec;

    EXPECT_EQ(0.0, du.mag(vec));

    vec.x(2.0);
    EXPECT_EQ(2.0, du.mag(vec));

    vec.y(3.0);
    vec.z(4.0);
    vec.x(0.0);
    EXPECT_EQ(5.0, du.mag(vec));
}

TEST_F(DynamicsUtilitiesTest, MultiplyJacobianTransposeTest)
{
    KDL::Jacobian jac(2);
    KDL::Wrench   wren;
    KDL::JntArray tau, test(2);

    du.MultiplyJacobianTranspose(jac, wren, tau);
    EXPECT_EQ(test, tau);
    // This is a pretty lame test.  Should robustify.
}


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