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

orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14