Go to the documentation of this file.00001 #include <math.h>
00002 #include "inertiatest.hpp"
00003 #include <frames_io.hpp>
00004 #include <rotationalinertia.hpp>
00005 #include <rigidbodyinertia.hpp>
00006 #include <articulatedbodyinertia.hpp>
00007
00008 #include <Eigen/Core>
00009 CPPUNIT_TEST_SUITE_REGISTRATION( InertiaTest );
00010
00011 using namespace KDL;
00012 using namespace Eigen;
00013
00014 void InertiaTest::setUp()
00015 {
00016 }
00017
00018 void InertiaTest::tearDown()
00019 {
00020 }
00021
00022 void InertiaTest::TestRotationalInertia() {
00023
00024 RotationalInertia I0=RotationalInertia::Zero();
00025 CPPUNIT_ASSERT(Map<Matrix3d>(I0.data).isZero());
00026 RotationalInertia I1;
00027 CPPUNIT_ASSERT(Map<Matrix3d>(I1.data).isZero());
00028 CPPUNIT_ASSERT(Map<Matrix3d>(I0.data).isApprox(Map<Matrix3d>(I1.data)));
00029 RotationalInertia I2(1,2,3,4,5,6);
00030
00031
00032 RotationalInertia I3=I2;
00033 CPPUNIT_ASSERT(Map<Matrix3d>(I3.data).isApprox(Map<Matrix3d>(I2.data)));
00034 I2.data[0]=0.0;
00035 CPPUNIT_ASSERT(!Map<Matrix3d>(I3.data).isApprox(Map<Matrix3d>(I2.data)));
00036
00037
00038 Map<Matrix3d>(I0.data).setRandom();
00039 I1=-2*I0;
00040 CPPUNIT_ASSERT(!Map<Matrix3d>(I1.data).isZero());
00041 I1=I1+I0+I0;
00042 CPPUNIT_ASSERT(Map<Matrix3d>(I1.data).isZero());
00043
00044
00045 CPPUNIT_ASSERT(Map<Matrix3d>(I2.data).isApprox(Map<Matrix3d>(I2.data).transpose()));
00046
00047
00048 Vector omega;
00049 random(omega);
00050 Vector momentum=I2*omega;
00051
00052 CPPUNIT_ASSERT(Map<Vector3d>(momentum.data).isApprox(Map<Matrix3d>(I2.data)*Map<Vector3d>(omega.data)));
00053
00054 }
00055
00056 void InertiaTest::TestRigidBodyInertia() {
00057 RigidBodyInertia I1(0.0);
00058 double mass;
00059 Vector c;
00060 RotationalInertia Ic;
00061 random(mass);
00062 random(c);
00063 Matrix3d Ic_eig = Matrix3d::Random();
00064
00065 Map<Matrix3d>(Ic.data)=Ic_eig+Ic_eig.transpose();
00066 RigidBodyInertia I2(mass,c,Ic);
00067
00068 CPPUNIT_ASSERT_EQUAL(I2.getMass(),mass);
00069 CPPUNIT_ASSERT(!Map<Matrix3d>(I2.getRotationalInertia().data).isZero());
00070 CPPUNIT_ASSERT_EQUAL(I2.getCOG(),c);
00071 CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity()))));
00072
00073 RigidBodyInertia I3=I2;
00074
00075 CPPUNIT_ASSERT_EQUAL(I2.getMass(),I3.getMass());
00076 CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I3.getCOG());
00077 CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I3.getRotationalInertia().data)));
00078
00079 RigidBodyInertia I4=-2*I2 +I3+I3;
00080 CPPUNIT_ASSERT_DOUBLES_EQUAL(I4.getMass(),0.0,epsilon);
00081 CPPUNIT_ASSERT_EQUAL(I4.getCOG(),Vector::Zero());
00082 CPPUNIT_ASSERT(Map<Matrix3d>(I4.getRotationalInertia().data).isZero());
00083
00084
00085
00086
00087 Rotation R;
00088 random(R);
00089 I3 = R*I2;
00090 I4 = R.Inverse()*I3;
00091 CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(), epsilon);
00092 CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
00093 CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
00094
00095 Frame T(R);
00096 I4 = T*I2;
00097 CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(), epsilon);
00098 CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
00099 CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
00100
00101
00102 Vector p;
00103 random(p);
00104 I3 = I2.RefPoint(p);
00105 I4 = I3.RefPoint(-p);
00106 CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
00107 CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
00108 CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
00109 T=Frame(-p);
00110 I4 = T*I2;
00111 CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(),epsilon);
00112 CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
00113 CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
00114
00115 random(T);
00116 I3=T*I2;
00117 I4=T.Inverse()*I3;
00118
00119 Twist a;
00120 random(a);
00121 Wrench f=I2*a;
00122 CPPUNIT_ASSERT_EQUAL(T*f,(T*I2)*(T*a));
00123
00124 random(T);
00125 I3 = T*I2;
00126 I4 = T.Inverse()*I3;
00127 CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
00128 CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
00129 CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
00130
00131 }
00132 void InertiaTest::TestArticulatedBodyInertia() {
00133 double mass;
00134 Vector c;
00135 RotationalInertia Ic;
00136 random(mass);
00137 random(c);
00138 Matrix3d::Map(Ic.data).triangularView<Lower>()= Matrix3d::Random().triangularView<Lower>();
00139 RigidBodyInertia RBI2(mass,c,Ic);
00140 ArticulatedBodyInertia I2(RBI2);
00141 ArticulatedBodyInertia I1(mass,c,Ic);
00142
00143 CPPUNIT_ASSERT_EQUAL(I2.M,I1.M);
00144 CPPUNIT_ASSERT_EQUAL(I2.H,I1.H);
00145 CPPUNIT_ASSERT_EQUAL(I2.I,I1.I);
00146 I1 = ArticulatedBodyInertia(I2);
00147 CPPUNIT_ASSERT_EQUAL(I2.M,I1.M);
00148 CPPUNIT_ASSERT_EQUAL(I2.H,I1.H);
00149 CPPUNIT_ASSERT_EQUAL(I2.I,I1.I);
00150
00151 CPPUNIT_ASSERT_EQUAL(I2.M,(Matrix3d::Identity()*mass).eval());
00152 CPPUNIT_ASSERT(!I2.I.isZero());
00153
00154
00155 ArticulatedBodyInertia I3=I2;
00156
00157 CPPUNIT_ASSERT(I2.M.isApprox(I3.M));
00158 CPPUNIT_ASSERT(I2.H.isApprox(I3.H));
00159 CPPUNIT_ASSERT(I2.I.isApprox(I3.I));
00160
00161 ArticulatedBodyInertia I4=-2*I2 +I3+I3;
00162 CPPUNIT_ASSERT(I4.M.isApprox(Matrix3d::Zero().eval()));
00163 CPPUNIT_ASSERT(I4.H.isApprox(Matrix3d::Zero().eval()));
00164 CPPUNIT_ASSERT(I4.I.isApprox(Matrix3d::Zero().eval()));
00165
00166
00167
00168
00169 Rotation R;
00170 random(R);
00171 I3 = R*I2;
00172 Map<Matrix3d> E(R.data);
00173 Matrix3d tmp = E.transpose()*I2.M*E;
00174 CPPUNIT_ASSERT(I3.M.isApprox(tmp));
00175 tmp = E.transpose()*I2.H*E;
00176 CPPUNIT_ASSERT(I3.H.isApprox(tmp));
00177 tmp = E.transpose()*I2.I*E;
00178 CPPUNIT_ASSERT(I3.I.isApprox(tmp));
00179
00180 I4 = R.Inverse()*I3;
00181 CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
00182 CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
00183 CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
00184
00185 Frame T(R);
00186 I4 = T*I2;
00187 CPPUNIT_ASSERT(I3.M.isApprox(I4.M));
00188 CPPUNIT_ASSERT(I3.H.isApprox(I4.H));
00189 CPPUNIT_ASSERT(I3.I.isApprox(I4.I));
00190
00191
00192 Vector p;
00193 random(p);
00194 I3 = I2.RefPoint(p);
00195 I4 = I3.RefPoint(-p);
00196 CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
00197 CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
00198 CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
00199 T=Frame(-p);
00200 I4 = T*I2;
00201 CPPUNIT_ASSERT(I3.M.isApprox(I4.M));
00202 CPPUNIT_ASSERT(I3.H.isApprox(I4.H));
00203 CPPUNIT_ASSERT(I3.I.isApprox(I4.I));
00204
00205
00206 random(T);
00207 I3=T*I2;
00208 I4=T.Inverse()*I3;
00209
00210 Twist a;
00211 random(a);
00212 Wrench f=I2*a;
00213 CPPUNIT_ASSERT_EQUAL(T*f,(T*I2)*(T*a));
00214
00215 random(T);
00216 I3 = T*I2;
00217 I4 = T.Inverse()*I3;
00218 CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
00219 CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
00220 CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
00221 }