JointDynamicsDataTest.cpp

Go to the documentation of this file.

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

class JointDynamicsDataTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        std::string packagePath = ros::package::getPath("robodyn_controllers");
        id.loadFromFile(packagePath + "/test/urdf/SimpleTestRobot.xml");
        tree = id.getTree();
    }

    virtual void TearDown()
    {
    }

    KdlTreeId         id;
    KDL::Tree         tree;
    JointDynamicsData jd;
    JntDynData        data;
};

TEST_F(JointDynamicsDataTest, InitializeMapsTest)
{
    // Initialize maps with blank tree
    KDL::Tree blankTree;
    jd.InitializeMaps(blankTree);

    EXPECT_FALSE(jd.extForceMap.empty()) ;
    EXPECT_FALSE(jd.segForceMap.empty()) ; // Empty trees still have root
    // segments
    EXPECT_TRUE(jd.jointDataMap.empty());
    EXPECT_TRUE(jd.jointTorqueCommandMap.empty());
    EXPECT_TRUE(jd.jointInertiaMap.empty());

    // Initialize maps with test tree
    jd.InitializeMaps(tree);

    ASSERT_FALSE(                   jd.segForceMap.empty());
    EXPECT_EQ (1,                   jd.segForceMap.count("link1"));
    EXPECT_EQ (1,                   jd.segForceMap.count("link4"));
    EXPECT_EQ (KDL::Wrench::Zero(), jd.segForceMap["link3"]);

    ASSERT_FALSE( jd.jointTorqueCommandMap.empty());
    EXPECT_EQ (1, jd.jointTorqueCommandMap.count("joint1"));
    EXPECT_EQ (0, jd.jointTorqueCommandMap["joint3"]);
    EXPECT_EQ (0, jd.jointTorqueCommandMap.count("link2"));
}

TEST_F(JointDynamicsDataTest, PopulateJointInfoTest)
{
    jd.InitializeMaps(tree);
    KDL::Chain chain1, chain2;
    tree.getChain("link1", "link2", chain1);
    tree.getChain("link1", "link4", chain2);

    KDL::JntArray q1(chain1.getNrOfJoints());
    KDL::JntArray q1_dot(chain1.getNrOfJoints());
    KDL::JntArray q1_dotdot(chain1.getNrOfJoints());
    KDL::JntArray q2(chain2.getNrOfJoints());
    KDL::JntArray q2_dot(chain2.getNrOfJoints());
    KDL::JntArray q2_dotdot(chain2.getNrOfJoints());

    data.pos                  = 2.0;
    data.vel                  = 1.0;
    data.acc                  = 3.0;
    jd.jointDataMap["joint1"] = data;
    jd.jointDataMap["joint3"] = data;

    EXPECT_EQ(-1,  jd.PopulateJointInfo(chain2, q1, q1_dot, q1_dotdot));
    ASSERT_EQ(0,   jd.PopulateJointInfo(chain1, q1, q1_dot, q1_dotdot));
    EXPECT_EQ(2.0, q1(0) );
    EXPECT_EQ(1.0, q1_dot(0));
    EXPECT_EQ(3.0, q1_dotdot(0));

    ASSERT_EQ(0,   jd.PopulateJointInfo(chain2, q2, q2_dot, q2_dotdot));
    EXPECT_EQ(0.0, q2(0));
    EXPECT_EQ(1.0, q2_dot(1));
    EXPECT_EQ(0.0, q2_dotdot(0));

}

TEST_F(JointDynamicsDataTest, PopulateExtForceInfoTest)
{
    jd.InitializeMaps(tree);
    KDL::Chain chain1, chain2;
    tree.getChain("link1", "link2", chain1);
    tree.getChain("link1", "link4", chain2);

    KDL::Wrenches f1(chain1.getNrOfSegments());
    KDL::Wrenches f2(chain2.getNrOfSegments());

    KDL::Vector force(1.0, 0.0, 0.0), torque(0.0, 2.0, 1.0);
    KDL::Wrench testW(force, torque);
    jd.extForceMap["link3"] = testW;

    EXPECT_EQ(-1, jd.PopulateExtForceInfo(chain1, f2));
    ASSERT_EQ(0,  jd.PopulateExtForceInfo(chain1, f1));
    EXPECT_EQ(KDL::Wrench::Zero(), f1[0]);

    ASSERT_EQ(0, jd.PopulateExtForceInfo(chain2, f2));
    EXPECT_EQ(testW, f2[0]);
    EXPECT_EQ(KDL::Wrench::Zero(), f2[1]);

}

TEST_F(JointDynamicsDataTest, StoreJointTorquesTest)
{
    jd.InitializeMaps(tree);
    KDL::Chain chain1, chain2;
    tree.getChain("link1", "link2", chain1);
    tree.getChain("link1", "link4", chain2);

    KDL::JntArray tau1(chain1.getNrOfJoints());
    KDL::JntArray tau2(chain2.getNrOfJoints());

    tau1(0) = 2.0;
    tau2(0) = 1.0;
    tau2(1) = 3.0;

    EXPECT_EQ(-1, jd.StoreJointTorqueCommands(chain1, tau2));
    ASSERT_EQ(0,  jd.StoreJointTorqueCommands(chain1, tau1));
    ASSERT_EQ(0,  jd.StoreJointTorqueCommands(chain2, tau2));

    EXPECT_EQ(-2.0, jd.jointTorqueCommandMap["joint1"]);
    EXPECT_EQ(-1.0, jd.jointTorqueCommandMap["joint2"]);
    EXPECT_EQ(-3.0, jd.jointTorqueCommandMap["joint3"]);

}

TEST_F(JointDynamicsDataTest, StoreJointInertiaTest)
{
    jd.InitializeMaps(tree);
    KDL::Chain chain1, chain2;
    tree.getChain("link1", "link2", chain1);
    tree.getChain("link1", "link4", chain2);

    KDL::JntArray Hv1(chain1.getNrOfJoints());
    KDL::JntArray Hv2(chain2.getNrOfJoints());

    Hv1(0) = 2.0;
    Hv2(0) = 1.0;
    Hv2(1) = 3.0;

    EXPECT_EQ(-1, jd.StoreJointInertia(chain2, Hv1));
    ASSERT_EQ(0,  jd.StoreJointInertia(chain1, Hv1));
    ASSERT_EQ(0,  jd.StoreJointInertia(chain2, Hv2));

    EXPECT_EQ(2.0, jd.jointInertiaMap["joint1"]);
    EXPECT_EQ(1.0, jd.jointInertiaMap["joint2"]);
    EXPECT_EQ(3.0, jd.jointInertiaMap["joint3"]);

}

TEST_F(JointDynamicsDataTest, StoreSegmentWrenchesTest)
{
    jd.InitializeMaps(tree);
    KDL::Chain chain1, chain2;
    tree.getChain("link1", "link2", chain1);
    tree.getChain("link1", "link4", chain2);

    KDL::Wrenches f1(chain1.getNrOfSegments());
    KDL::Wrenches f2(chain2.getNrOfSegments());

    KDL::Vector force(1.0, 0.0, 0.0), torque(0.0, 2.0, 1.0);
    KDL::Wrench testW(force, torque);

    f1[0] = testW;
    f2[1] = testW;

    EXPECT_EQ(-1, jd.StoreSegmentWrenches(chain2, f1));
    ASSERT_EQ(0,  jd.StoreSegmentWrenches(chain1, f1));
    ASSERT_EQ(0,  jd.StoreSegmentWrenches(chain2, f2));

    EXPECT_EQ(testW,               jd.segForceMap["link2"]);
    EXPECT_EQ(KDL::Wrench::Zero(), jd.segForceMap["link3"]);
    EXPECT_EQ(testW,               jd.segForceMap["link4"]);

}

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