FrameVectorTest.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include "rdl_dynamics/FrameVector.hpp"
3 #include "UnitTestUtils.hpp"
4 
5 using namespace RobotDynamics;
6 
7 class FrameVectorTest : public ::testing::Test
8 {
9  protected:
10  virtual void SetUp()
11  {
12  std::srand(time(NULL));
13 
14  root = ReferenceFrame::createARootFrame("root1");
15  frame1 = unit_test_utils::createRandomUnchangingFrame("frame1", root, 1);
16  frame2 = unit_test_utils::createRandomUnchangingFrame("frame2", frame1, 2);
17  }
18 
19  virtual void TearDown()
20  {
21  }
22 
23  ReferenceFramePtr root;
24  ReferenceFramePtr frame1;
25  ReferenceFramePtr frame2;
26 
27  int nTests = 1000;
28 
29  private:
30 };
31 
32 TEST_F(FrameVectorTest, operators)
33 {
34  FrameVector v1(root, 1., 2., 3.);
35  Vector3d v2(2., 3., 4.);
36  FrameVector v3;
37 
38  v3 = v1 + v2;
39 
40  EXPECT_STREQ(v3.getReferenceFrame()->getName().c_str(), "root1");
41  EXPECT_EQ(v3.x(), 3.);
42  EXPECT_EQ(v3.y(), 5.);
43  EXPECT_EQ(v3.z(), 7.);
44 
45  EXPECT_STREQ(v1.getReferenceFrame()->getName().c_str(), "root1");
46  EXPECT_EQ(v1.x(), 1.);
47  EXPECT_EQ(v1.y(), 2.);
48  EXPECT_EQ(v1.z(), 3.);
49 
50  v3 = v1 - v2;
51  EXPECT_STREQ(v3.getReferenceFrame()->getName().c_str(), "root1");
52  EXPECT_EQ(v3.x(), -1.);
53  EXPECT_EQ(v3.y(), -1.);
54  EXPECT_EQ(v3.z(), -1.);
55 
56  v1.setReferenceFrame(frame1);
57  EXPECT_STREQ(v1.getReferenceFrame()->getName().c_str(), "frame1");
58 
59  FrameVector v4(root, 1., 2., 3.);
60  FrameVector v5(root, 1.9, 2.41, 1.3);
61  Vector3d v6 = v4 - (v4.dot(v5)) * v5;
62 
63  v6.isApprox(Vector3d(v4) - Vector3d(v4).dot(Vector3d(v5)) * Vector3d(v5), 1.e-12);
64 
65  v6 = v4 - v5 * (v4.dot(v5));
66 }
67 
68 TEST_F(FrameVectorTest, testConstructors)
69 {
70  FrameVector frameVector(root, 1., 2., 3.);
71 
72  RobotDynamics::Math::Vector3d vec = frameVector.vec();
73  EXPECT_EQ(vec.x(), 1.);
74  EXPECT_EQ(vec.y(), 2.);
75  EXPECT_EQ(vec.z(), 3.);
76 
77  Eigen::Vector3d vector = frameVector;
78  EXPECT_TRUE(vector(0) == 1.);
79  EXPECT_TRUE(vector(1) == 2.);
80  EXPECT_TRUE(vector(2) == 3.);
81 
82  Eigen::Vector3d vector2(3., 2., 1.);
83  FrameVector frameVector2(root, vector2);
84 
85  Eigen::Vector3d vectorCheck = frameVector2;
86  EXPECT_TRUE(vectorCheck(0) == 3.);
87  EXPECT_TRUE(vectorCheck(1) == 2.);
88  EXPECT_TRUE(vectorCheck(2) == 1.);
89 }
90 
91 TEST_F(FrameVectorTest, testSetIncludingFrame)
92 {
93  FrameVector frameVector(root, 1., 2., 3.);
94 
95  frameVector.setIncludingFrame(0.1, 0.2, 0.3, frame1);
96 
97  EXPECT_STREQ(frameVector.getReferenceFrame()->getName().c_str(), "frame1");
98  EXPECT_EQ(frameVector.x(), 0.1);
99  EXPECT_EQ(frameVector.y(), 0.2);
100  EXPECT_EQ(frameVector.z(), 0.3);
101 
102  frameVector.setIncludingFrame(Vector3d(0.3, 0.2, 0.1), frame2);
103 
104  EXPECT_STREQ(frameVector.getReferenceFrame()->getName().c_str(), "frame2");
105  EXPECT_EQ(frameVector.x(), 0.3);
106  EXPECT_EQ(frameVector.y(), 0.2);
107  EXPECT_EQ(frameVector.z(), 0.1);
108 
109  try
110  {
111  frameVector.setIncludingFrame(0.1, 0.2, 0.3, nullptr);
112  }
113  catch (RobotDynamics::ReferenceFrameException& e)
114  {
115  EXPECT_STREQ(e.what(), "Reference frame cannot be nullptr!");
116  }
117 
118  try
119  {
120  frameVector.setIncludingFrame(Vector3d(-0.1, -0.2, -0.3), nullptr);
121  }
122  catch (RobotDynamics::ReferenceFrameException& e)
123  {
124  EXPECT_STREQ(e.what(), "Reference frame cannot be nullptr!");
125  }
126 }
127 
128 TEST_F(FrameVectorTest, testDot)
129 {
130  FrameVector frameVector1(frame1, -1., 2., -3.);
131  FrameVector frameVector2(frame2, 1., 2., 3.);
132  FrameVector frameVector3(frame1, 4., 5., -6.);
133 
134  try
135  {
136  frameVector1.dot(frameVector2);
137  }
138  catch (ReferenceFrameException& e)
139  {
140  EXPECT_STREQ(e.what(), "Reference frames do not match!");
141  }
142 
143  double value = frameVector1.dot(frameVector3);
144 
145  EXPECT_TRUE(value = 24);
146 }
147 
148 TEST_F(FrameVectorTest, testCross)
149 {
150  Eigen::Vector3d result;
151  Eigen::Vector3d expectedResult;
152 
153  for (int i = 0; i < nTests; i++)
154  {
155  Eigen::Vector3d v1(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
156  Eigen::Vector3d v2(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
157  Eigen::Vector3d v3(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
158 
159  FrameVector frameVector1(frame1, v1);
160  FrameVector frameVector2(frame2, v2);
161  FrameVector frameVector3(frame1, v3);
162 
163  try
164  {
165  frameVector1.cross(frameVector2);
166  }
167  catch (ReferenceFrameException& e)
168  {
169  EXPECT_STREQ(e.what(), "Reference frames do not match!");
170  }
171 
172  Eigen::Vector3d result = frameVector1.cross(frameVector3);
173  Eigen::Vector3d expectedResult = v1.cross(v3);
174  EXPECT_TRUE(result(0) == expectedResult(0));
175  EXPECT_TRUE(result(1) == expectedResult(1));
176  EXPECT_TRUE(result(2) == expectedResult(2));
177 
178  result = frameVector1.cross(v2);
179  expectedResult = v1.cross(v2);
180  EXPECT_TRUE(result.isApprox(expectedResult, 1.e-12));
181  }
182 }
183 
184 TEST_F(FrameVectorTest, testAngleBetweenVectors)
185 {
186  FrameVector frameVector1(frame1, 2., 3., 1.);
187  FrameVector frameVector2(frame1, 4., 1., 2.);
188 
189  double angle = frameVector1.getAngleBetweenVectors(frameVector2);
190  double expectedResult = acos(13 / (sqrt(14) * sqrt(21)));
191  EXPECT_TRUE(angle == expectedResult);
192 
193  frameVector1.set(0., 0., 0.);
194 
195  EXPECT_NEAR(frameVector1.getAngleBetweenVectors(frameVector2), M_PI, 1e-12);
196 }
197 
198 TEST_F(FrameVectorTest, testChangeFrame)
199 {
200  RobotDynamics::Math::SpatialTransform transform1;
201 
202  transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
203  transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
204 
205  ReferenceFramePtr frameA(new ReferenceFrame("A", root, transform1, false, 3));
206 
207  transform1.E = RobotDynamics::Math::Xroty(M_PI_2).E;
208  transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
209 
210  ReferenceFramePtr frameB(new ReferenceFrame("B", frameA, transform1, false, 4));
211 
212  transform1.E = RobotDynamics::Math::Xrotz(M_PI_2).E;
213  transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
214 
215  ReferenceFramePtr frameC(new ReferenceFrame("C", frameB, transform1, false, 5));
216 
217  double x = 3.0;
218  double y = 1.0;
219  double z = -9.0;
220 
221  FrameVector frameVector(frameC, x, y, z);
222  frameVector.changeFrame(frameB);
223 
224  EXPECT_NEAR(frameVector.x(), -1, unit_test_utils::TEST_PREC);
225  EXPECT_NEAR(frameVector.y(), 3, unit_test_utils::TEST_PREC);
226  EXPECT_NEAR(frameVector.z(), -9, unit_test_utils::TEST_PREC);
227 
228  frameVector.changeFrame(frameA);
229 
230  EXPECT_NEAR(frameVector.x(), -9, unit_test_utils::TEST_PREC);
231  EXPECT_NEAR(frameVector.y(), 3, unit_test_utils::TEST_PREC);
232  EXPECT_NEAR(frameVector.z(), 1, unit_test_utils::TEST_PREC);
233 }
234 
235 TEST_F(FrameVectorTest, testChangeFrameAndCopy)
236 {
237  RobotDynamics::Math::SpatialTransform transform1;
238 
239  transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
240  transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
241 
242  ReferenceFramePtr frameA(new ReferenceFrame("A", root, transform1, false, 3));
243 
244  transform1.E = RobotDynamics::Math::Xroty(M_PI_2).E;
245  transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
246 
247  ReferenceFramePtr frameB(new ReferenceFrame("B", frameA, transform1, false, 4));
248 
249  transform1.E = RobotDynamics::Math::Xrotz(M_PI_2).E;
250  transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
251 
252  ReferenceFramePtr frameC(new ReferenceFrame("C", frameB, transform1, false, 5));
253 
254  double x = 3.0;
255  double y = 1.0;
256  double z = -9.0;
257 
258  FrameVector v;
259  FrameVector frameVector(frameC, x, y, z);
260  v = frameVector.changeFrameAndCopy(frameB);
261 
262  EXPECT_NEAR(v.x(), -1, unit_test_utils::TEST_PREC);
263  EXPECT_NEAR(v.y(), 3, unit_test_utils::TEST_PREC);
264  EXPECT_NEAR(v.z(), -9, unit_test_utils::TEST_PREC);
265 
266  FrameVector v2;
267  v2 = v.changeFrameAndCopy(frameA);
268 
269  EXPECT_NEAR(v2.x(), -9, unit_test_utils::TEST_PREC);
270  EXPECT_NEAR(v2.y(), 3, unit_test_utils::TEST_PREC);
271  EXPECT_NEAR(v2.z(), 1, unit_test_utils::TEST_PREC);
272 }
273 
274 TEST_F(FrameVectorTest, testVectorLength)
275 {
276  Eigen::Vector3d result;
277  Eigen::Vector3d expectedResult;
278 
279  for (int i = 0; i < nTests; i++)
280  {
281  Eigen::Vector3d v1(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
282 
283  FrameVector frameVector1(frame1, v1);
284 
285  double result = frameVector1.norm();
286  double expectedResult = v1.norm();
287  EXPECT_TRUE(result == expectedResult);
288  }
289 }
290 
291 int main(int argc, char** argv)
292 {
293  ::testing::InitGoogleTest(&argc, argv);
294  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
295  return RUN_ALL_TESTS();
296 }
RobotDynamics::FrameObject::getReferenceFrame
ReferenceFramePtr getReferenceFrame() const
Get a pointer to the reference frame this FrameObject is expressed in.
Definition: FrameObject.hpp:52
unit_test_utils::createRandomUnchangingFrame
static ReferenceFramePtr createRandomUnchangingFrame(const std::string &frameName, ReferenceFramePtr parentFrame, const unsigned int movableBodyId)
Definition: UnitTestUtils.hpp:376
FrameVectorTest::TearDown
virtual void TearDown()
Definition: FrameVectorTest.cpp:19
RobotDynamics::ReferenceFrameException
A custom exception for frame operations.
Definition: FrameExceptions.hpp:30
RobotDynamics::ReferenceFrame::createARootFrame
static ReferenceFramePtr createARootFrame(const std::string &frameName)
Creates a root frame with ReferenceFrame::parentFrame=nullptr.
Definition: ReferenceFrame.hpp:258
RobotDynamics::Math::Xroty
SpatialTransform Xroty(const double &yrot)
Get transform with zero translation and pure rotation about y axis.
Definition: SpatialAlgebraOperators.h:286
FrameVectorTest::root
ReferenceFramePtr root
Definition: FrameVectorTest.cpp:23
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::Math::FrameVector::changeFrameAndCopy
FrameVector changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
copy into new frame vector and change the frame of that
Definition: FrameVector.hpp:93
RobotDynamics::FrameObject::setReferenceFrame
void setReferenceFrame(ReferenceFramePtr frame)
Set frame objects internal reference frame.
Definition: FrameObject.hpp:61
RobotDynamics::Math::FrameVector::cross
FrameVector cross(const FrameVector &vector) const
Cross product between two FrameVectors, i.e. .
Definition: FrameVector.hpp:163
RobotDynamics::ReferenceFrameException::what
virtual const char * what() const
Definition: FrameExceptions.hpp:41
RobotDynamics::Math::Xrotz
SpatialTransform Xrotz(const double &zrot)
Get transform with zero translation and pure rotation about z axis.
Definition: SpatialAlgebraOperators.h:300
RobotDynamics::ReferenceFramePtr
std::shared_ptr< ReferenceFrame > ReferenceFramePtr
Definition: ReferenceFrame.hpp:72
RobotDynamics::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: rdl_eigenmath.h:337
RobotDynamics::FrameObject::changeFrame
virtual void changeFrame(ReferenceFramePtr desiredFrame)
Change the ReferenceFrame this FrameObject is expressed in.
Definition: FrameObject.cpp:12
RobotDynamics::Math::FrameVector::setIncludingFrame
void setIncludingFrame(const double x, const double y, const double z, ReferenceFramePtr referenceFrame)
Set the x, y, and z components and the ReferenceFrame these components are expressed in...
Definition: FrameVector.hpp:116
RobotDynamics::Math::FrameVector
A FrameVector is a 3D vector with a ReferenceFrame, and all operations between FrameVectors and other...
Definition: FrameVector.hpp:33
RobotDynamics::ReferenceFrame
ReferenceFrame object used to tell what frame objects are expressed in. Every ReferenceFrame has a po...
Definition: ReferenceFrame.hpp:88
RobotDynamics::Math::Xrotx
SpatialTransform Xrotx(const double &xrot)
Get transform with zero translation and pure rotation about x axis.
Definition: SpatialAlgebraOperators.h:272
main
int main(int argc, char **argv)
Definition: FrameVectorTest.cpp:291
FrameVectorTest::frame1
ReferenceFramePtr frame1
Definition: FrameVectorTest.cpp:24
RobotDynamics::Math::Vector3d::set
void set(const Eigen::Vector3d &v)
Definition: rdl_eigenmath.h:83
RobotDynamics::Math::FrameVector::dot
double dot(const FrameVector &frameVector) const
Dot product between two FrameVectors, i.e. .
Definition: FrameVector.hpp:150
FrameVectorTest::frame2
ReferenceFramePtr frame2
Definition: FrameVectorTest.cpp:25
TEST_F
TEST_F(FrameVectorTest, operators)
Definition: FrameVectorTest.cpp:32
FrameVectorTest::SetUp
virtual void SetUp()
Definition: FrameVectorTest.cpp:10
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
RobotDynamics::Math::FrameVector::getAngleBetweenVectors
double getAngleBetweenVectors(const FrameVector &frameVector) const
Computer the angle between two FrameVectors, .
Definition: FrameVector.hpp:185

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