RdlFloatingBaseTests.cpp
Go to the documentation of this file.
1 #include <iostream>
2 
3 #include <gtest/gtest.h>
4 
5 #include "rdl_dynamics/Dynamics.h"
6 #include "rdl_dynamics/FramePoint.hpp"
7 #include "rdl_dynamics/Kinematics.h"
8 #include "rdl_dynamics/Model.h"
9 #include "UnitTestUtils.hpp"
10 
11 using namespace std;
12 using namespace RobotDynamics;
13 using namespace RobotDynamics::Math;
14 
15 struct FloatingBaseTestFixture : public testing::Test
16 {
17  FloatingBaseTestFixture()
18  {
19  }
20 
21  void SetUp()
22  {
23  model = new Model;
24  model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
25 
26  base = Body(1., Vector3d(1., 0., 0.), Vector3d(1., 1., 1.));
27  }
28 
29  void TearDown()
30  {
31  delete model;
32  }
33 
34  Model* model;
35  Body base;
36  unsigned int base_body_id;
37 
38  VectorNd q, qdot, qddot, tau;
39 };
40 
41 TEST_F(FloatingBaseTestFixture, TestCalcPointTransformation)
42 {
43  base_body_id = model->addBody(0, SpatialTransform(),
44  Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
45  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.)),
46  base);
47 
48  q = VectorNd::Constant(model->dof_count, 0.);
49  qdot = VectorNd::Constant(model->dof_count, 0.);
50  qddot = VectorNd::Constant(model->dof_count, 0.);
51  tau = VectorNd::Constant(model->dof_count, 0.);
52 
53  q[1] = 1.;
54  forwardDynamics(*model, q, qdot, tau, qddot);
55 
56  // test_point = calcBaseToBodyCoordinates(*model, q, base_body_id, Vector3d(0., 0., 0.), false);
57  FramePoint p(model->worldFrame, Vector3d::Zero());
58  p.changeFrame(model->bodyFrames[base_body_id]);
59 
60  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., -1., 0.), p.vec(), unit_test_utils::TEST_PREC));
61 }
62 
63 TEST_F(FloatingBaseTestFixture, TestCalcDynamicFloatingBaseDoubleImplicit)
64 {
65  // floating base
66  base_body_id = model->addBody(0, SpatialTransform(),
67  Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
68  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.)),
69  base);
70 
71  // body_a
72  Body body_a(1., Vector3d(1., 0., 0), Vector3d(1., 1., 1.));
73  Joint joint_a(SpatialVector(0., 0., 1., 0., 0., 0.));
74 
75  model->addBody(base_body_id, Xtrans(Vector3d(2., 0., 0.)), joint_a, body_a);
76 
77  // Initialization of the input vectors
78  VectorNd Q = VectorNd::Zero((size_t)model->dof_count);
79  VectorNd QDot = VectorNd::Zero((size_t)model->dof_count);
80  VectorNd QDDot = VectorNd::Zero((size_t)model->dof_count);
81  VectorNd Tau = VectorNd::Zero((size_t)model->dof_count);
82 
83  forwardDynamics(*model, Q, QDot, Tau, QDDot);
84 
85  EXPECT_NEAR(0.0000, QDDot[0], unit_test_utils::TEST_PREC);
86  EXPECT_NEAR(-9.8100, QDDot[1], unit_test_utils::TEST_PREC);
87  EXPECT_NEAR(0.0000, QDDot[2], unit_test_utils::TEST_PREC);
88  EXPECT_NEAR(0.0000, QDDot[3], unit_test_utils::TEST_PREC);
89  EXPECT_NEAR(0.0000, QDDot[4], unit_test_utils::TEST_PREC);
90  EXPECT_NEAR(0.0000, QDDot[5], unit_test_utils::TEST_PREC);
91  EXPECT_NEAR(0.0000, QDDot[6], unit_test_utils::TEST_PREC);
92 
93  // We rotate the base... let's see what happens...
94  Q[3] = 0.8;
95  forwardDynamics(*model, Q, QDot, Tau, QDDot);
96 
97  EXPECT_NEAR(0.0000, QDDot[0], unit_test_utils::TEST_PREC);
98  EXPECT_NEAR(-9.8100, QDDot[1], unit_test_utils::TEST_PREC);
99  EXPECT_NEAR(0.0000, QDDot[2], unit_test_utils::TEST_PREC);
100  EXPECT_NEAR(0.0000, QDDot[3], unit_test_utils::TEST_PREC);
101  EXPECT_NEAR(0.0000, QDDot[4], unit_test_utils::TEST_PREC);
102  EXPECT_NEAR(0.0000, QDDot[5], unit_test_utils::TEST_PREC);
103  EXPECT_NEAR(0.0000, QDDot[6], unit_test_utils::TEST_PREC);
104 
105  // We apply a torqe let's see what happens...
106  Q[3] = 0.;
107  Tau[6] = 1.;
108 
109  forwardDynamics(*model, Q, QDot, Tau, QDDot);
110 
111  EXPECT_NEAR(0.0000, QDDot[0], unit_test_utils::TEST_PREC);
112  EXPECT_NEAR(-8.8100, QDDot[1], unit_test_utils::TEST_PREC);
113  EXPECT_NEAR(0.0000, QDDot[2], unit_test_utils::TEST_PREC);
114  EXPECT_NEAR(-1.0000, QDDot[3], unit_test_utils::TEST_PREC);
115  EXPECT_NEAR(0.0000, QDDot[4], unit_test_utils::TEST_PREC);
116  EXPECT_NEAR(0.0000, QDDot[5], unit_test_utils::TEST_PREC);
117  EXPECT_NEAR(2.0000, QDDot[6], unit_test_utils::TEST_PREC);
118 }
119 
120 TEST_F(FloatingBaseTestFixture, TestCalcPointVelocityFloatingBaseSimple)
121 {
122  // floating base
123  base_body_id = model->addBody(0, SpatialTransform(),
124  Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
125  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.)),
126  base);
127 
128  VectorNd Q = VectorNd::Zero(model->dof_count);
129  VectorNd QDot = VectorNd::Zero(model->dof_count);
130  VectorNd QDDot = VectorNd::Zero(model->dof_count);
131  VectorNd Tau = VectorNd::Zero(model->dof_count);
132 
133  unsigned int ref_body_id = base_body_id;
134 
135  // first we calculate the velocity when moving along the X axis
136  QDot[0] = 1.;
137  Vector3d point_position(1., 0., 0.);
138  Vector3d point_velocity;
139 
140  point_velocity = calcPointVelocity(*model, Q, QDot, ref_body_id, point_position);
141 
142  EXPECT_NEAR(1., point_velocity[0], unit_test_utils::TEST_PREC);
143  EXPECT_NEAR(0., point_velocity[1], unit_test_utils::TEST_PREC);
144  EXPECT_NEAR(0., point_velocity[2], unit_test_utils::TEST_PREC);
145 
146  // Now we calculate the velocity when rotating around the Z axis
147  QDot[0] = 0.;
148  QDot[3] = 1.;
149 
150  point_velocity = calcPointVelocity(*model, Q, QDot, ref_body_id, point_position);
151 
152  EXPECT_NEAR(0., point_velocity[0], unit_test_utils::TEST_PREC);
153  EXPECT_NEAR(1., point_velocity[1], unit_test_utils::TEST_PREC);
154  EXPECT_NEAR(0., point_velocity[2], unit_test_utils::TEST_PREC);
155 
156  // Now we calculate the velocity when rotating around the Z axis and the
157  // base is rotated around the z axis by 90 degrees
158  Q[3] = M_PI * 0.5;
159  QDot[3] = 1.;
160 
161  point_velocity = calcPointVelocity(*model, Q, QDot, ref_body_id, point_position);
162 
163  EXPECT_NEAR(-1., point_velocity[0], unit_test_utils::TEST_PREC);
164  EXPECT_NEAR(0., point_velocity[1], unit_test_utils::TEST_PREC);
165  EXPECT_NEAR(0., point_velocity[2], unit_test_utils::TEST_PREC);
166 }
167 
168 TEST_F(FloatingBaseTestFixture, TestCalcPointVelocityCustom)
169 {
170  // floating base
171  base = Body(1., Vector3d(0., 1., 0.), Vector3d(1., 1., 1.));
172  base_body_id = model->addBody(0, SpatialTransform(),
173  Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
174  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.)),
175  base);
176 
177  VectorNd q = VectorNd::Zero(model->dof_count);
178  VectorNd qdot = VectorNd::Zero(model->dof_count);
179  VectorNd qddot = VectorNd::Zero(model->dof_count);
180  VectorNd tau = VectorNd::Zero(model->dof_count);
181 
182  unsigned int ref_body_id = base_body_id;
183 
184  q[0] = 0.1;
185  q[1] = 1.1;
186  q[2] = 1.2;
187  q[3] = 1.3;
188  q[4] = 1.5;
189  q[5] = 1.7;
190 
191  qdot[0] = 0.1;
192  qdot[1] = 1.1;
193  qdot[2] = 1.2;
194  qdot[3] = 1.3;
195  qdot[4] = 1.5;
196  qdot[5] = 1.7;
197 
198  // first we calculate the velocity when rotating around the Z axis
199  Vector3d point_body_position(1., 0., 0.);
200  Vector3d point_base_position;
201  Vector3d point_base_velocity;
202  Vector3d point_base_velocity_reference;
203 
204  forwardDynamics(*model, q, qdot, tau, qddot);
205 
206  point_base_velocity = calcPointVelocity(*model, q, qdot, ref_body_id, point_body_position);
207 
208  point_base_velocity_reference = Vector3d(-3.888503432977729e-01, -3.171179347202455e-01, 1.093894197498446e+00);
209 
210  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(point_base_velocity_reference, point_base_velocity, unit_test_utils::TEST_PREC));
211 }
212 
222 TEST_F(FloatingBaseTestFixture, TestCalcPointAccelerationNoQDDot)
223 {
224  // floating base
225  base = Body(1., Vector3d(0., 1., 0.), Vector3d(1., 1., 1.));
226  base_body_id = model->addBody(0, SpatialTransform(),
227  Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
228  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.)),
229  base);
230 
231  VectorNd q = VectorNd::Zero(model->dof_count);
232  VectorNd qdot = VectorNd::Zero(model->dof_count);
233  VectorNd qddot = VectorNd::Zero(model->dof_count);
234  VectorNd tau = VectorNd::Zero(model->dof_count);
235 
236  unsigned int ref_body_id = base_body_id;
237 
238  q[0] = 0.1;
239  q[1] = 1.1;
240  q[2] = 1.2;
241  q[3] = 1.3;
242  q[4] = 1.5;
243  q[5] = 1.7;
244 
245  qdot[0] = 0.1;
246  qdot[1] = 1.1;
247  qdot[2] = 1.2;
248  qdot[3] = 1.3;
249  qdot[4] = 1.5;
250  qdot[5] = 1.7;
251 
252  // first we calculate the velocity when rotating around the Z axis
253  Vector3d point_body_position(-1.9, -1.8, 0.);
254  Vector3d point_world_velocity;
255  Vector3d point_world_acceleration;
256 
257  // call ForwardDynamics to update the model
258  forwardDynamics(*model, q, qdot, tau, qddot);
259  qddot = VectorNd::Zero(qddot.size());
260 
261  qdot = qdot;
262 
263  FramePoint point_world_position(model->bodyFrames[ref_body_id], point_body_position);
264  point_world_position.changeFrame(model->worldFrame);
265  point_world_velocity = calcPointVelocity(*model, q, qdot, ref_body_id, point_body_position);
266 
267  point_world_acceleration = calcPointAcceleration(*model, q, qdot, qddot, ref_body_id, point_body_position);
268 
269  Vector3d humans_point_position(-6.357089363622626e-01, -6.831041744630977e-01, 2.968974805916970e+00);
270  Vector3d humans_point_velocity(3.091226260907569e-01, 3.891012095550828e+00, 4.100277995030419e+00);
271  Vector3d humans_point_acceleration(-5.302760158847160e+00, 6.541369639625232e+00, -4.795115077652286e+00);
272 
273  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(humans_point_position, point_world_position.vec(), unit_test_utils::TEST_PREC));
274  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(humans_point_velocity, point_world_velocity, unit_test_utils::TEST_PREC));
275  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(humans_point_acceleration, point_world_acceleration, unit_test_utils::TEST_PREC));
276 }
277 
288 TEST_F(FloatingBaseTestFixture, TestCalcPointAccelerationOnlyQDDot)
289 {
290  // floating base
291  base = Body(1., Vector3d(0., 1., 0.), Vector3d(1., 1., 1.));
292  base_body_id = model->addBody(0, SpatialTransform(),
293  Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
294  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.)),
295  base);
296 
297  VectorNd q = VectorNd::Zero(model->dof_count);
298  VectorNd qdot = VectorNd::Zero(model->dof_count);
299  VectorNd qddot = VectorNd::Zero(model->dof_count);
300  VectorNd tau = VectorNd::Zero(model->dof_count);
301 
302  unsigned int ref_body_id = base_body_id;
303 
304  // first we calculate the velocity when rotating around the Z axis
305  Vector3d point_body_position(-1.9, -1.8, 0.);
306  Vector3d point_world_velocity;
307  Vector3d point_world_acceleration;
308 
309  forwardDynamics(*model, q, qdot, tau, qddot);
310 
311  qddot = VectorNd::Zero(qddot.size());
312 
313  qddot[0] = 0.1;
314  qddot[1] = 1.1;
315  qddot[2] = 1.2;
316  qddot[3] = 1.3;
317  qddot[4] = 1.5;
318  qddot[5] = 1.7;
319 
320  FramePoint point_world_position(model->bodyFrames[ref_body_id], point_body_position);
321  point_world_position.changeFrame(model->worldFrame);
322  point_world_velocity = calcPointVelocity(*model, q, qdot, ref_body_id, point_body_position);
323 
324  point_world_acceleration = calcPointAcceleration(*model, q, qdot, qddot, ref_body_id, point_body_position);
325 
326  Vector3d humans_point_position(-1.900000000000000e+00, -1.800000000000000e+00, 0.000000000000000e+00);
327  Vector3d humans_point_velocity(0.000000000000000e+00, 0.000000000000000e+00, 0.000000000000000e+00);
328  Vector3d humans_point_acceleration(2.440000000000000e+00, -1.370000000000000e+00, 9.899999999999999e-01);
329 
330  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(humans_point_position, point_world_position.vec(), unit_test_utils::TEST_PREC));
331  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(humans_point_velocity, point_world_velocity, unit_test_utils::TEST_PREC));
332  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(humans_point_acceleration, point_world_acceleration, unit_test_utils::TEST_PREC));
333 }
334 
345 TEST_F(FloatingBaseTestFixture, TestCalcPointAccelerationFull)
346 {
347  // floating base
348  base = Body(1., Vector3d(0., 1., 0.), Vector3d(1., 1., 1.));
349  base_body_id = model->addBody(0, SpatialTransform(),
350  Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
351  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.)),
352  base);
353 
354  VectorNd q = VectorNd::Zero(model->dof_count);
355  VectorNd qdot = VectorNd::Zero(model->dof_count);
356  VectorNd qddot = VectorNd::Zero(model->dof_count);
357  VectorNd tau = VectorNd::Zero(model->dof_count);
358 
359  unsigned int ref_body_id = base_body_id;
360 
361  // first we calculate the velocity when rotating around the Z axis
362  Vector3d point_body_position(-1.9, -1.8, 0.);
363  Vector3d point_world_velocity;
364  Vector3d point_world_acceleration;
365 
366  q[0] = 0.1;
367  q[1] = 1.1;
368  q[2] = 1.2;
369  q[3] = 1.3;
370  q[4] = 1.5;
371  q[5] = 1.7;
372 
373  qdot[0] = 0.1;
374  qdot[1] = 1.1;
375  qdot[2] = 1.2;
376  qdot[3] = 1.3;
377  qdot[4] = 1.5;
378  qdot[5] = 1.7;
379 
380  forwardDynamics(*model, q, qdot, tau, qddot);
381 
382  qddot[0] = 0.1;
383  qddot[1] = 1.1;
384  qddot[2] = 1.2;
385  qddot[3] = 1.3;
386  qddot[4] = 1.5;
387  qddot[5] = 1.7;
388 
389  // cout << "ref_body_id = " << ref_body_id << endl;
390  // cout << "point_body_position = " << point_body_position << endl;
391  FramePoint point_world_position(model->bodyFrames[ref_body_id], point_body_position);
392  point_world_position.changeFrame(model->worldFrame);
393  point_world_velocity = calcPointVelocity(*model, q, qdot, ref_body_id, point_body_position);
394 
395  point_world_acceleration = calcPointAcceleration(*model, q, qdot, qddot, ref_body_id, point_body_position);
396 
397  Vector3d humans_point_position(-6.357089363622626e-01, -6.831041744630977e-01, 2.968974805916970e+00);
398  Vector3d humans_point_velocity(3.091226260907569e-01, 3.891012095550828e+00, 4.100277995030419e+00);
399  Vector3d humans_point_acceleration(-4.993637532756404e+00, 1.043238173517606e+01, -6.948370826218673e-01);
400 
401  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(humans_point_position, point_world_position.vec(), unit_test_utils::TEST_PREC));
402  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(humans_point_velocity, point_world_velocity, unit_test_utils::TEST_PREC));
403  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(humans_point_acceleration, point_world_acceleration, unit_test_utils::TEST_PREC));
404 }
405 
406 int main(int argc, char** argv)
407 {
408  ::testing::InitGoogleTest(&argc, argv);
409  return RUN_ALL_TESTS();
410 }
TEST_F
TEST_F(FloatingBaseTestFixture, TestCalcPointTransformation)
Definition: RdlFloatingBaseTests.cpp:41
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
QDot
VectorNd QDot
Definition: RdlTwoLegModelTests.cpp:30
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
RobotDynamics::Math::Xtrans
SpatialTransform Xtrans(const Vector3d &r)
Get pure translation transform.
Definition: SpatialAlgebraOperators.h:369
RobotDynamics::calcPointAcceleration
Math::FrameVector calcPointAcceleration(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes the linear acceleration of a point on a body.
Definition: Kinematics.cc:1593
RobotDynamics::Model::gravity
Math::MotionVector gravity
the cartesian vector of the gravity
Definition: Model.h:196
main
int main(int argc, char **argv)
Definition: RdlFloatingBaseTests.cpp:406
RobotDynamics::Math::Point3d::vec
EIGEN_STRONG_INLINE Math::Vector3d vec() const
Definition: Point3.hpp:269
RobotDynamics::calcPointVelocity
Math::FrameVector calcPointVelocity(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes the velocity of a point on a body.
Definition: Kinematics.cc:1422
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::Joint
Describes a joint relative to the predecessor body.
Definition: Joint.h:228
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
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::forwardDynamics
void forwardDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
Computes forward dynamics with the Articulated Body Algorithm.
Definition: Dynamics.cc:455
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:28