inertiatest.cpp
Go to the documentation of this file.
1 #include <math.h>
2 #include "inertiatest.hpp"
3 #include <frames_io.hpp>
4 #include <rotationalinertia.hpp>
5 #include <rigidbodyinertia.hpp>
6 #include <articulatedbodyinertia.hpp>
7 
8 #include <Eigen/Core>
9 CPPUNIT_TEST_SUITE_REGISTRATION( InertiaTest );
10 
11 using namespace KDL;
12 using namespace Eigen;
13 
14 void InertiaTest::setUp()
15 {
16 }
17 
18 void InertiaTest::tearDown()
19 {
20 }
21 
22 void InertiaTest::TestRotationalInertia() {
23  //Check if construction works fine
24  RotationalInertia I0=RotationalInertia::Zero();
25  CPPUNIT_ASSERT(Map<Matrix3d>(I0.data).isZero());
26  RotationalInertia I1;
27  CPPUNIT_ASSERT(Map<Matrix3d>(I1.data).isZero());
28  CPPUNIT_ASSERT(Map<Matrix3d>(I0.data).isApprox(Map<Matrix3d>(I1.data)));
29  RotationalInertia I2(1,2,3,4,5,6);
30 
31  //Check if copying works fine
32  RotationalInertia I3=I2;
33  CPPUNIT_ASSERT(Map<Matrix3d>(I3.data).isApprox(Map<Matrix3d>(I2.data)));
34  I2.data[0]=0.0;
35  CPPUNIT_ASSERT(!Map<Matrix3d>(I3.data).isApprox(Map<Matrix3d>(I2.data)));
36 
37  //Check if addition and multiplication works fine
38  Map<Matrix3d>(I0.data).setRandom();
39  I1=-2*I0;
40  CPPUNIT_ASSERT(!Map<Matrix3d>(I1.data).isZero());
41  I1=I1+I0+I0;
42  CPPUNIT_ASSERT(Map<Matrix3d>(I1.data).isZero());
43 
44  //Check if matrix is symmetric
45  CPPUNIT_ASSERT(Map<Matrix3d>(I2.data).isApprox(Map<Matrix3d>(I2.data).transpose()));
46 
47  //Check if angular momentum is correctly calculated:
48  Vector omega;
49  random(omega);
50  Vector momentum=I2*omega;
51 
52  CPPUNIT_ASSERT(Map<Vector3d>(momentum.data).isApprox(Map<Matrix3d>(I2.data)*Map<Vector3d>(omega.data)));
53 
54 }
55 
56 void InertiaTest::TestRigidBodyInertia() {
57  RigidBodyInertia I1(0.0);
58  double mass;
59  Vector c;
60  RotationalInertia Ic;
61  random(mass);
62  random(c);
63  Matrix3d Ic_eig = Matrix3d::Random();
64  //make it symmetric:
65  Map<Matrix3d>(Ic.data)=Ic_eig+Ic_eig.transpose();
66  RigidBodyInertia I2(mass,c,Ic);
67  //Check if construction works fine
68  CPPUNIT_ASSERT_EQUAL(I2.getMass(),mass);
69  CPPUNIT_ASSERT(!Map<Matrix3d>(I2.getRotationalInertia().data).isZero());
70  CPPUNIT_ASSERT_EQUAL(I2.getCOG(),c);
71  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()))));
72 
73  RigidBodyInertia I3=I2;
74  //check if copying works fine
75  CPPUNIT_ASSERT_EQUAL(I2.getMass(),I3.getMass());
76  CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I3.getCOG());
77  CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I3.getRotationalInertia().data)));
78  //Check if multiplication and addition works fine
79  RigidBodyInertia I4=-2*I2 +I3+I3;
80  CPPUNIT_ASSERT_DOUBLES_EQUAL(I4.getMass(),0.0,epsilon);
81  CPPUNIT_ASSERT_EQUAL(I4.getCOG(),Vector::Zero());
82  CPPUNIT_ASSERT(Map<Matrix3d>(I4.getRotationalInertia().data).isZero());
83 
84  //Check if transformations work fine
85  //Check only rotation transformation
86  //back and forward
87  Rotation R;
88  random(R);
89  I3 = R*I2;
90  I4 = R.Inverse()*I3;
91  CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(), epsilon);
92  CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
93  CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
94  //rotation and total with p=0
95  Frame T(R);
96  I4 = T*I2;
97  CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(), epsilon);
98  CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
99  CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
100 
101  //Check only transformation
102  Vector p;
103  random(p);
104  I3 = I2.RefPoint(p);
105  I4 = I3.RefPoint(-p);
106  CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
107  CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
108  CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
109  T=Frame(-p);
110  I4 = T*I2;
111  CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(),epsilon);
112  CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
113  CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
114 
115  random(T);
116  I3=T*I2;
117  I4=T.Inverse()*I3;
118 
119  Twist a;
120  random(a);
121  Wrench f=I2*a;
122  CPPUNIT_ASSERT_EQUAL(T*f,(T*I2)*(T*a));
123 
124  random(T);
125  I3 = T*I2;
126  I4 = T.Inverse()*I3;
127  CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
128  CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
129  CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
130 
131 }
132 void InertiaTest::TestArticulatedBodyInertia() {
133  double mass;
134  Vector c;
135  RotationalInertia Ic;
136  random(mass);
137  random(c);
138  Matrix3d::Map(Ic.data).triangularView<Lower>()= Matrix3d::Random().triangularView<Lower>();
139  RigidBodyInertia RBI2(mass,c,Ic);
140  ArticulatedBodyInertia I2(RBI2);
141  ArticulatedBodyInertia I1(mass,c,Ic);
142  //Check if construction works fine
143  CPPUNIT_ASSERT_EQUAL(I2.M,I1.M);
144  CPPUNIT_ASSERT_EQUAL(I2.H,I1.H);
145  CPPUNIT_ASSERT_EQUAL(I2.I,I1.I);
146  I1 = ArticulatedBodyInertia(I2);
147  CPPUNIT_ASSERT_EQUAL(I2.M,I1.M);
148  CPPUNIT_ASSERT_EQUAL(I2.H,I1.H);
149  CPPUNIT_ASSERT_EQUAL(I2.I,I1.I);
150 
151  CPPUNIT_ASSERT_EQUAL(I2.M,(Matrix3d::Identity()*mass).eval());
152  CPPUNIT_ASSERT(!I2.I.isZero());
153  //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())),epsilon));
154  //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()),epsilon));
155  ArticulatedBodyInertia I3=I2;
156  //check if copying works fine
157  CPPUNIT_ASSERT(I2.M.isApprox(I3.M));
158  CPPUNIT_ASSERT(I2.H.isApprox(I3.H));
159  CPPUNIT_ASSERT(I2.I.isApprox(I3.I));
160  //Check if multiplication and addition works fine
161  ArticulatedBodyInertia I4=-2*I2 +I3+I3;
162  CPPUNIT_ASSERT(I4.M.isApprox(Matrix3d::Zero().eval()));
163  CPPUNIT_ASSERT(I4.H.isApprox(Matrix3d::Zero().eval()));
164  CPPUNIT_ASSERT(I4.I.isApprox(Matrix3d::Zero().eval()));
165 
166  //Check if transformations work fine
167  //Check only rotation transformation
168  //back and forward
169  Rotation R;
170  random(R);
171  I3 = R*I2;
172  Map<Matrix3d> E(R.data);
173  Matrix3d tmp = E.transpose()*I2.M*E;
174  CPPUNIT_ASSERT(I3.M.isApprox(tmp));
175  tmp = E.transpose()*I2.H*E;
176  CPPUNIT_ASSERT(I3.H.isApprox(tmp));
177  tmp = E.transpose()*I2.I*E;
178  CPPUNIT_ASSERT(I3.I.isApprox(tmp));
179 
180  I4 = R.Inverse()*I3;
181  CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
182  CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
183  CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
184  //rotation and total with p=0
185  Frame T(R);
186  I4 = T*I2;
187  CPPUNIT_ASSERT(I3.M.isApprox(I4.M));
188  CPPUNIT_ASSERT(I3.H.isApprox(I4.H));
189  CPPUNIT_ASSERT(I3.I.isApprox(I4.I));
190 
191  //Check only transformation
192  Vector p;
193  random(p);
194  I3 = I2.RefPoint(p);
195  I4 = I3.RefPoint(-p);
196  CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
197  CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
198  CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
199  T=Frame(-p);
200  I4 = T*I2;
201  CPPUNIT_ASSERT(I3.M.isApprox(I4.M));
202  CPPUNIT_ASSERT(I3.H.isApprox(I4.H));
203  CPPUNIT_ASSERT(I3.I.isApprox(I4.I));
204 
205 
206  random(T);
207  I3=T*I2;
208  I4=T.Inverse()*I3;
209 
210  Twist a;
211  random(a);
212  Wrench f=I2*a;
213  CPPUNIT_ASSERT_EQUAL(T*f,(T*I2)*(T*a));
214 
215  random(T);
216  I3 = T*I2;
217  I4 = T.Inverse()*I3;
218  CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
219  CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
220  CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
221 }
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
InertiaTest::TestArticulatedBodyInertia
void TestArticulatedBodyInertia()
Definition: inertiatest.cpp:132
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.hpp:633
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
InertiaTest::TestRigidBodyInertia
void TestRigidBodyInertia()
Definition: inertiatest.cpp:56
frames_io.hpp
KDL::dot
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.hpp:138
InertiaTest::tearDown
void tearDown()
Definition: inertiatest.cpp:18
CPPUNIT_TEST_SUITE_REGISTRATION
CPPUNIT_TEST_SUITE_REGISTRATION(InertiaTest)
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::ArticulatedBodyInertia
6D Inertia of a articulated body
Definition: articulatedbodyinertia.hpp:40
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
KDL::Vector::data
double data[3]
Definition: frames.hpp:163
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
KDL::RotationalInertia::Zero
static RotationalInertia Zero()
Definition: rotationalinertia.hpp:39
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:878
KDL::RigidBodyInertia::getRotationalInertia
RotationalInertia getRotationalInertia() const
Definition: rigidbodyinertia.hpp:86
KDL::random
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1208
KDL::Vector::Zero
static Vector Zero()
Definition: frames.hpp:139
InertiaTest::setUp
void setUp()
Definition: inertiatest.cpp:14
KDL::Rotation::data
double data[9]
Definition: frames.hpp:304
InertiaTest::TestRotationalInertia
void TestRotationalInertia()
Definition: inertiatest.cpp:22

orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36