FramePointTest.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include "rdl_dynamics/FramePoint.hpp"
3 #include "UnitTestUtils.hpp"
4 
5 using namespace RobotDynamics;
6 
7 class FramePointTest : public ::testing::Test
8 {
9  protected:
10  virtual void SetUp()
11  {
12  std::srand(time(NULL));
13  }
14 
15  virtual void TearDown()
16  {
17  }
18 
19  ReferenceFramePtr root1 = ReferenceFrame::createARootFrame("root1");
20 
21  int nTests = 1000;
22 
23  private:
24 };
25 
26 TEST_F(FramePointTest, testChangeFrame)
27 {
28  RobotDynamics::Math::SpatialTransform transform1;
29 
30  transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
31  transform1.r.set(6., 5., 3.);
32 
33  ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1, false, 1));
34 
35  transform1.E = RobotDynamics::Math::Xroty(M_PI_2).E;
36  transform1.r.set(3., -6., 4.);
37 
38  ReferenceFramePtr frameB(new ReferenceFrame("B", frameA, transform1, false, 2));
39 
40  transform1.E = RobotDynamics::Math::Xrotz(M_PI_2).E;
41  transform1.r = RobotDynamics::Math::Vector3d(0., -5., 0.);
42  ReferenceFramePtr frameC(new ReferenceFrame("C", frameB, transform1, false, 3));
43 
44  frameA->update();
45  frameB->update();
46  frameC->update();
47 
48  double x = 3.;
49  double y = -1.;
50  double z = 2.;
51 
52  FramePoint framePoint(root1, x, y, z);
53 
54  EXPECT_NEAR(framePoint.x(), x, unit_test_utils::TEST_PREC);
55  EXPECT_NEAR(framePoint.y(), y, unit_test_utils::TEST_PREC);
56  EXPECT_NEAR(framePoint.z(), z, unit_test_utils::TEST_PREC);
57 
58  framePoint.changeFrame(root1);
59 
60  EXPECT_NEAR(framePoint.x(), x, unit_test_utils::TEST_PREC);
61  EXPECT_NEAR(framePoint.y(), y, unit_test_utils::TEST_PREC);
62  EXPECT_NEAR(framePoint.z(), z, unit_test_utils::TEST_PREC);
63 
64  framePoint.changeFrame(frameA);
65 
66  EXPECT_NEAR(framePoint.x(), -3., unit_test_utils::TEST_PREC);
67  EXPECT_NEAR(framePoint.y(), -1., unit_test_utils::TEST_PREC);
68  EXPECT_NEAR(framePoint.z(), 6., unit_test_utils::TEST_PREC);
69 
70  framePoint.changeFrame(frameB);
71 
72  EXPECT_NEAR(framePoint.x(), -2., unit_test_utils::TEST_PREC);
73  EXPECT_NEAR(framePoint.y(), 5., unit_test_utils::TEST_PREC);
74  EXPECT_NEAR(framePoint.z(), -6., unit_test_utils::TEST_PREC);
75 
76  framePoint.changeFrame(frameC);
77 
78  EXPECT_NEAR(framePoint.x(), 10., unit_test_utils::TEST_PREC);
79  EXPECT_NEAR(framePoint.y(), 2., unit_test_utils::TEST_PREC);
80  EXPECT_NEAR(framePoint.z(), -6., unit_test_utils::TEST_PREC);
81 
82  framePoint.changeFrame(frameB);
83 
84  EXPECT_NEAR(framePoint.x(), -2., unit_test_utils::TEST_PREC);
85  EXPECT_NEAR(framePoint.y(), 5., unit_test_utils::TEST_PREC);
86  EXPECT_NEAR(framePoint.z(), -6., unit_test_utils::TEST_PREC);
87 
88  framePoint.changeFrame(frameA);
89 
90  EXPECT_NEAR(framePoint.x(), -3., unit_test_utils::TEST_PREC);
91  EXPECT_NEAR(framePoint.y(), -1., unit_test_utils::TEST_PREC);
92  EXPECT_NEAR(framePoint.z(), 6., unit_test_utils::TEST_PREC);
93 
94  framePoint.changeFrame(root1);
95 
96  EXPECT_NEAR(framePoint.x(), x, unit_test_utils::TEST_PREC);
97  EXPECT_NEAR(framePoint.y(), y, unit_test_utils::TEST_PREC);
98  EXPECT_NEAR(framePoint.z(), z, unit_test_utils::TEST_PREC);
99 }
100 
101 TEST_F(FramePointTest, testChangeFrameAndCopy)
102 {
103  RobotDynamics::Math::SpatialTransform transform1;
104 
105  transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
106  transform1.r.set(6., 5., 3.);
107 
108  ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1, false, 1));
109 
110  transform1.E = RobotDynamics::Math::Xroty(M_PI_2).E;
111  transform1.r.set(3., -6., 4.);
112 
113  ReferenceFramePtr frameB(new ReferenceFrame("B", frameA, transform1, false, 2));
114 
115  transform1.E = RobotDynamics::Math::Xrotz(M_PI_2).E;
116  transform1.r = RobotDynamics::Math::Vector3d(0., -5., 0.);
117  ReferenceFramePtr frameC(new ReferenceFrame("C", frameB, transform1, false, 3));
118 
119  frameA->update();
120  frameB->update();
121  frameC->update();
122 
123  double x = 3.;
124  double y = -1.;
125  double z = 2.;
126 
127  FramePoint framePoint(root1, x, y, z);
128 
129  EXPECT_NEAR(framePoint.x(), x, unit_test_utils::TEST_PREC);
130  EXPECT_NEAR(framePoint.y(), y, unit_test_utils::TEST_PREC);
131  EXPECT_NEAR(framePoint.z(), z, unit_test_utils::TEST_PREC);
132 
133  FramePoint p1 = framePoint.changeFrameAndCopy(root1);
134 
135  EXPECT_STREQ(framePoint.getReferenceFrame()->getName().c_str(), root1->getName().c_str());
136  EXPECT_NEAR(p1.x(), x, unit_test_utils::TEST_PREC);
137  EXPECT_NEAR(p1.y(), y, unit_test_utils::TEST_PREC);
138  EXPECT_NEAR(p1.z(), z, unit_test_utils::TEST_PREC);
139 
140  FramePoint p2 = p1;
141  p2 = p1.changeFrameAndCopy(frameA);
142 
143  EXPECT_NEAR(p2.x(), -3., unit_test_utils::TEST_PREC);
144  EXPECT_NEAR(p2.y(), -1., unit_test_utils::TEST_PREC);
145  EXPECT_NEAR(p2.z(), 6., unit_test_utils::TEST_PREC);
146 }
147 
148 TEST_F(FramePointTest, testDistance)
149 {
150  FramePoint framePoint1(root1, 1, 2, 3);
151  FramePoint framePoint2(root1, -1, -2, -3);
152  Point3d p1 = framePoint1.point();
153  EXPECT_EQ(p1.x(), framePoint1.x());
154  EXPECT_EQ(p1.y(), framePoint1.y());
155  EXPECT_EQ(p1.z(), framePoint1.z());
156 
157  RobotDynamics::Math::SpatialTransform transform1;
158 
159  transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
160  transform1.r.set(5., 0., 0.);
161 
162  ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1, false, 1));
163 
164  FramePoint framePoint3(frameA, 1., 2., 3.);
165 
166  EXPECT_TRUE(framePoint1.distance(framePoint2) == sqrt(36. + 16. + 4.));
167 
168  try
169  {
170  framePoint3.distance(framePoint2);
171  }
172  catch (RobotDynamics::ReferenceFrameException& e)
173  {
174  EXPECT_STREQ(e.what(), "Reference frames do not match!");
175  }
176 }
177 
178 TEST_F(FramePointTest, testSetIncludingFrameMismatchingFrames)
179 {
180  FramePoint framePoint2;
181 
182  try
183  {
184  framePoint2.setIncludingFrame(0.1, 0.2, 0.3, nullptr);
185  }
186  catch (RobotDynamics::ReferenceFrameException& e)
187  {
188  EXPECT_STREQ(e.what(), "Reference frame is nullptr!");
189  }
190 }
191 
192 TEST_F(FramePointTest, testDistanceMixingTypes)
193 {
194  FramePoint framePoint1(root1, 1, 2, 3);
195  FramePoint framePoint2(root1, -1, -2, -3);
196 
197  RobotDynamics::Math::SpatialTransform transform1;
198 
199  transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
200  transform1.r.set(5., 0., 0.);
201 
202  ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1.inverse(), false, 1));
203 
204  FramePoint framePoint3(frameA, 1., 2., 3.);
205 
206  EXPECT_TRUE(framePoint1.distance(framePoint2) == sqrt(36. + 16. + 4.));
207 
208  try
209  {
210  framePoint3.distance(framePoint2);
211  }
212  catch (RobotDynamics::ReferenceFrameException& e)
213  {
214  EXPECT_STREQ(e.what(), "Reference frames do not match!");
215  }
216 }
217 
218 TEST_F(FramePointTest, testDistanceL1)
219 {
220  std::shared_ptr<FramePoint> framePoint1(new RobotDynamics::Math::FramePoint(root1, 1., 2., 3.));
221  std::shared_ptr<FramePoint> framePoint2(new RobotDynamics::Math::FramePoint(root1, -1., -2., -3.));
222 
223  RobotDynamics::Math::SpatialTransform transform1;
224 
225  transform1 = RobotDynamics::Math::Xrotx(M_PI_2);
226  transform1.r.set(5., 0., 0.);
227 
228  ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1.inverse(), false, 1));
229 
230  std::shared_ptr<FramePoint> framePoint3(new RobotDynamics::Math::FramePoint(frameA, 1., 2., 3.));
231 
232  EXPECT_TRUE(framePoint1->distanceL1(*framePoint2) == 12.);
233 
234  try
235  {
236  framePoint3->distanceL1(*framePoint2);
237  }
238  catch (RobotDynamics::ReferenceFrameException& e)
239  {
240  EXPECT_STREQ(e.what(), "Reference frames do not match!");
241  }
242 }
243 
244 TEST_F(FramePointTest, testDistanceL1MixingTypes)
245 {
246  std::shared_ptr<FramePoint> framePoint1(new RobotDynamics::Math::FramePoint(root1, 1., 2., 3.));
247  std::shared_ptr<FramePoint> framePoint2(new RobotDynamics::Math::FramePoint(root1, -1., -2., -3.));
248 
249  RobotDynamics::Math::SpatialTransform transform1;
250 
251  transform1 = RobotDynamics::Math::Xrotx(M_PI_2);
252  transform1.r.set(5., 0., 0.);
253 
254  ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1.inverse(), false, 1));
255 
256  std::shared_ptr<FramePoint> framePoint3(new RobotDynamics::Math::FramePoint(frameA, 1., 2., 3.));
257 
258  EXPECT_TRUE(framePoint1->distanceL1(*framePoint2) == 12.);
259 
260  try
261  {
262  framePoint3->distanceL1(*framePoint2);
263  }
264  catch (RobotDynamics::ReferenceFrameException& e)
265  {
266  EXPECT_STREQ(e.what(), "Reference frames do not match!");
267  }
268 }
269 
270 TEST_F(FramePointTest, testDistanceLinf)
271 {
272  std::shared_ptr<FramePoint> framePoint1(new RobotDynamics::Math::FramePoint(root1, 1., 2., 3.));
273  std::shared_ptr<FramePoint> framePoint2(new RobotDynamics::Math::FramePoint(root1, -1., -2., -3.));
274 
275  RobotDynamics::Math::SpatialTransform transform1;
276 
277  transform1 = RobotDynamics::Math::Xrotx(M_PI_2);
278  transform1.r.set(5., 0., 0.);
279 
280  ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1.inverse(), false, 1));
281 
282  std::shared_ptr<FramePoint> framePoint3(new RobotDynamics::Math::FramePoint(frameA, 1., 2., 3.));
283 
284  EXPECT_TRUE(framePoint1->distanceLinf(*framePoint2) == 6.);
285 
286  try
287  {
288  framePoint3->distanceL1(*framePoint2);
289  }
290  catch (RobotDynamics::ReferenceFrameException& e)
291  {
292  EXPECT_STREQ(e.what(), "Reference frames do not match!");
293  }
294 }
295 
296 TEST_F(FramePointTest, testDistanceLinfMixingTypes)
297 {
298  std::shared_ptr<FramePoint> framePoint1(new RobotDynamics::Math::FramePoint(root1, 1., 2., 3.));
299  std::shared_ptr<FramePoint> framePoint2(new RobotDynamics::Math::FramePoint(root1, -1., -2., -3.));
300 
301  RobotDynamics::Math::SpatialTransform transform1;
302 
303  transform1 = RobotDynamics::Math::Xrotx(M_PI_2);
304  transform1.r.set(5., 0., 0.);
305 
306  ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1.inverse(), false, 1));
307 
308  std::shared_ptr<FramePoint> framePoint3(new RobotDynamics::Math::FramePoint(frameA, 1., 2., 3.));
309 
310  EXPECT_TRUE(framePoint1->distanceLinf(*framePoint2) == 6.);
311 
312  try
313  {
314  framePoint3->distanceL1(*framePoint2);
315  }
316  catch (RobotDynamics::ReferenceFrameException& e)
317  {
318  EXPECT_STREQ(e.what(), "Reference frames do not match!");
319  }
320 }
321 
322 TEST_F(FramePointTest, testOperatorOverloads)
323 {
324  FramePoint framePoint(root1, 1., 2., 3.);
325 
326  framePoint *= 3.;
327 
328  EXPECT_EQ(framePoint.x(), 3.);
329  EXPECT_EQ(framePoint.y(), 6.);
330  EXPECT_EQ(framePoint.z(), 9.);
331 
332  FramePoint p2(root1, RobotDynamics::Math::Vector3d(-1., -2., -3.));
333 
334  FrameVector v = framePoint - p2;
335 
336  EXPECT_EQ(v.x(), 4.);
337  EXPECT_EQ(v.y(), 8.);
338  EXPECT_EQ(v.z(), 12.);
339 
340  FramePoint p_check = p2 + v;
341 
342  EXPECT_NEAR(p_check.x(), framePoint.x(), 1.e-12);
343  EXPECT_NEAR(p_check.y(), framePoint.y(), 1.e-12);
344  EXPECT_NEAR(p_check.z(), framePoint.z(), 1.e-12);
345 
346  p_check = framePoint - v;
347 
348  EXPECT_NEAR(p_check.x(), p2.x(), 1.e-12);
349  EXPECT_NEAR(p_check.y(), p2.y(), 1.e-12);
350  EXPECT_NEAR(p_check.z(), p2.z(), 1.e-12);
351 }
352 
353 int main(int argc, char** argv)
354 {
355  ::testing::InitGoogleTest(&argc, argv);
356  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
357  return RUN_ALL_TESTS();
358 }
RobotDynamics::Math::Point3d::x
EIGEN_STRONG_INLINE double & x()
Definition: Point3.hpp:234
TEST_F
TEST_F(FramePointTest, testChangeFrame)
Definition: FramePointTest.cpp:26
FramePoint.hpp
File containing the FramePoint<T> object definition.
RobotDynamics::FrameObject::getReferenceFrame
ReferenceFramePtr getReferenceFrame() const
Get a pointer to the reference frame this FrameObject is expressed in.
Definition: FrameObject.hpp:52
RobotDynamics::Math::FramePoint
A FramePoint is a 3D point that is expressed in a ReferenceFrame. To change the ReferenceFrame a Fram...
Definition: FramePoint.hpp:40
RobotDynamics::Math::FramePoint::point
Math::Point3d point() const
Get as point3d.
Definition: FramePoint.hpp:106
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
FramePointTest::SetUp
virtual void SetUp()
Definition: FramePointTest.cpp:10
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
main
int main(int argc, char **argv)
Definition: FramePointTest.cpp:353
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::Math::FramePoint::setIncludingFrame
EIGEN_STRONG_INLINE void setIncludingFrame(const Math::Vector3d &v, ReferenceFramePtr referenceFrame)
Set both the ReferenceFrame this object is expressed in as well as the (x,y,z) coordinates of the poi...
Definition: FramePoint.hpp:138
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::SpatialTransform::inverse
SpatialTransform inverse() const
Returns inverse of transform.
Definition: rdl_eigenmath.h:488
RobotDynamics::Math::FramePoint::changeFrameAndCopy
FramePoint changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
copy into new frame point and change the frame of that
Definition: FramePoint.hpp:126
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
FramePointTest::TearDown
virtual void TearDown()
Definition: FramePointTest.cpp:15
RobotDynamics::Math::Point3d::y
EIGEN_STRONG_INLINE double & y()
Definition: Point3.hpp:244
RobotDynamics::Math::Vector3d::set
void set(const Eigen::Vector3d &v)
Definition: rdl_eigenmath.h:83
RobotDynamics::Math::FramePoint::distance
double distance(const FramePoint &point) const
Calculate the distance between two FramePoints. .
Definition: FramePoint.hpp:201
RobotDynamics::Math::Point3d::z
EIGEN_STRONG_INLINE double & z()
Definition: Point3.hpp:254
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21

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