RdlBodyTests.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 
3 #include "rdl_dynamics/rdl_mathutils.h"
4 #include "rdl_dynamics/Body.h"
5 #include "UnitTestUtils.hpp"
6 
7 using namespace std;
8 using namespace RobotDynamics;
9 using namespace RobotDynamics::Math;
10 
11 class RdlBodyTests : public testing::Test
12 {
13  public:
14  RdlBodyTests(){};
15 
16  void SetUp()
17  {
18  }
19 
20  void TearDown()
21  {
22  }
23 };
24 
25 /* Tests whether the spatial inertia matches the one specified by its
26  * parameters
27  */
28 TEST_F(RdlBodyTests, TestComputeSpatialInertiaFromAbsoluteRadiiGyration)
29 {
30  Body body(1.1, Vector3d(1.5, 1.2, 1.3), Vector3d(1.4, 2., 3.));
31 
32  Matrix3d inertia_C(1.4, 0., 0., 0., 2., 0., 0., 0., 3.);
33 
34  SpatialMatrix reference_inertia(4.843, -1.98, -2.145, 0, -1.43, 1.32, -1.98, 6.334, -1.716, 1.43, 0, -1.65, -2.145, -1.716, 7.059, -1.32, 1.65, 0, 0, 1.43, -1.32,
35  1.1, 0, 0, -1.43, 0, 1.65, 0, 1.1, 0, 1.32, -1.65, 0, 0, 0, 1.1);
36 
37  RigidBodyInertia rbi = createFromMassComInertiaC(body.mMass, body.mCenterOfMass, body.mInertia);
38 
39  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(reference_inertia, rbi.toMatrix(), unit_test_utils::TEST_PREC));
40  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(inertia_C, body.mInertia, unit_test_utils::TEST_PREC));
41 }
42 
43 TEST_F(RdlBodyTests, TestJoinTwoBodiesWithZeroMassThrows)
44 {
45  Body body1(0., Vector3d(1.5, 1.2, 1.3), Vector3d(1.4, 2., 3.));
46  Body body2(0., Vector3d(1.5, 1.2, 1.3), Vector3d(1.4, 2., 3.));
47 
48  try
49  {
50  body1.join(RobotDynamics::Math::SpatialTransform(), body2);
51  FAIL();
52  }
53  catch (RobotDynamics::RdlException& e)
54  {
55  EXPECT_STREQ(e.what(), "Error: cannot join bodies as both have zero mass!");
56  }
57 }
58 
59 TEST_F(RdlBodyTests, TestBodyConstructorMassComInertia)
60 {
61  double mass = 1.1;
62  Vector3d com(1.5, 1.2, 1.3);
63  Matrix3d inertia_C(8.286, -3.96, -4.29, -3.96, 10.668, -3.432, -4.29, -3.432, 11.118);
64 
65  Body body(mass, com, inertia_C);
66 
67  SpatialMatrix reference_inertia(11.729, -5.94, -6.435, 0, -1.43, 1.32, -5.94, 15.002, -5.148, 1.43, 0, -1.65, -6.435, -5.148, 15.177, -1.32, 1.65, 0, 0, 1.43, -1.32,
68  1.1, 0, 0, -1.43, 0, 1.65, 0, 1.1, 0, 1.32, -1.65, 0, 0, 0, 1.1);
69 
70  RigidBodyInertia body_rbi = createFromMassComInertiaC(body.mMass, body.mCenterOfMass, body.mInertia);
71  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(reference_inertia, body_rbi.toMatrix(), unit_test_utils::TEST_PREC));
72  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(inertia_C, body.mInertia, unit_test_utils::TEST_PREC));
73 }
74 
75 TEST_F(RdlBodyTests, TestBodyJoinNullbody)
76 {
77  Body body(1.1, Vector3d(1.5, 1.2, 1.3), Vector3d(1.4, 2., 3.));
78  Body nullbody(0., Vector3d(0., 0., 0.), Vector3d(0., 0., 0.));
79 
80  Body joined_body = body;
81  joined_body.join(Xtrans(Vector3d(0., 0., 0.)), nullbody);
82 
83  RigidBodyInertia body_rbi(body.mMass, body.mCenterOfMass, body.mInertia);
84  RigidBodyInertia joined_body_rbi(joined_body.mMass, joined_body.mCenterOfMass, joined_body.mInertia);
85 
86  ASSERT_EQ(1.1, body.mMass);
87  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(body.mCenterOfMass, joined_body.mCenterOfMass, unit_test_utils::TEST_PREC));
88  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(body_rbi.toMatrix(), joined_body_rbi.toMatrix(), unit_test_utils::TEST_PREC));
89 }
90 
91 TEST_F(RdlBodyTests, TestBodyJoinTwoBodies)
92 {
93  Body body_a(1.1, Vector3d(-1.1, 1.3, 0.), Vector3d(3.1, 3.2, 3.3));
94  Body body_b(1.1, Vector3d(1.1, 1.3, 0.), Vector3d(3.1, 3.2, 3.3));
95 
96  Body body_joined(body_a);
97  body_joined.join(Xtrans(Vector3d(0., 0., 0.)), body_b);
98 
99  RigidBodyInertia body_joined_rbi = createFromMassComInertiaC(body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia);
100 
101  SpatialMatrix reference_inertia(9.918, 0, 0, 0, -0, 2.86, 0, 9.062, 0, 0, 0, -0, 0, 0, 12.98, -2.86, 0, 0, 0, 0, -2.86, 2.2, 0, 0, -0, 0, 0, 0, 2.2, 0, 2.86, -0, 0,
102  0, 0, 2.2);
103 
104  ASSERT_EQ(2.2, body_joined.mMass);
105  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(0., 1.3, 0.), body_joined.mCenterOfMass));
106  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(reference_inertia, body_joined_rbi.toMatrix(), unit_test_utils::TEST_PREC));
107 }
108 
109 TEST_F(RdlBodyTests, TestBodyJoinTwoBodiesDisplaced)
110 {
111  Body body_a(1.1, Vector3d(-1.1, 1.3, 0.), Vector3d(3.1, 3.2, 3.3));
112  Body body_b(1.1, Vector3d(0., 0., 0.), Vector3d(3.1, 3.2, 3.3));
113 
114  Body body_joined(body_a);
115  body_joined.join(Xtrans(Vector3d(1.1, 1.3, 0.)), body_b);
116 
117  RigidBodyInertia body_joined_rbi = createFromMassComInertiaC(body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia);
118 
119  SpatialMatrix reference_inertia(9.918, 0, 0, 0, -0, 2.86, 0, 9.062, 0, 0, 0, -0, 0, 0, 12.98, -2.86, 0, 0, 0, 0, -2.86, 2.2, 0, 0, -0, 0, 0, 0, 2.2, 0, 2.86, -0, 0,
120  0, 0, 2.2);
121 
122  ASSERT_EQ(2.2, body_joined.mMass);
123  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(0., 1.3, 0.), body_joined.mCenterOfMass));
124  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(reference_inertia, body_joined_rbi.toMatrix(), unit_test_utils::TEST_PREC));
125 }
126 
127 TEST_F(RdlBodyTests, TestBodyJoinTwoBodiesRotated)
128 {
129  Body body_a(1.1, Vector3d(0., 0., 0.), Vector3d(3.1, 3.2, 3.3));
130  Body body_b(1.1, Vector3d(0., 0., 0.), Vector3d(3.1, 3.3, 3.2));
131 
132  Body body_joined(body_a);
133  body_joined.join(Xrotx(-M_PI * 0.5), body_b);
134 
135  RigidBodyInertia body_joined_rbi(body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia);
136 
137  SpatialMatrix reference_inertia(6.2, 0., 0., 0., 0., 0., 0., 6.4, 0., 0., 0., 0., 0., 0., 6.6, 0., 0., 0., 0., 0., 0., 2.2, 0., 0., 0., 0., 0., 0., 2.2, 0., 0., 0.,
138  0., 0., 0., 2.2);
139 
140  ASSERT_EQ(2.2, body_joined.mMass);
141  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(0., 0., 0.), body_joined.mCenterOfMass));
142  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(reference_inertia, body_joined_rbi.toMatrix(), unit_test_utils::TEST_PREC));
143 }
144 
145 TEST_F(RdlBodyTests, TestBodyJoinTwoBodiesRotatedAndTranslated)
146 {
147  Body body_a(1.1, Vector3d(0., 0., 0.), Vector3d(3.1, 3.2, 3.3));
148  Body body_b(1.1, Vector3d(-1., 1., 0.), Vector3d(3.2, 3.1, 3.3));
149 
150  Body body_joined(body_a);
151  body_joined.join(Xrotz(M_PI * 0.5) * Xtrans(Vector3d(1., 1., 0.)), body_b);
152 
153  RigidBodyInertia body_joined_rbi(body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia);
154 
155  SpatialMatrix reference_inertia(6.2, 0., 0., 0., 0., 0., 0., 6.4, 0., 0., 0., 0., 0., 0., 6.6, 0., 0., 0., 0., 0., 0., 2.2, 0., 0., 0., 0., 0., 0., 2.2, 0., 0., 0.,
156  0., 0., 0., 2.2);
157 
158  ASSERT_EQ(2.2, body_joined.mMass);
159  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 0., 0.), body_joined.mCenterOfMass, unit_test_utils::TEST_PREC));
160  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(reference_inertia, body_joined_rbi.toMatrix(), unit_test_utils::TEST_PREC));
161 }
162 
163 TEST_F(RdlBodyTests, TestBodyConstructorRigidBodyInertiaMultiplyMotion)
164 {
165  Body body(1.1, Vector3d(1.5, 1.2, 1.3), Vector3d(1.4, 2., 3.));
166 
167  RigidBodyInertia rbi = RigidBodyInertia(body.mMass, body.mCenterOfMass * body.mMass, body.mInertia);
168 
169  SpatialVector mv(1.1, 1.2, 1.3, 1.4, 1.5, 1.6);
170  SpatialVector fv_matrix = rbi.toMatrix() * mv;
171  SpatialVector fv_rbi = rbi * mv;
172 
173  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(fv_matrix, fv_rbi, unit_test_utils::TEST_PREC));
174 }
175 
176 TEST_F(RdlBodyTests, TestBodyConstructorRigidBodyInertia)
177 {
178  Body body(1.1, Vector3d(1.5, 1.2, 1.3), Vector3d(1.4, 2., 3.));
179 
180  RigidBodyInertia rbi = RigidBodyInertia(body.mMass, body.mCenterOfMass * body.mMass, body.mInertia);
181  SpatialMatrix spatial_inertia = rbi.toMatrix();
182 
183  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(spatial_inertia, rbi.toMatrix(), unit_test_utils::TEST_PREC));
184 }
185 
186 TEST_F(RdlBodyTests, TestBodyConstructorCopyRigidBodyInertia)
187 {
188  Body body(1.1, Vector3d(1.5, 1.2, 1.3), Vector3d(1.4, 2., 3.));
189 
190  RigidBodyInertia rbi = RigidBodyInertia(body.mMass, body.mCenterOfMass * body.mMass, body.mInertia);
191 
192  RigidBodyInertia rbi_from_matrix;
193  rbi_from_matrix.createFromMatrix(rbi.toMatrix());
194 
195  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(rbi.toMatrix(), rbi_from_matrix.toMatrix(), unit_test_utils::TEST_PREC));
196 }
197 
198 int main(int argc, char** argv)
199 {
200  ::testing::InitGoogleTest(&argc, argv);
201  return RUN_ALL_TESTS();
202 }
RobotDynamics::Body
Describes all properties of a single body.
Definition: Body.h:30
Body.h
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
RobotDynamics::Math::Xtrans
SpatialTransform Xtrans(const Vector3d &r)
Get pure translation transform.
Definition: SpatialAlgebraOperators.h:369
RobotDynamics::Body::join
void join(const Math::SpatialTransform &transform, const Body &other_body)
Joins inertial parameters of two bodies to create a composite body.
Definition: Body.h:98
rdl_mathutils.h
RobotDynamics::Body::mMass
double mMass
The mass of the body.
Definition: Body.h:156
RobotDynamics::Body::mInertia
Math::Matrix3d mInertia
Inertia matrix at the center of mass.
Definition: Body.h:162
RobotDynamics::Math::RigidBodyInertia::createFromMatrix
void createFromMatrix(const SpatialMatrix &Ic)
Create a Math::RigidBodyInertia object from a 6x6 Math::SpatialMatrix.
Definition: RigidBodyInertia.cpp:90
RobotDynamics::Math::Xrotz
SpatialTransform Xrotz(const double &zrot)
Get transform with zero translation and pure rotation about z axis.
Definition: SpatialAlgebraOperators.h:300
unit_test_utils::checkVector3dEq
static bool checkVector3dEq(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2)
Definition: UnitTestUtils.hpp:212
RobotDynamics::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: rdl_eigenmath.h:337
RobotDynamics::Math::RigidBodyInertia
This class stores a bodies mass, center of mass, and inertia information. The inertia elements are st...
Definition: RigidBodyInertia.hpp:45
RobotDynamics::RdlException::what
virtual const char * what() const
Definition: RdlExceptions.hpp:31
RobotDynamics::Math::Xrotx
SpatialTransform Xrotx(const double &xrot)
Get transform with zero translation and pure rotation about x axis.
Definition: SpatialAlgebraOperators.h:272
unit_test_utils::checkSpatialMatrixEpsilonClose
static bool checkSpatialMatrixEpsilonClose(const RobotDynamics::Math::SpatialMatrix &t1, const RobotDynamics::Math::SpatialMatrix &t2, const double epsilon)
Definition: UnitTestUtils.hpp:122
RobotDynamics::Math
Math types such as vectors and matrices and utility functions.
Definition: ForceVector.hpp:21
unit_test_utils::checkSpatialVectorsEpsilonClose
static bool checkSpatialVectorsEpsilonClose(const RobotDynamics::Math::SpatialVector &v1, const RobotDynamics::Math::SpatialVector &v2, const double epsilon)
Definition: UnitTestUtils.hpp:306
RobotDynamics::Body::mCenterOfMass
Math::Vector3d mCenterOfMass
The position of the center of mass in body coordinates.
Definition: Body.h:159
RobotDynamics::RdlException
A custom exception.
Definition: RdlExceptions.hpp:20
unit_test_utils::checkVector3dEpsilonClose
static bool checkVector3dEpsilonClose(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:256
TEST_F
TEST_F(RdlBodyTests, TestComputeSpatialInertiaFromAbsoluteRadiiGyration)
Definition: RdlBodyTests.cpp:28
main
int main(int argc, char **argv)
Definition: RdlBodyTests.cpp:198
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
unit_test_utils::checkMatrix3dEpsilonClose
static bool checkMatrix3dEpsilonClose(const RobotDynamics::Math::Matrix3d &t1, const RobotDynamics::Math::Matrix3d &t2, const double epsilon)
Definition: UnitTestUtils.hpp:138
RobotDynamics::Math::createFromMassComInertiaC
static RigidBodyInertia createFromMassComInertiaC(double mass, const Vector3d &com, const Matrix3d &inertia_C)
Definition: RigidBodyInertia.hpp:273

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:27