FrameVectorPairTests.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include "rdl_dynamics/FrameVectorPair.hpp"
3 #include "UnitTestUtils.hpp"
4 
5 using namespace RobotDynamics;
6 
7 class FrameVectorPairTest : public ::testing::Test
8 {
9  public:
10  FrameVectorPairTest()
11  {
12  }
13 
14  ~FrameVectorPairTest()
15  {
16  }
17 
18  protected:
19  virtual void SetUp()
20  {
21  std::srand(time(NULL));
22 
23  root = ReferenceFrame::createARootFrame("root1");
24  frame1 = unit_test_utils::createRandomUnchangingFrame("frame1", root, 1);
25  frame2 = unit_test_utils::createRandomUnchangingFrame("frame2", frame1, 2);
26  }
27 
28  virtual void TearDown()
29  {
30  }
31 
32  ReferenceFramePtr root;
33  ReferenceFramePtr frame1;
34  ReferenceFramePtr frame2;
35 
36  int nTests = 1000;
37 
38  private:
39 };
40 
41 TEST_F(FrameVectorPairTest, simple1)
42 {
43  FrameVector v1(root, 1., 2., 3.);
44  FrameVector v2(root, 2., 3., 4.);
45 
46  FrameVectorPair pair(v1, v2);
47 
48  ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
49  ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
50 
51  pair.changeFrame(frame2);
52 
53  ASSERT_FALSE(pair.linear().isApprox(v1, 1e-12));
54  ASSERT_FALSE(pair.angular().isApprox(v2, 1e-12));
55 
56  v1.changeFrame(frame2);
57  v2.changeFrame(frame2);
58 
59  ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
60  ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
61 
62  FrameVectorPair pair2 = pair;
63 
64  ASSERT_TRUE(pair.linear().isApprox(pair2.linear(), 1e-12));
65  ASSERT_TRUE(pair.angular().isApprox(pair2.angular(), 1e-12));
66 
67  FrameVectorPair pair3 = pair2.changeFrameAndCopy(frame1);
68 
69  v1.changeFrame(frame1);
70  v2.changeFrame(frame1);
71 
72  ASSERT_TRUE(pair3.linear().isApprox(v1, 1e-12));
73  ASSERT_TRUE(pair3.angular().isApprox(v2, 1e-12));
74 
75  ASSERT_FALSE(pair.linear().isApprox(pair3.linear(), 1e-12));
76  ASSERT_FALSE(pair.angular().isApprox(pair3.angular(), 1e-12));
77 
78  ASSERT_FALSE(pair2.linear().isApprox(pair3.linear(), 1e-12));
79  ASSERT_FALSE(pair2.angular().isApprox(pair3.angular(), 1e-12));
80 }
81 
82 TEST_F(FrameVectorPairTest, simple2)
83 {
84  FrameVector v1(root, 3., 11., 8.);
85  FrameVector v2(root, 1., 33., 11.);
86 
87  FrameVectorPair pair(v1.getReferenceFrame(), v1.vec(), v2.vec());
88 
89  ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
90  ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
91 
92  pair.changeFrame(frame2);
93 
94  ASSERT_FALSE(pair.linear().isApprox(v1, 1e-12));
95  ASSERT_FALSE(pair.angular().isApprox(v2, 1e-12));
96 
97  v1.changeFrame(frame2);
98  v2.changeFrame(frame2);
99 
100  ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
101  ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
102 
103  FrameVectorPair pair2 = pair;
104 
105  ASSERT_TRUE(pair.linear().isApprox(pair2.linear(), 1e-12));
106  ASSERT_TRUE(pair.angular().isApprox(pair2.angular(), 1e-12));
107 
108  FrameVectorPair pair3 = pair2.changeFrameAndCopy(frame1);
109 
110  v1.changeFrame(frame1);
111  v2.changeFrame(frame1);
112 
113  ASSERT_TRUE(pair3.linear().isApprox(v1, 1e-12));
114  ASSERT_TRUE(pair3.angular().isApprox(v2, 1e-12));
115 
116  ASSERT_FALSE(pair.linear().isApprox(pair3.linear(), 1e-12));
117  ASSERT_FALSE(pair.angular().isApprox(pair3.angular(), 1e-12));
118 
119  ASSERT_FALSE(pair2.linear().isApprox(pair3.linear(), 1e-12));
120  ASSERT_FALSE(pair2.angular().isApprox(pair3.angular(), 1e-12));
121 }
122 
123 TEST_F(FrameVectorPairTest, simple3)
124 {
125  FrameVector v1(root, 3., 11., 8.);
126  FrameVector v2(root, 1., 33., 11.);
127 
128  FrameVectorPair pair;
129  pair.setIncludingFrame(v1.getReferenceFrame(), v1.vec(), v2.vec());
130 
131  ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
132  ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
133 
134  pair.changeFrame(frame2);
135 
136  ASSERT_FALSE(pair.linear().isApprox(v1, 1e-12));
137  ASSERT_FALSE(pair.angular().isApprox(v2, 1e-12));
138 
139  v1.changeFrame(frame2);
140  v2.changeFrame(frame2);
141 
142  ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
143  ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
144 
145  FrameVectorPair pair2 = pair;
146 
147  ASSERT_TRUE(pair.linear().isApprox(pair2.linear(), 1e-12));
148  ASSERT_TRUE(pair.angular().isApprox(pair2.angular(), 1e-12));
149 
150  FrameVectorPair pair3 = pair2.changeFrameAndCopy(frame1);
151 
152  v1.changeFrame(frame1);
153  v2.changeFrame(frame1);
154 
155  ASSERT_TRUE(pair3.linear().isApprox(v1, 1e-12));
156  ASSERT_TRUE(pair3.angular().isApprox(v2, 1e-12));
157 
158  ASSERT_FALSE(pair.linear().isApprox(pair3.linear(), 1e-12));
159  ASSERT_FALSE(pair.angular().isApprox(pair3.angular(), 1e-12));
160 
161  ASSERT_FALSE(pair2.linear().isApprox(pair3.linear(), 1e-12));
162  ASSERT_FALSE(pair2.angular().isApprox(pair3.angular(), 1e-12));
163 }
164 
165 TEST_F(FrameVectorPairTest, simple4)
166 {
167  FrameVector v1(root, 3., 11., 8.);
168  FrameVector v2(root, 1., 33., 11.);
169 
170  FrameVectorPair pair;
171  pair.setIncludingFrame(root, v1.vec(), v2.vec());
172 
173  ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
174  ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
175 
176  pair.changeFrame(frame2);
177 
178  ASSERT_FALSE(pair.linear().isApprox(v1, 1e-12));
179  ASSERT_FALSE(pair.angular().isApprox(v2, 1e-12));
180 
181  v1.changeFrame(frame2);
182  v2.changeFrame(frame2);
183 
184  ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
185  ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
186 
187  FrameVectorPair pair2 = pair;
188 
189  ASSERT_TRUE(pair.linear().isApprox(pair2.linear(), 1e-12));
190  ASSERT_TRUE(pair.angular().isApprox(pair2.angular(), 1e-12));
191 
192  FrameVectorPair pair3 = pair2.changeFrameAndCopy(frame1);
193 
194  v1.changeFrame(frame1);
195  v2.changeFrame(frame1);
196 
197  ASSERT_TRUE(pair3.linear().isApprox(v1, 1e-12));
198  ASSERT_TRUE(pair3.angular().isApprox(v2, 1e-12));
199 
200  ASSERT_FALSE(pair.linear().isApprox(pair3.linear(), 1e-12));
201  ASSERT_FALSE(pair.angular().isApprox(pair3.angular(), 1e-12));
202 
203  ASSERT_FALSE(pair2.linear().isApprox(pair3.linear(), 1e-12));
204  ASSERT_FALSE(pair2.angular().isApprox(pair3.angular(), 1e-12));
205 
206  FrameVector* lin_ptr = pair3.linearPtr();
207  FrameVector* ang_ptr = pair3.angularPtr();
208 
209  ASSERT_TRUE(lin_ptr->isApprox(pair3.linear(), 1e-12));
210  ASSERT_TRUE(ang_ptr->isApprox(pair3.angular(), 1e-12));
211 }
212 
213 int main(int argc, char** argv)
214 {
215  ::testing::InitGoogleTest(&argc, argv);
216  return RUN_ALL_TESTS();
217 }
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
RobotDynamics::ReferenceFrame::createARootFrame
static ReferenceFramePtr createARootFrame(const std::string &frameName)
Creates a root frame with ReferenceFrame::parentFrame=nullptr.
Definition: ReferenceFrame.hpp:258
RobotDynamics::Math::FrameVectorPair::linear
FrameVector linear() const
Get copy of linear component.
Definition: FrameVectorPair.hpp:222
FrameVectorPairTest::frame1
ReferenceFramePtr frame1
Definition: FrameVectorPairTests.cpp:33
RobotDynamics::Math::FrameVectorPair::angular
FrameVector angular() const
Get copy of angular component.
Definition: FrameVectorPair.hpp:231
TEST_F
TEST_F(FrameVectorPairTest, simple1)
Definition: FrameVectorPairTests.cpp:41
FrameVectorPairTest::frame2
ReferenceFramePtr frame2
Definition: FrameVectorPairTests.cpp:34
RobotDynamics::ReferenceFramePtr
std::shared_ptr< ReferenceFrame > ReferenceFramePtr
Definition: ReferenceFrame.hpp:72
RobotDynamics::FrameObject::changeFrame
virtual void changeFrame(ReferenceFramePtr desiredFrame)
Change the ReferenceFrame this FrameObject is expressed in.
Definition: FrameObject.cpp:12
RobotDynamics::Math::FrameVector
A FrameVector is a 3D vector with a ReferenceFrame, and all operations between FrameVectors and other...
Definition: FrameVector.hpp:33
main
int main(int argc, char **argv)
Definition: FrameVectorPairTests.cpp:213
RobotDynamics::Math::FrameVectorPair::changeFrame
void changeFrame(ReferenceFramePtr referenceFrame)
Change the frame of the two 3d vectors. Equivalent to the following math expression ...
Definition: FrameVectorPair.hpp:112
RobotDynamics::Math::FrameVectorPair::angularPtr
FrameVector * angularPtr()
Get pointer to angular vector.
Definition: FrameVectorPair.hpp:268
RobotDynamics::Math::FrameVectorPair::changeFrameAndCopy
FrameVectorPair changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
copy into new frame vector and change the frame of that
Definition: FrameVectorPair.hpp:124
RobotDynamics::Math::FrameVectorPair
A FrameVector is a pair of 3D vector with a ReferenceFrame.
Definition: FrameVectorPair.hpp:33
RobotDynamics::Math::FrameVectorPair::setIncludingFrame
void setIncludingFrame(ReferenceFramePtr referenceFrame, const Vector3d &linear, const Vector3d &angular)
Set the components and the ReferenceFrame these components are expressed in.
Definition: FrameVectorPair.hpp:156
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
RobotDynamics::Math::FrameVectorPair::linearPtr
FrameVector * linearPtr()
Get pointer to linear vector.
Definition: FrameVectorPair.hpp:259

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