orocos_kdl: orocos_kdl: inertiatest.cpp Source 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     //Check if construction works fine
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     //Check if copying works fine
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     //Check if addition and multiplication works fine
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     //Check if matrix is symmetric
00045     CPPUNIT_ASSERT(Map<Matrix3d>(I2.data).isApprox(Map<Matrix3d>(I2.data).transpose()));
00046 
00047     //Check if angular momentum is correctly calculated:
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     //make it symmetric:
00065     Map<Matrix3d>(Ic.data)=Ic_eig+Ic_eig.transpose();
00066     RigidBodyInertia I2(mass,c,Ic);
00067     //Check if construction works fine
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     //check if copying works fine
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     //Check if multiplication and addition works fine
00079     RigidBodyInertia I4=-2*I2 +I3+I3;
00080     CPPUNIT_ASSERT_EQUAL(I4.getMass(),0.0);
00081     CPPUNIT_ASSERT_EQUAL(I4.getCOG(),Vector::Zero());
00082     CPPUNIT_ASSERT(Map<Matrix3d>(I4.getRotationalInertia().data).isZero());
00083     
00084     //Check if transformations work fine
00085     //Check only rotation transformation
00086     //back and forward
00087     Rotation R;
00088     random(R);
00089     I3 = R*I2;
00090     I4 = R.Inverse()*I3;
00091     CPPUNIT_ASSERT_EQUAL(I2.getMass(),I4.getMass());
00092     CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
00093     CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
00094     //rotation and total with p=0
00095     Frame T(R);
00096     I4 = T*I2;
00097     CPPUNIT_ASSERT_EQUAL(I3.getMass(),I4.getMass());
00098     CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
00099     CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
00100     
00101     //Check only transformation
00102     Vector p;
00103     random(p);
00104     I3 = I2.RefPoint(p);
00105     I4 = I3.RefPoint(-p);
00106     CPPUNIT_ASSERT_EQUAL(I2.getMass(),I4.getMass());
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_EQUAL(I3.getMass(),I4.getMass());
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_EQUAL(I2.getMass(),I4.getMass());
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     //Check if construction works fine
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     //CPPUNIT_ASSERT(I2.I.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()))));
00154     //CPPUNIT_ASSERT(I2.H.isApprox(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity())));
00155     ArticulatedBodyInertia I3=I2;
00156     //check if copying works fine
00157     CPPUNIT_ASSERT_EQUAL(I2.M,I3.M);
00158     CPPUNIT_ASSERT_EQUAL(I2.H,I3.H);
00159     CPPUNIT_ASSERT_EQUAL(I2.I,I3.I);
00160     //Check if multiplication and addition works fine
00161     ArticulatedBodyInertia I4=-2*I2 +I3+I3;
00162     CPPUNIT_ASSERT_EQUAL(I4.M,Matrix3d::Zero().eval());
00163     CPPUNIT_ASSERT_EQUAL(I4.H,Matrix3d::Zero().eval());
00164     CPPUNIT_ASSERT_EQUAL(I4.I,Matrix3d::Zero().eval());
00165     
00166     //Check if transformations work fine
00167     //Check only rotation transformation
00168     //back and forward
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_EQUAL(I3.H,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     //rotation and total with p=0
00185     Frame T(R);
00186     I4 = T*I2;
00187     CPPUNIT_ASSERT_EQUAL(I3.M,I4.M);
00188     CPPUNIT_ASSERT_EQUAL(I3.H,I4.H);
00189     CPPUNIT_ASSERT_EQUAL(I3.I,I4.I);
00190 
00191     //Check only transformation
00192     Vector p;
00193     random(p);
00194     I3 = I2.RefPoint(p);
00195     I4 = I3.RefPoint(-p);
00196     CPPUNIT_ASSERT_EQUAL(I2.M,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_EQUAL(I3.M,I4.M);
00202     CPPUNIT_ASSERT_EQUAL(I3.H,I4.H);
00203     CPPUNIT_ASSERT_EQUAL(I3.I,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 }