ModelFrameTests.cpp
Go to the documentation of this file.
1 //
2 // Created by jordan on 8/14/16.
3 //
4 
5 #include "gtest/gtest.h"
6 #include "Fixtures.h"
7 #include "rdl_dynamics/FramePoint.hpp"
8 #include "rdl_dynamics/Kinematics.h"
9 #include "rdl_dynamics/Dynamics.h"
10 #include "UnitTestUtils.hpp"
11 #include <math.h>
12 
13 using namespace RobotDynamics::Math;
14 using namespace RobotDynamics;
15 
16 class ModelFrameTest : public testing::Test
17 {
18  public:
19  ModelFrameTest()
20  {
21  }
22 
23  void SetUp()
24  {
25  }
26 
27  void TearDown()
28  {
29  }
30 };
31 
32 TEST_F(FixedBase3DoFPlanar, testPlanar3LinkPendulum)
33 {
34  Q[0] = 0.;
35  Q[1] = 0.;
36  Q[2] = 0.;
37  QDot[0] = 0.;
38  QDot[1] = 0.;
39  QDot[2] = 0.;
40  QDDot[0] = 0.;
41  QDDot[1] = 0.;
42  QDDot[2] = 0.;
43 
44  updateKinematics(*model, Q, QDot, QDDot);
45 
46  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[1]->getTransformFromParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
47  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[2]->getTransformFromParent().r, Vector3d(0., 0., -1.), unit_test_utils::TEST_PREC));
48  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[3]->getTransformFromParent().r, Vector3d(0., 0., -2.), unit_test_utils::TEST_PREC));
49 
50  EXPECT_TRUE(
51  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[0]->getTransformToParent().E, unit_test_utils::TEST_PREC));
52  EXPECT_TRUE(
53  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[1]->getTransformToParent().E, unit_test_utils::TEST_PREC));
54  EXPECT_TRUE(
55  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[2]->getTransformToParent().E, unit_test_utils::TEST_PREC));
56  EXPECT_TRUE(
57  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[3]->getTransformToParent().E, unit_test_utils::TEST_PREC));
58 
59  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[0]->getTransformToParent().r, Vector3d(0, 0, 0), unit_test_utils::TEST_PREC));
60  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[1]->getTransformToParent().r, Vector3d(0, 0, 0), unit_test_utils::TEST_PREC));
61  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[2]->getTransformToParent().r, Vector3d(0, 0, 1), unit_test_utils::TEST_PREC));
62  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[3]->getTransformToParent().r, Vector3d(0, 0, 2), unit_test_utils::TEST_PREC));
63 
64  FramePoint p(model->bodyFrames[model->mBodyNameMap["body_c"]], 0., 0., -3.);
65  FramePoint p_com(model->bodyCenteredFrames[model->mBodyNameMap["body_c"]], 0., 0., 0.);
66 
67  Vector3d expected_c_com = 1 / (2 * fixed_body.mMass + body_c.mMass) *
68  ((fixed_body.mCenterOfMass + X_fixed_c.r) * fixed_body.mMass + (X_fixed_c.r + X_fixed_c2.r + fixed_body.mCenterOfMass) * fixed_body.mMass +
69  body_c.mCenterOfMass * body_c.mMass);
70  // Need to make sure that COM frames are properly updated when fixed joint bodies are involed in which bodies mass properties are combined.
71  EXPECT_NEAR(model->I[3].h[0] / model->I[3].m, expected_c_com[0], unit_test_utils::TEST_PREC);
72  EXPECT_NEAR(model->I[3].h[1] / model->I[3].m, expected_c_com[1], unit_test_utils::TEST_PREC);
73  EXPECT_NEAR(model->I[3].h[2] / model->I[3].m, expected_c_com[2], unit_test_utils::TEST_PREC);
74 
75  p.changeFrame(ReferenceFrame::getWorldFrame());
76  FramePoint p_com_point(model->bodyFrames[model->mBodyNameMap["body_c"]], expected_c_com);
77  p_com.changeFrame(ReferenceFrame::getWorldFrame());
78  p_com_point.changeFrame(model->worldFrame);
79 
80  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p.vec(), Vector3d(0., 0., -6.), unit_test_utils::TEST_PREC));
81  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p_com.vec(), p_com_point.vec(), unit_test_utils::TEST_PREC));
82 
83  p.changeFrame(model->bodyFrames[model->mBodyNameMap["body_b"]]);
84 
85  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p.vec(), Vector3d(0., 0., -5.), unit_test_utils::TEST_PREC));
86 
87  p.changeFrame(model->bodyFrames[model->mBodyNameMap["body_c"]]);
88 
89  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p.vec(), Vector3d(0., 0., -3.), unit_test_utils::TEST_PREC));
90 
91  p_com.changeFrame(model->bodyCenteredFrames[model->mBodyNameMap["body_c"]]);
92 
93  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p_com.vec(), Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
94 
95  double j1Angle = M_PI_2;
96  double j2Angle = -M_PI_2;
97  double j3Angle = 0.;
98 
99  Q[0] = j1Angle;
100  Q[1] = j2Angle;
101  Q[2] = j3Angle;
102  QDot[0] = 0.;
103  QDot[1] = 0.;
104  QDot[2] = 0.;
105  QDDot[0] = 0.;
106  QDDot[1] = 0.;
107  QDDot[2] = 0.;
108 
109  updateKinematicsCustom(*model, &Q, nullptr, nullptr);
110 
111  p_com_point.setIncludingFrame(expected_c_com, model->bodyFrames[model->mBodyNameMap["body_c"]]);
112 
113  EXPECT_TRUE(
114  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[0]->getTransformToParent().E, unit_test_utils::TEST_PREC));
115  EXPECT_TRUE(
116  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xroty(-j1Angle).E, model->bodyFrames[1]->getTransformToParent().E, unit_test_utils::TEST_PREC));
117  EXPECT_TRUE(
118  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xroty(-j2Angle).E, model->bodyFrames[2]->getTransformToParent().E, unit_test_utils::TEST_PREC));
119  EXPECT_TRUE(
120  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xroty(-j3Angle).E, model->bodyFrames[3]->getTransformToParent().E, unit_test_utils::TEST_PREC));
121 
122  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[0]->getTransformToParent().r, Vector3d(0, 0, 0), unit_test_utils::TEST_PREC));
123  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[1]->getTransformToParent().r, Vector3d(0, 0, 0), unit_test_utils::TEST_PREC));
124  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[2]->getTransformToParent().r, Vector3d(1, 0, 0), unit_test_utils::TEST_PREC));
125  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[3]->getTransformToParent().r, Vector3d(0, 0, 2), unit_test_utils::TEST_PREC));
126 
127  p.changeFrame(ReferenceFrame::getWorldFrame());
128  p_com.changeFrame(ReferenceFrame::getWorldFrame());
129  p_com_point.changeFrame(model->worldFrame);
130 
131  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p.vec(), Vector3d(-1, 0, -5), unit_test_utils::TEST_PREC));
132  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p_com.vec(), p_com_point.vec(), unit_test_utils::TEST_PREC));
133 
134  p.changeFrame(model->bodyFrames[model->mBodyNameMap["body_b"]]);
135 
136  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p.vec(), Vector3d(0, 0, -5), unit_test_utils::TEST_PREC));
137 
138  p.changeFrame(model->bodyFrames[model->mBodyNameMap["body_c"]]);
139 
140  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p.vec(), Vector3d(0, 0, -3), unit_test_utils::TEST_PREC));
141 
142  p.changeFrame(model->bodyFrames[model->mBodyNameMap["body_a"]]);
143 
144  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p.vec(), Vector3d(5, 0, -1), unit_test_utils::TEST_PREC));
145 
146  FramePoint p2(model->bodyFrames[model->mBodyNameMap["body_c"]], 1., 1., 1.);
147 
148  try
149  {
150  FrameVector v = p2 - p;
151  }
152  catch (RobotDynamics::ReferenceFrameException& e)
153  {
154  EXPECT_STREQ(e.what(), "Reference frames do not match!");
155  }
156 
157  EXPECT_STREQ(model->getReferenceFrame("body_a")->getName().c_str(), "body_a");
158  EXPECT_STREQ(model->getReferenceFrame("body_b")->getName().c_str(), "body_b");
159  EXPECT_STREQ(model->getReferenceFrame("body_c")->getName().c_str(), "body_c");
160  EXPECT_FALSE(model->getReferenceFrame("body_d"));
161 }
162 
163 TEST_F(FixedBase3DoFPlanar, testPlanar3LinkPendulumFixedBodies)
164 {
165  Q[0] = 0.;
166  Q[1] = 0.;
167  Q[2] = 0.;
168  QDot[0] = 0.;
169  QDot[1] = 0.;
170  QDot[2] = 0.;
171  QDDot[0] = 0.;
172  QDDot[1] = 0.;
173  QDDot[2] = 0.;
174 
175  updateKinematics(*model, Q, QDot, QDDot);
176 
177  FramePoint pFixedFrameOrigin(model->fixedBodyFrames[0], 0., 0., 0.);
178  FramePoint pFixedFrameOrigin_2(model->fixedBodyFrames[1], 0., 0., 0.);
179 
180  pFixedFrameOrigin.changeFrame(ReferenceFrame::getWorldFrame());
181  pFixedFrameOrigin_2.changeFrame(ReferenceFrame::getWorldFrame());
182 
183  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0.02, 0.01, -3.25), pFixedFrameOrigin.vec(), unit_test_utils::TEST_PREC));
184  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(-0.02, -0.01, -2.75), pFixedFrameOrigin_2.vec(), unit_test_utils::TEST_PREC));
185 
186  double j1Angle = M_PI_2;
187  double j2Angle = -M_PI_2;
188  double j3Angle = 0.;
189 
190  Q[0] = j1Angle;
191  Q[1] = j2Angle;
192  Q[2] = j3Angle;
193  QDot[0] = 0.;
194  QDot[1] = 0.;
195  QDot[2] = 0.;
196  QDDot[0] = 0.;
197  QDDot[1] = 0.;
198  QDDot[2] = 0.;
199 
200  updateKinematics(*model, Q, QDot, QDDot);
201 
202  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0.02, 0.01, -3.25), pFixedFrameOrigin.vec(), unit_test_utils::TEST_PREC));
203  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(-0.02, -0.01, -2.75), pFixedFrameOrigin_2.vec(), unit_test_utils::TEST_PREC));
204 
205  pFixedFrameOrigin.setIncludingFrame(0., 0., 0., model->fixedBodyFrames[0]);
206  pFixedFrameOrigin_2.setIncludingFrame(0., 0., 0., model->fixedBodyFrames[1]);
207 
208  pFixedFrameOrigin.changeFrame(ReferenceFrame::getWorldFrame());
209  pFixedFrameOrigin_2.changeFrame(ReferenceFrame::getWorldFrame());
210  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d((0.02 - 1), 0.01, -2.25), pFixedFrameOrigin.vec(), unit_test_utils::TEST_PREC));
211  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d((-0.02 - 1), -0.01, -1.75), pFixedFrameOrigin_2.vec(), unit_test_utils::TEST_PREC));
212 }
213 
214 // Kinda a weird test, but I had a bug where inverseDynamics didn't update the fixed body frames, so it gets a test.
215 TEST_F(FixedBase3DoFPlanar, testPlanar3LinkPendulumFixedBodiesUsingInverseDynamics)
216 {
217  Q[0] = 0.;
218  Q[1] = 0.;
219  Q[2] = 0.;
220  QDot[0] = 0.;
221  QDot[1] = 0.;
222  QDot[2] = 0.;
223  QDDot[0] = 0.;
224  QDDot[1] = 0.;
225  QDDot[2] = 0.;
226 
227  inverseDynamics(*model, Q, QDot, QDDot, Tau);
228 
229  FramePoint pFixedFrameOrigin(model->fixedBodyFrames[0], 0., 0., 0.);
230  FramePoint pFixedFrameOrigin_2(model->fixedBodyFrames[1], 0., 0., 0.);
231 
232  pFixedFrameOrigin.changeFrame(ReferenceFrame::getWorldFrame());
233  pFixedFrameOrigin_2.changeFrame(ReferenceFrame::getWorldFrame());
234 
235  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0.02, 0.01, -3.25), pFixedFrameOrigin.vec(), unit_test_utils::TEST_PREC));
236  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(-0.02, -0.01, -2.75), pFixedFrameOrigin_2.vec(), unit_test_utils::TEST_PREC));
237 
238  double j1Angle = M_PI_2;
239  double j2Angle = -M_PI_2;
240  double j3Angle = 0.;
241 
242  Q[0] = j1Angle;
243  Q[1] = j2Angle;
244  Q[2] = j3Angle;
245  QDot[0] = 0.;
246  QDot[1] = 0.;
247  QDot[2] = 0.;
248  QDDot[0] = 0.;
249  QDDot[1] = 0.;
250  QDDot[2] = 0.;
251 
252  inverseDynamics(*model, Q, QDot, QDDot, Tau);
253 
254  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0.02, 0.01, -3.25), pFixedFrameOrigin.vec(), unit_test_utils::TEST_PREC));
255  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(-0.02, -0.01, -2.75), pFixedFrameOrigin_2.vec(), unit_test_utils::TEST_PREC));
256 
257  pFixedFrameOrigin.setIncludingFrame(0., 0., 0., model->fixedBodyFrames[0]);
258  pFixedFrameOrigin_2.setIncludingFrame(0., 0., 0., model->fixedBodyFrames[1]);
259 
260  pFixedFrameOrigin.changeFrame(ReferenceFrame::getWorldFrame());
261  pFixedFrameOrigin_2.changeFrame(ReferenceFrame::getWorldFrame());
262  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d((0.02 - 1), 0.01, -2.25), pFixedFrameOrigin.vec(), unit_test_utils::TEST_PREC));
263  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d((-0.02 - 1), -0.01, -1.75), pFixedFrameOrigin_2.vec(), unit_test_utils::TEST_PREC));
264 }
265 
266 TEST_F(FixedBaseTwoChain6DoF3D, testPointChangeFrameWith2ChainModel)
267 {
268  Q[0] = 0.;
269  Q[1] = 0.;
270  Q[2] = 0.;
271  Q[3] = 0.;
272  Q[4] = 0.;
273  Q[5] = 0.;
274  QDot[0] = 0.;
275  QDot[1] = 0.;
276  QDot[2] = 0.;
277  QDot[3] = 0.;
278  QDot[4] = 0.;
279  QDot[5] = 0.;
280  QDDot[0] = 0.;
281  QDDot[1] = 0.;
282  QDDot[2] = 0.;
283  QDDot[3] = 0.;
284  QDDot[4] = 0.;
285  QDDot[5] = 0.;
286 
287  updateKinematics(*model, Q, QDot, QDDot);
288 
289  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[1]->getTransformFromParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
290  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[2]->getTransformFromParent().r, Vector3d(0., 0., -1.), unit_test_utils::TEST_PREC));
291  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[3]->getTransformFromParent().r, Vector3d(0., -2., 0.), unit_test_utils::TEST_PREC));
292  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[4]->getTransformFromParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
293  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[5]->getTransformFromParent().r, Vector3d(2., 0., 0.), unit_test_utils::TEST_PREC));
294  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[6]->getTransformFromParent().r, Vector3d(0., 0., 1.), unit_test_utils::TEST_PREC));
295 
296  EXPECT_TRUE(
297  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[0]->getTransformToParent().E, unit_test_utils::TEST_PREC));
298  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xrotx(0.).E, model->bodyFrames[1]->getTransformToParent().E, unit_test_utils::TEST_PREC));
299  EXPECT_TRUE(
300  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xrotx(-M_PI_2).E, model->bodyFrames[2]->getTransformToParent().E, unit_test_utils::TEST_PREC));
301  EXPECT_TRUE(
302  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xrotx(-M_PI_2).E, model->bodyFrames[3]->getTransformToParent().E, unit_test_utils::TEST_PREC));
303 
304  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xrotx(0.).E, model->bodyFrames[4]->getTransformToParent().E, unit_test_utils::TEST_PREC));
305  EXPECT_TRUE(
306  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xrotz(-M_PI_2).E, model->bodyFrames[5]->getTransformToParent().E, unit_test_utils::TEST_PREC));
307  EXPECT_TRUE(
308  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xrotx(-M_PI_2).E, model->bodyFrames[6]->getTransformToParent().E, unit_test_utils::TEST_PREC));
309 
310  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[0]->getTransformToParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
311  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[1]->getTransformToParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
312  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[2]->getTransformToParent().r, Vector3d(0., 1., 0.), unit_test_utils::TEST_PREC));
313  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[3]->getTransformToParent().r, Vector3d(0., 0., -2.), unit_test_utils::TEST_PREC));
314  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[4]->getTransformToParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
315  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[5]->getTransformToParent().r, Vector3d(0., 2., 0.), unit_test_utils::TEST_PREC));
316  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[6]->getTransformToParent().r, Vector3d(0., -1., 0.), unit_test_utils::TEST_PREC));
317 
318  Math::FramePoint p(model->bodyFrames[6], 3., 0., 0.);
319  Math::FramePoint p_com(model->bodyCenteredFrames[6], 0., 0., 0.);
320 
321  p.changeFrame(model->bodyFrames[6]);
322  p_com.changeFrame(model->bodyCenteredFrames[6]);
323  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p.vec(), Vector3d(3., 0., 0.), unit_test_utils::TEST_PREC));
324  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p_com.vec(), Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
325 
326  p.changeFrame(ReferenceFrame::getWorldFrame());
327  p_com.changeFrame(ReferenceFrame::getWorldFrame());
328  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p.vec(), Vector3d(2., 3., 1.), unit_test_utils::TEST_PREC));
329  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p_com.vec(), Vector3d(2.5, 0., 1.), unit_test_utils::TEST_PREC));
330 
331  p.changeFrame(model->bodyFrames[3]);
332  p_com.changeFrame(model->bodyFrames[3]);
333  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p.vec(), Vector3d(2., -3., -4.), unit_test_utils::TEST_PREC));
334  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p_com.vec(), Vector3d(2.5, 0., -4.), unit_test_utils::TEST_PREC));
335 
336  Q[0] = M_PI_2;
337 
338  updateKinematics(*model, Q, QDot, QDDot);
339 
340  Math::FramePoint p2(model->bodyFrames[6], 3., 0., 0.);
341 
342  p2.changeFrame(model->bodyFrames[3]);
343 
344  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p2.vec(), Vector3d(2., -1., 0.), unit_test_utils::TEST_PREC));
345 }
346 
347 TEST_F(FloatingBaseWith2SingleDofJoints, testPointChangeFrames)
348 {
349  Q[0] = 0.;
350  Q[1] = 0.;
351  Q[2] = 0.;
352  Q[3] = 0.;
353  Q[4] = 0.;
354  Q[5] = 0.;
355  Q[6] = 0.;
356  Q[7] = 0.;
357  Q[8] = 1.;
358  QDot[0] = 0.;
359  QDot[1] = 0.;
360  QDot[2] = 0.;
361  QDot[3] = 0.;
362  QDot[4] = 0.;
363  QDot[5] = 0.;
364  QDDot[0] = 0.;
365  QDDot[1] = 0.;
366  QDDot[2] = 0.;
367  QDDot[3] = 0.;
368  QDDot[4] = 0.;
369  QDDot[5] = 0.;
370 
371  updateKinematics(*model, Q, QDot, QDDot);
372 
373  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[0]->getTransformFromParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
374  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[1]->getTransformFromParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
375  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[2]->getTransformFromParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
376  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[3]->getTransformFromParent().r, Vector3d(-1.5, 0., 0.), unit_test_utils::TEST_PREC));
377  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[4]->getTransformFromParent().r, Vector3d(1.5, 0., 0.), unit_test_utils::TEST_PREC));
378 
379  EXPECT_TRUE(
380  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[0]->getTransformFromParent().E, unit_test_utils::TEST_PREC));
381  EXPECT_TRUE(
382  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[1]->getTransformFromParent().E, unit_test_utils::TEST_PREC));
383  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xtrans(Vector3d(0., 0., 0.)).E, model->bodyFrames[2]->getTransformFromParent().E,
384  unit_test_utils::TEST_PREC));
385  EXPECT_TRUE(
386  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[3]->getTransformFromParent().E, unit_test_utils::TEST_PREC));
387  EXPECT_TRUE(
388  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xrotz(M_PI_2).E, model->bodyFrames[4]->getTransformFromParent().E, unit_test_utils::TEST_PREC));
389 
390  EXPECT_TRUE(
391  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[0]->getTransformToParent().E, unit_test_utils::TEST_PREC));
392  EXPECT_TRUE(
393  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[1]->getTransformToParent().E, unit_test_utils::TEST_PREC));
394  EXPECT_TRUE(
395  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[2]->getTransformToParent().E, unit_test_utils::TEST_PREC));
396  EXPECT_TRUE(
397  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Matrix3dIdentity, model->bodyFrames[3]->getTransformToParent().E, unit_test_utils::TEST_PREC));
398  EXPECT_TRUE(
399  unit_test_utils::checkMatrix3dEpsilonClose(RobotDynamics::Math::Xrotz(-M_PI_2).E, model->bodyFrames[4]->getTransformToParent().E, unit_test_utils::TEST_PREC));
400 
401  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[0]->getTransformToParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
402  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[1]->getTransformToParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
403  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[2]->getTransformToParent().r, Vector3d(0., 0., 0.), unit_test_utils::TEST_PREC));
404  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[3]->getTransformToParent().r, Vector3d(1.5, 0., 0.), unit_test_utils::TEST_PREC));
405  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(model->bodyFrames[4]->getTransformToParent().r, Vector3d(0., 1.5, 0.), unit_test_utils::TEST_PREC));
406 
407  FramePoint p1_end_effector(model->bodyFrames[model->mBodyNameMap["body_1"]], 0., 0., -1.);
408  FramePoint p2_end_effector(model->bodyFrames[model->mBodyNameMap["body_2"]], 0., 0., 1.);
409  FramePoint root_origin(model->bodyFrames[model->mBodyNameMap["floating_body"]], 0., 0., 0.);
410  FramePoint world_origin(ReferenceFrame::getWorldFrame(), 0., 0., 0.);
411 
412  p1_end_effector.changeFrame(ReferenceFrame::getWorldFrame());
413  p2_end_effector.changeFrame(model->bodyFrames[model->mBodyNameMap["body_1"]]);
414  root_origin.changeFrame(model->bodyFrames[model->mBodyNameMap["body_2"]]);
415 
416  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p1_end_effector.vec(), Vector3d(-1.5, 0., -1.), unit_test_utils::TEST_PREC));
417  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(p2_end_effector.vec(), Vector3d(3., 0., 1.), unit_test_utils::TEST_PREC));
418  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(root_origin.vec(), Vector3d(0., 1.5, 0.), unit_test_utils::TEST_PREC));
419 
420  Q[0] = 1.;
421  Q[1] = 2.;
422  Q[2] = 3.;
423 
424  updateKinematics(*model, Q, QDot, QDDot);
425 
426  world_origin.changeFrame(model->bodyFrames[model->mBodyNameMap["floating_body"]]);
427 
428  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(world_origin.vec(), Vector3d(-1., -2., -3.), unit_test_utils::TEST_PREC));
429 
430  double a = M_PI_2;
431  Q[0] = 1.;
432  Q[1] = 2.;
433  Q[2] = 3.;
434  Q[3] = 0.;
435  Q[4] = 0.;
436  Q[5] = sin(a / 2);
437  Q[6] = 0.;
438  Q[7] = 0.;
439  Q[8] = cos(a / 2);
440 
441  world_origin.setIncludingFrame(0., 0., 0., ReferenceFrame::getWorldFrame());
442  updateKinematics(*model, Q, QDot, QDDot);
443 
444  root_origin.changeFrame(ReferenceFrame::getWorldFrame());
445 
446  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(root_origin.vec(), Vector3d(1., 2., 3.), unit_test_utils::TEST_PREC));
447 
448  world_origin.changeFrame(model->bodyFrames[model->mBodyNameMap["floating_body"]]);
449 
450  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(world_origin.vec(), Vector3d(-2., 1., -3.), unit_test_utils::TEST_PREC));
451 }
452 
453 TEST_F(FixedAndMovableJoint, testComFramesWithFixedJoint)
454 {
455  EXPECT_TRUE(model_fixed->bodyCenteredFrames[1]->getTransformToParent().r[0] == -1.);
456  EXPECT_TRUE(model_fixed->bodyCenteredFrames[1]->getTransformToParent().r[1] == -0.5);
457  EXPECT_TRUE(model_fixed->bodyCenteredFrames[1]->getTransformToParent().r[2] == 0.);
458 }
459 
460 TEST_F(ModelFrameTest, movableBodyWithParentBodyBeingFixed)
461 {
462  RobotDynamics::Model model;
463  RobotDynamics::Body b(1., RobotDynamics::Math::Vector3dZero, RobotDynamics::Math::Vector3d(1., 1., 1.));
464 
465  RobotDynamics::Joint j(JointTypeRevoluteX);
466 
467  model.appendBody(RobotDynamics::Math::SpatialTransform(), j, b, "b1");
468  model.appendBody(RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(0., 0., 1.)), j, b, "b2");
469  unsigned int fbid = model.appendBody(RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(0., -1., 1.)),
470  RobotDynamics::Joint(RobotDynamics::JointType::JointTypeFixed), b, "b3");
471  model.addBody(fbid, RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(0., 0., 1.)), j, b, "b4");
472 
473  EXPECT_TRUE(model.referenceFrameMap["b4"]->getTransformToRoot().r.isApprox(RobotDynamics::Math::Vector3d(0., 1., -3.), 1.e-10));
474 }
475 
476 int main(int argc, char** argv)
477 {
478  ::testing::InitGoogleTest(&argc, argv);
479  return RUN_ALL_TESTS();
480 }
FramePoint.hpp
File containing the FramePoint<T> object definition.
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::Body
Describes all properties of a single body.
Definition: Body.h:30
Tau
VectorNd Tau
Definition: RdlTwoLegModelTests.cpp:32
QDDot
VectorNd QDDot
Definition: RdlTwoLegModelTests.cpp:31
RobotDynamics::ReferenceFrameException
A custom exception for frame operations.
Definition: FrameExceptions.hpp:30
RobotDynamics::Math::Matrix3dIdentity
Matrix3d Matrix3dIdentity
RobotDynamics::Math::Xroty
SpatialTransform Xroty(const double &yrot)
Get transform with zero translation and pure rotation about y axis.
Definition: SpatialAlgebraOperators.h:286
QDot
VectorNd QDot
Definition: RdlTwoLegModelTests.cpp:30
FixedAndMovableJoint
Fixture that contains two models of which one has one joint fixed.
Definition: Fixtures.h:676
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
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
RobotDynamics::JointTypeFixed
Fixed joint which causes the inertial properties to be merged with.
Definition: Joint.h:205
TEST_F
TEST_F(FixedBase3DoFPlanar, testPlanar3LinkPendulum)
Definition: ModelFrameTests.cpp:32
RobotDynamics::ReferenceFrame::getWorldFrame
static ReferenceFramePtr getWorldFrame()
Get a pointer to the world frame.
Definition: ReferenceFrame.hpp:267
RobotDynamics::Model::appendBody
unsigned int appendBody(const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Adds a Body to the model such that the previously added Body is the Parent.
Definition: Model.cc:517
RobotDynamics::ReferenceFrameException::what
virtual const char * what() const
Definition: FrameExceptions.hpp:41
RobotDynamics::Model::addBody
unsigned int addBody(const unsigned int parent_id, const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Connects a given body to the model.
Definition: Model.cc:271
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::Math::Point3d::vec
EIGEN_STRONG_INLINE Math::Vector3d vec() const
Definition: Point3.hpp:269
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
A FrameVector is a 3D vector with a ReferenceFrame, and all operations between FrameVectors and other...
Definition: FrameVector.hpp:33
RobotDynamics::Joint
Describes a joint relative to the predecessor body.
Definition: Joint.h:228
RobotDynamics::Math::Xrotx
SpatialTransform Xrotx(const double &xrot)
Get transform with zero translation and pure rotation about x axis.
Definition: SpatialAlgebraOperators.h:272
RobotDynamics::updateKinematics
void updateKinematics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
Updates and computes velocities and accelerations of the bodies.
Definition: Kinematics.cc:25
RobotDynamics::Model::referenceFrameMap
std::map< std::string, ReferenceFramePtr > referenceFrameMap
Definition: Model.h:327
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
RobotDynamics::updateKinematicsCustom
void updateKinematicsCustom(Model &model, const Math::VectorNd *Q, const Math::VectorNd *QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
Selectively updates model internal states of body positions, velocities and/or accelerations.
Definition: Kinematics.cc:72
main
int main(int argc, char **argv)
Definition: ModelFrameTests.cpp:476
RobotDynamics::Math
Math types such as vectors and matrices and utility functions.
Definition: ForceVector.hpp:21
unit_test_utils::checkVector3dEpsilonClose
static bool checkVector3dEpsilonClose(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:256
RobotDynamics::inverseDynamics
void inverseDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, Math::VectorNd &Tau, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
Definition: Dynamics.cc:23
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

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