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)-Map<Matrix3d>(I1.data)).isZero());
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)-Map<Matrix3d>(I2.data)).isZero());
34  I2.data[0]=0.0;
35  CPPUNIT_ASSERT(!(Map<Matrix3d>(I3.data)-Map<Matrix3d>(I2.data)).isZero());
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)-Map<Matrix3d>(I2.data).transpose()).isZero());
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)-(Map<Matrix3d>(I2.data)*Map<Vector3d>(omega.data))).isZero());
53 }
54 
55 void InertiaTest::TestRigidBodyInertia() {
56  RigidBodyInertia I1(0.0);
57  double mass;
58  Vector c;
59  RotationalInertia Ic;
60  random(mass);
61  random(c);
62  Matrix3d Ic_eig = Matrix3d::Random();
63  //make it symmetric:
64  Map<Matrix3d>(Ic.data)=Ic_eig+Ic_eig.transpose();
65  RigidBodyInertia I2(mass,c,Ic);
66  //Check if construction works fine
67  CPPUNIT_ASSERT_EQUAL(I2.getMass(),mass);
68  CPPUNIT_ASSERT(!Map<Matrix3d>(I2.getRotationalInertia().data).isZero());
69  CPPUNIT_ASSERT_EQUAL(I2.getCOG(),c);
70  CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-(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())))).isZero());
71 
72  RigidBodyInertia I3=I2;
73  //check if copying works fine
74  CPPUNIT_ASSERT_EQUAL(I2.getMass(),I3.getMass());
75  CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I3.getCOG());
76  CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I3.getRotationalInertia().data)).isZero());
77  //Check if multiplication and addition works fine
78  RigidBodyInertia I4=-2*I2 +I3+I3;
79  CPPUNIT_ASSERT_DOUBLES_EQUAL(I4.getMass(),0.0,epsilon);
80  CPPUNIT_ASSERT_EQUAL(I4.getCOG(),Vector::Zero());
81  CPPUNIT_ASSERT(Map<Matrix3d>(I4.getRotationalInertia().data).isZero());
82 
83  //Check if transformations work fine
84  //Check only rotation transformation
85  //back and forward
86  Rotation R;
87  random(R);
88  I3 = R*I2;
89  I4 = R.Inverse()*I3;
90  CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(), epsilon);
91  CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
92  CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
93  //rotation and total with p=0
94  Frame T(R);
95  I4 = T*I2;
96  CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(), epsilon);
97  CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
98  CPPUNIT_ASSERT((Map<Matrix3d>(I3.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
99 
100  //Check only transformation
101  Vector p;
102  random(p);
103  I3 = I2.RefPoint(p);
104  I4 = I3.RefPoint(-p);
105  CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
106  CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
107  CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
108  T=Frame(-p);
109  I4 = T*I2;
110  CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(),epsilon);
111  CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
112  CPPUNIT_ASSERT((Map<Matrix3d>(I3.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
113 
114  random(T);
115  I3=T*I2;
116  I4=T.Inverse()*I3;
117 
118  Twist a;
119  random(a);
120  Wrench f=I2*a;
121  CPPUNIT_ASSERT_EQUAL(T*f,(T*I2)*(T*a));
122 
123  random(T);
124  I3 = T*I2;
125  I4 = T.Inverse()*I3;
126  CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
127  CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
128  CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
129 
130 }
131 void InertiaTest::TestArticulatedBodyInertia() {
132  double mass;
133  Vector c;
134  RotationalInertia Ic;
135  random(mass);
136  random(c);
137  Matrix3d::Map(Ic.data).triangularView<Lower>()= Matrix3d::Random().triangularView<Lower>();
138  RigidBodyInertia RBI2(mass,c,Ic);
139  ArticulatedBodyInertia I2(RBI2);
140  ArticulatedBodyInertia I1(mass,c,Ic);
141  //Check if construction works fine
142  CPPUNIT_ASSERT_EQUAL(I2.M,I1.M);
143  CPPUNIT_ASSERT_EQUAL(I2.H,I1.H);
144  CPPUNIT_ASSERT_EQUAL(I2.I,I1.I);
145  I1 = ArticulatedBodyInertia(I2);
146  CPPUNIT_ASSERT_EQUAL(I2.M,I1.M);
147  CPPUNIT_ASSERT_EQUAL(I2.H,I1.H);
148  CPPUNIT_ASSERT_EQUAL(I2.I,I1.I);
149 
150  CPPUNIT_ASSERT_EQUAL(I2.M,(Matrix3d::Identity()*mass).eval());
151  CPPUNIT_ASSERT(!I2.I.isZero());
152 // CPPUNIT_ASSERT((I2.I-(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())))).isZero());
153 // CPPUNIT_ASSERT((I2.H-(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity()))).isZero());
154  ArticulatedBodyInertia I3=I2;
155  //check if copying works fine
156  CPPUNIT_ASSERT((I2.M-I3.M).isZero());
157  CPPUNIT_ASSERT((I2.H-I3.H).isZero());
158  CPPUNIT_ASSERT((I2.I-I3.I).isZero());
159  //Check if multiplication and addition works fine
160  ArticulatedBodyInertia I4=-2*I2 +I3+I3;
161  CPPUNIT_ASSERT(I4.M.isZero());
162  CPPUNIT_ASSERT(I4.H.isZero());
163  CPPUNIT_ASSERT(I4.I.isZero());
164 
165  //Check if transformations work fine
166  //Check only rotation transformation
167  //back and forward
168  Rotation R;
169  random(R);
170  I3 = R*I2;
171  Map<Matrix3d> E(R.data);
172  Matrix3d tmp = E.transpose()*I2.M*E;
173  CPPUNIT_ASSERT((I3.M-tmp).isZero());
174  tmp = E.transpose()*I2.H*E;
175  CPPUNIT_ASSERT((I3.H-tmp).isZero());
176  tmp = E.transpose()*I2.I*E;
177  CPPUNIT_ASSERT((I3.I-tmp).isZero());
178 
179  I4 = R.Inverse()*I3;
180  CPPUNIT_ASSERT((I2.M-I4.M).isZero());
181  CPPUNIT_ASSERT((I2.H-I4.H).isZero());
182  CPPUNIT_ASSERT((I2.I-I4.I).isZero());
183  //rotation and total with p=0
184  Frame T(R);
185  I4 = T*I2;
186  CPPUNIT_ASSERT((I3.M-I4.M).isZero());
187  CPPUNIT_ASSERT((I3.H-I4.H).isZero());
188  CPPUNIT_ASSERT((I3.I-I4.I).isZero());
189 
190  //Check only transformation
191  Vector p;
192  random(p);
193  I3 = I2.RefPoint(p);
194  I4 = I3.RefPoint(-p);
195  CPPUNIT_ASSERT((I2.M-I4.M).isZero());
196  CPPUNIT_ASSERT((I2.H-I4.H).isZero());
197  CPPUNIT_ASSERT((I2.I-I4.I).isZero());
198  T=Frame(-p);
199  I4 = T*I2;
200  CPPUNIT_ASSERT((I3.M-I4.M).isZero());
201  CPPUNIT_ASSERT((I3.H-I4.H).isZero());
202  CPPUNIT_ASSERT((I3.I-I4.I).isZero());
203 
204 
205  random(T);
206  I3=T*I2;
207  I4=T.Inverse()*I3;
208 
209  Twist a;
210  random(a);
211  Wrench f=I2*a;
212  CPPUNIT_ASSERT_EQUAL(T*f,(T*I2)*(T*a));
213 
214  random(T);
215  I3 = T*I2;
216  I4 = T.Inverse()*I3;
217  CPPUNIT_ASSERT((I2.M-I4.M).isZero());
218  CPPUNIT_ASSERT((I2.H-I4.H).isZero());
219  CPPUNIT_ASSERT((I2.I-I4.I).isZero());
220 }
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
InertiaTest::TestArticulatedBodyInertia
void TestArticulatedBodyInertia()
Definition: inertiatest.cpp:131
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
InertiaTest::TestRigidBodyInertia
void TestRigidBodyInertia()
Definition: inertiatest.cpp:55
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:1215
KDL::Vector::Zero
static Vector Zero()
Definition: frames.hpp:139
InertiaTest::setUp
void setUp()
Definition: inertiatest.cpp:14
InertiaTest::TestRotationalInertia
void TestRotationalInertia()
Definition: inertiatest.cpp:22

orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:43