00001 #include <gtest/gtest.h>
00002 #include "robodyn_controllers/KdlTreeId.h"
00003 #include <ros/package.h>
00004
00005 class KdlChainIdRneTest : public ::testing::Test
00006 {
00007 protected:
00008 virtual void SetUp()
00009 {
00010 std::string packagePath = ros::package::getPath("robodyn_controllers");
00011 id.loadFromFile(packagePath + "/test/urdf/SimpleTestRobot.xml");
00012
00013 tree = id.getTree();
00014 velBase = KDL::Twist::Zero();
00015 accelBase = KDL::Twist::Zero();
00016 velOut = KDL::Twist::Zero();
00017 accelOut = KDL::Twist::Zero();
00018
00019 forceBase = KDL::Wrench::Zero();
00020
00021 inertiaBase = KDL::RigidBodyInertia::Zero();
00022 inertiaExt = KDL::RigidBodyInertia::Zero();
00023 inertiaSeg = KDL::RigidBodyInertia::Zero();
00024 }
00025
00026 virtual void TearDown()
00027 {
00028 }
00029
00030 KdlTreeId id;
00031 KDL::Tree tree;
00032 KDL::Twist velBase, accelBase, velOut, accelOut;
00033 KDL::Wrench forceBase;
00034 KDL::RigidBodyInertia inertiaBase, inertiaExt, inertiaSeg;
00035 };
00036
00037
00038 TEST_F(KdlChainIdRneTest, Chain1NoBaseMotionTest)
00039 {
00040 KDL::Chain chain;
00041 tree.getChain("link1", "link2", chain);
00042
00043 KDL::JntArray q(2), q_dot(1), q_dotdot(1);
00044
00045 KdlChainIdRne cid(chain, velBase, accelBase);
00046
00047
00048 ASSERT_EQ(-1, cid.KinematicsPass(q, q_dot, q_dotdot, velOut, accelOut));
00049
00050 q.resize(1);
00051 ASSERT_EQ(0, cid.KinematicsPass(q, q_dot, q_dotdot, velOut, accelOut));
00052 EXPECT_EQ(KDL::Twist::Zero(), velOut);
00053 EXPECT_EQ(KDL::Twist::Zero(), accelOut);
00054
00055
00056 KDL::Wrenches forceExt(1), forceSeg(1);
00057 KDL::JntArray tau(2), Hv(1);
00058
00059 ASSERT_EQ(-1, cid.DynamicsPass(forceExt, tau, forceBase, forceSeg, inertiaExt, inertiaSeg, Hv, inertiaBase));
00060
00061 tau.resize(1);
00062 ASSERT_EQ(0, cid.DynamicsPass(forceExt, tau, forceBase, forceSeg, inertiaExt, inertiaSeg, Hv, inertiaBase));
00063 EXPECT_EQ(0.0, tau(0));
00064 EXPECT_EQ(KDL::Wrench::Zero(), forceBase);
00065
00066 KDL::Frame X = chain.getSegment(0).pose(0);
00067 KDL::RigidBodyInertia Ix = X * chain.getSegment(0).getInertia();
00068 EXPECT_EQ(Ix.getMass(), inertiaBase.getMass());
00069 EXPECT_EQ(Ix.getCOG(), inertiaBase.getCOG());
00070 EXPECT_EQ(Ix.getRotationalInertia().data[0], inertiaBase.getRotationalInertia().data[0]);
00071 EXPECT_EQ(Ix.getRotationalInertia().data[1], inertiaBase.getRotationalInertia().data[1]);
00072 EXPECT_EQ(Ix.getRotationalInertia().data[2], inertiaBase.getRotationalInertia().data[2]);
00073 EXPECT_EQ(Ix.getRotationalInertia().data[4], inertiaBase.getRotationalInertia().data[4]);
00074 EXPECT_EQ(Ix.getRotationalInertia().data[5], inertiaBase.getRotationalInertia().data[5]);
00075 EXPECT_EQ(Ix.getRotationalInertia().data[8], inertiaBase.getRotationalInertia().data[8]);
00076
00077 KDL::Twist S = X.M.Inverse(chain.getSegment(0).twist(0.0, 1.0));
00078 KDL::Wrench Fisx = chain.getSegment(0).getInertia() * S;
00079 double Hvx = dot(S, Fisx);
00080 EXPECT_EQ(Hvx, Hv(0));
00081
00082 }
00083
00085 TEST_F(KdlChainIdRneTest, Chain2NoBaseMotionTest)
00086 {
00087 KDL::Chain chain;
00088 tree.getChain("link1", "link4", chain);
00089
00090 KDL::JntArray q(3), q_dot(2), q_dotdot(2);
00091
00092 KdlChainIdRne cid(chain, velBase, accelBase);
00093
00094 ASSERT_EQ(-1, cid.KinematicsPass(q, q_dot, q_dotdot, velOut, accelOut));
00095
00096 q.resize(2);
00097 ASSERT_EQ(0, cid.KinematicsPass(q, q_dot, q_dotdot, velOut, accelOut));
00098 EXPECT_NEAR(0, velOut(0), 1e-12);
00099 EXPECT_NEAR(0, velOut(1), 1e-12);
00100 EXPECT_NEAR(0, velOut(2), 1e-12);
00101 EXPECT_NEAR(0, velOut(3), 1e-12);
00102 EXPECT_NEAR(0, velOut(4), 1e-12);
00103 EXPECT_NEAR(0, velOut(5), 1e-12);
00104 EXPECT_NEAR(0, accelOut(0), 1e-12);
00105 EXPECT_NEAR(0, accelOut(1), 1e-12);
00106 EXPECT_NEAR(0, accelOut(2), 1e-12);
00107 EXPECT_NEAR(0, accelOut(3), 1e-12);
00108 EXPECT_NEAR(0, accelOut(4), 1e-12);
00109 EXPECT_NEAR(0, accelOut(5), 1e-12);
00110
00111
00112
00113 KDL::Wrenches forceExt(2), forceSeg(2);
00114 KDL::JntArray tau(3), Hv(2);
00115
00116 ASSERT_EQ(-1, cid.DynamicsPass(forceExt, tau, forceBase, forceSeg, inertiaExt, inertiaSeg, Hv, inertiaBase));
00117
00118 tau.resize(2);
00119 ASSERT_EQ(0, cid.DynamicsPass(forceExt, tau, forceBase, forceSeg, inertiaExt, inertiaSeg, Hv, inertiaBase));
00120 EXPECT_LT(abs(tau(0)), 1e-12);
00121 EXPECT_LT(abs(tau(1)), 1e-12);
00122 EXPECT_EQ(KDL::Wrench::Zero(), forceBase);
00123
00124 KDL::Frame X1 = chain.getSegment(1).pose(0);
00125 KDL::Frame X0 = chain.getSegment(0).pose(0);
00126 KDL::RigidBodyInertia Ix1 = X1 * chain.getSegment(1).getInertia();
00127 KDL::RigidBodyInertia Ix0 = X0 * (chain.getSegment(0).getInertia() + Ix1);
00128 EXPECT_NEAR(Ix0.getMass(), inertiaBase.getMass(), 1e-12);
00129 EXPECT_EQ(Ix0.getCOG(), inertiaBase.getCOG());
00130 EXPECT_NEAR(Ix0.getRotationalInertia().data[0], inertiaBase.getRotationalInertia().data[0], 1e-12);
00131 EXPECT_NEAR(Ix0.getRotationalInertia().data[1], inertiaBase.getRotationalInertia().data[1], 1e-12);
00132 EXPECT_NEAR(Ix0.getRotationalInertia().data[2], inertiaBase.getRotationalInertia().data[2], 1e-12);
00133 EXPECT_NEAR(Ix0.getRotationalInertia().data[4], inertiaBase.getRotationalInertia().data[4], 1e-12);
00134 EXPECT_NEAR(Ix0.getRotationalInertia().data[5], inertiaBase.getRotationalInertia().data[5], 1e-12);
00135 EXPECT_NEAR(Ix0.getRotationalInertia().data[8], inertiaBase.getRotationalInertia().data[8], 1e-12);
00136
00137 KDL::Twist S = X1.M.Inverse(chain.getSegment(1).twist(0.0, 1.0));
00138 KDL::Wrench Fisx = chain.getSegment(1).getInertia() * S;
00139 double Hvx = dot(S, Fisx);
00140 EXPECT_EQ(Hvx, Hv(1));
00141
00142
00143 }
00144
00145 TEST_F(KdlChainIdRneTest, Chain1GravityTest)
00146 {
00147 KDL::Chain chain;
00148 tree.getChain("link1", "link2", chain);
00149
00150 KDL::JntArray q(1), q_dot(1), q_dotdot(1);
00151
00152 accelBase = KDL::Twist(KDL::Vector(0, -9.8, 0), KDL::Vector::Zero());
00153
00154 KdlChainIdRne cid(chain, velBase, accelBase);
00155
00156 ASSERT_EQ(0, cid.KinematicsPass(q, q_dot, q_dotdot, velOut, accelOut));
00157
00158 KDL::Frame X = chain.getSegment(0).pose(0);
00159 KDL::Twist ax = X.Inverse(accelBase);
00160
00161 EXPECT_EQ(KDL::Twist::Zero(), velOut);
00162 EXPECT_EQ(ax, accelOut);
00163
00164
00165 KDL::Wrenches forceExt(1), forceSeg(1);
00166 KDL::JntArray tau(1), Hv(1);
00167
00168 ASSERT_EQ(0, cid.DynamicsPass(forceExt, tau, forceBase, forceSeg, inertiaExt, inertiaSeg, Hv, inertiaBase));
00169
00170 KDL::Wrench fx = chain.getSegment(0).getInertia() * ax;
00171 KDL::Twist S = X.M.Inverse(chain.getSegment(0).twist(0.0, 1.0));
00172 EXPECT_EQ(dot(S, fx), tau(0));
00173 EXPECT_EQ(X * fx, forceBase);
00174
00175 KDL::RigidBodyInertia Ix = X * chain.getSegment(0).getInertia();
00176 EXPECT_EQ(Ix.getMass(), inertiaBase.getMass());
00177 EXPECT_EQ(Ix.getCOG(), inertiaBase.getCOG());
00178 EXPECT_EQ(Ix.getRotationalInertia().data[0], inertiaBase.getRotationalInertia().data[0]);
00179 EXPECT_EQ(Ix.getRotationalInertia().data[1], inertiaBase.getRotationalInertia().data[1]);
00180 EXPECT_EQ(Ix.getRotationalInertia().data[2], inertiaBase.getRotationalInertia().data[2]);
00181 EXPECT_EQ(Ix.getRotationalInertia().data[4], inertiaBase.getRotationalInertia().data[4]);
00182 EXPECT_EQ(Ix.getRotationalInertia().data[5], inertiaBase.getRotationalInertia().data[5]);
00183 EXPECT_EQ(Ix.getRotationalInertia().data[8], inertiaBase.getRotationalInertia().data[8]);
00184
00185 KDL::Wrench Fisx = chain.getSegment(0).getInertia() * S;
00186 double Hvx = dot(S, Fisx);
00187 EXPECT_EQ(Hvx, Hv(0));
00188
00189 }
00190
00191
00192
00193 int main(int argc, char** argv)
00194 {
00195 ::testing::InitGoogleTest(&argc, argv);
00196 return RUN_ALL_TESTS();
00197 }