RdlCalcAccelerationTests.cpp
Go to the documentation of this file.
1 #include <iostream>
2 
3 #include <gtest/gtest.h>
4 
5 #include "rdl_dynamics/Model.h"
6 #include "rdl_dynamics/Kinematics.h"
7 #include "rdl_dynamics/Dynamics.h"
8 
9 #include "UnitTestUtils.hpp"
10 
11 #include "Fixtures.h"
12 
13 using namespace std;
14 using namespace RobotDynamics;
15 using namespace RobotDynamics::Math;
16 
17 class RdlCalcAccelerationTests : public testing::Test
18 {
19  public:
20  RdlCalcAccelerationTests()
21  {
22  }
23 
24  void SetUp()
25  {
26  }
27 
28  void TearDown()
29  {
30  }
31 };
32 
33 TEST_F(FixedBase3DoF, TestCalcPointSimple)
34 {
35  QDDot[0] = 1.;
36  ref_body_id = body_a_id;
37  point_position = Vector3d(1., 0., 0.);
38  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
39 
40  EXPECT_NEAR(0., point_accel[0], unit_test_utils::TEST_PREC);
41  EXPECT_NEAR(1., point_accel[1], unit_test_utils::TEST_PREC);
42  EXPECT_NEAR(0., point_accel[2], unit_test_utils::TEST_PREC);
43 }
44 
45 TEST_F(FixedBase3DoF, TestCalcPointSimpleRotated)
46 {
47  Q[0] = 0.5 * M_PI;
48 
49  ref_body_id = body_a_id;
50  QDDot[0] = 1.;
51  point_position = Vector3d(1., 0., 0.);
52  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
53 
54  EXPECT_NEAR(-1., point_accel[0], unit_test_utils::TEST_PREC);
55  EXPECT_NEAR(0., point_accel[1], unit_test_utils::TEST_PREC);
56  EXPECT_NEAR(0., point_accel[2], unit_test_utils::TEST_PREC);
57 }
58 
59 TEST_F(FixedBase3DoF, TestCalcPointRotation)
60 {
61  ref_body_id = 1;
62  QDot[0] = 1.;
63  point_position = Vector3d(1., 0., 0.);
64  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
65 
66  EXPECT_NEAR(-1., point_accel[0], unit_test_utils::TEST_PREC);
67  EXPECT_NEAR(0., point_accel[1], unit_test_utils::TEST_PREC);
68  EXPECT_NEAR(0., point_accel[2], unit_test_utils::TEST_PREC);
69 
70  // if we are on the other side we should have the opposite value
71  point_position = Vector3d(-1., 0., 0.);
72  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
73 
74  EXPECT_NEAR(1., point_accel[0], unit_test_utils::TEST_PREC);
75  EXPECT_NEAR(0., point_accel[1], unit_test_utils::TEST_PREC);
76  EXPECT_NEAR(0., point_accel[2], unit_test_utils::TEST_PREC);
77 }
78 
79 TEST_F(FixedBase3DoF, TestCalcPointRotatedBaseSimple)
80 {
81  ref_body_id = 1;
82  Q[0] = M_PI * 0.5;
83  QDot[0] = 1.;
84  point_position = Vector3d(1., 0., 0.);
85  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
86 
87  EXPECT_NEAR(0., point_accel[0], unit_test_utils::TEST_PREC);
88  EXPECT_NEAR(-1., point_accel[1], unit_test_utils::TEST_PREC);
89  EXPECT_NEAR(0., point_accel[2], unit_test_utils::TEST_PREC);
90 
91  point_position = Vector3d(-1., 0., 0.);
92  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
93 
94  EXPECT_NEAR(0., point_accel[0], unit_test_utils::TEST_PREC);
95  EXPECT_NEAR(1., point_accel[1], unit_test_utils::TEST_PREC);
96  EXPECT_NEAR(0., point_accel[2], unit_test_utils::TEST_PREC);
97 }
98 
99 TEST_F(FixedBase3DoF, TestCalcPointRotatingBodyB)
100 {
101  // rotating second joint, point at third body
102  ref_body_id = 3;
103  QDot[1] = 1.;
104  point_position = Vector3d(1., 0., 0.);
105  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
106 
107  EXPECT_NEAR(-1., point_accel[0], unit_test_utils::TEST_PREC);
108  EXPECT_NEAR(0., point_accel[1], unit_test_utils::TEST_PREC);
109  EXPECT_NEAR(0., point_accel[2], unit_test_utils::TEST_PREC);
110 
111  // move it a bit further up (acceleration should stay the same)
112  point_position = Vector3d(1., 1., 0.);
113  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
114 
115  EXPECT_NEAR(-1., point_accel[0], unit_test_utils::TEST_PREC);
116  EXPECT_NEAR(0., point_accel[1], unit_test_utils::TEST_PREC);
117  EXPECT_NEAR(0., point_accel[2], unit_test_utils::TEST_PREC);
118 }
119 
120 TEST_F(FixedBase3DoF, TestCalcPointBodyOrigin)
121 {
122  // rotating second joint, point at third body
123 
124  QDot[0] = 1.;
125 
126  ref_body_id = body_b_id;
127  point_position = Vector3d(0., 0., 0.);
128  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
129 
130  EXPECT_NEAR(-1., point_accel[0], unit_test_utils::TEST_PREC);
131  EXPECT_NEAR(0., point_accel[1], unit_test_utils::TEST_PREC);
132  EXPECT_NEAR(0., point_accel[2], unit_test_utils::TEST_PREC);
133 }
134 
135 TEST_F(FixedBase3DoF, TestAccelerationLinearFuncOfQddot)
136 {
137  // rotating second joint, point at third body
138 
139  QDot[0] = 1.1;
140  QDot[1] = 1.3;
141  QDot[2] = 1.5;
142 
143  ref_body_id = body_c_id;
144  point_position = Vector3d(1., 1., 1.);
145 
146  VectorNd qddot_1 = VectorNd::Zero(model->dof_count);
147  VectorNd qddot_2 = VectorNd::Zero(model->dof_count);
148  VectorNd qddot_0 = VectorNd::Zero(model->dof_count);
149 
150  qddot_1[0] = 0.1;
151  qddot_1[1] = 0.2;
152  qddot_1[2] = 0.3;
153 
154  qddot_2[0] = 0.32;
155  qddot_2[1] = -0.1;
156  qddot_2[2] = 0.53;
157 
158  Vector3d acc_1 = calcPointAcceleration(*model, Q, QDot, qddot_1, ref_body_id, point_position);
159  Vector3d acc_2 = calcPointAcceleration(*model, Q, QDot, qddot_2, ref_body_id, point_position);
160 
161  MatrixNd G = MatrixNd::Zero(3, model->dof_count);
162  calcPointJacobian(*model, Q, ref_body_id, point_position, G, true);
163 
164  VectorNd net_acc = G * (qddot_1 - qddot_2);
165 
166  Vector3d acc_new = acc_1 - acc_2;
167 
168  EXPECT_NEAR(net_acc(0), acc_new(0), unit_test_utils::TEST_PREC);
169  EXPECT_NEAR(net_acc(1), acc_new(1), unit_test_utils::TEST_PREC);
170  EXPECT_NEAR(net_acc(2), acc_new(2), unit_test_utils::TEST_PREC);
171 }
172 
173 TEST_F(FloatingBase12DoF, TestAccelerationFloatingBaseWithUpdateKinematics)
174 {
175  forwardDynamics(*model, Q, QDot, Tau, QDDot);
176 
177  Vector3d accel = calcPointAcceleration(*model, Q, QDot, QDDot, child_2_rot_x_id, Vector3d(0., 0., 0.), true);
178 
179  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., -9.81, 0.), accel, unit_test_utils::TEST_PREC));
180 }
181 
182 TEST_F(FloatingBase12DoF, TestAccelerationFloatingBaseWithoutUpdateKinematics)
183 {
184  forwardDynamics(*model, Q, QDot, Tau, QDDot);
185 
186  Vector3d accel = calcPointAcceleration(*model, Q, QDot, QDDot, child_2_rot_x_id, Vector3d(0., 0., 0.), false);
187 
188  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 0., 0.), accel, unit_test_utils::TEST_PREC));
189 }
190 
191 TEST_F(FixedBase3DoF, TestCalcPointRotationFixedJoint)
192 {
193  Body fixed_body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
194  unsigned int fixed_body_id = model->addBody(body_c_id, Xtrans(Vector3d(1., -1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body");
195 
196  QDot[0] = 1.;
197  point_position = Vector3d(0., 0., 0.);
198  Vector3d point_accel_reference = calcPointAcceleration(*model, Q, QDot, QDDot, body_c_id, Vector3d(1., -1., 0.));
199 
200  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, fixed_body_id, point_position);
201 
202  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(point_accel_reference, point_accel, unit_test_utils::TEST_PREC));
203 }
204 
205 TEST_F(FixedBase3DoF, TestCalcPointRotationFixedJointRotatedTransform)
206 {
207  Body fixed_body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
208 
209  SpatialTransform fixed_transform = Xtrans(Vector3d(1., -1., 0.)) * Xrotz(M_PI * 0.5);
210  unsigned int fixed_body_id = model->addBody(body_c_id, fixed_transform, Joint(JointTypeFixed), fixed_body, "fixed_body");
211 
212  QDot[0] = 1.;
213  point_position = Vector3d(0., 0., 0.);
214 
215  Vector3d point_accel_reference = calcPointAcceleration(*model, Q, QDot, QDDot, body_c_id, Vector3d(1., 1., 0.));
216 
217  point_accel = calcPointAcceleration(*model, Q, QDot, QDDot, fixed_body_id, point_position);
218 
219  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(point_accel_reference, point_accel, unit_test_utils::TEST_PREC));
220 }
221 
222 int main(int argc, char** argv)
223 {
224  ::testing::InitGoogleTest(&argc, argv);
225  return RUN_ALL_TESTS();
226 }
RobotDynamics::calcPointJacobian
void calcPointJacobian(Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
Computes the 3D point jacobian for a point on a body.
Definition: Kinematics.cc:510
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
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
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::Math::Xrotz
SpatialTransform Xrotz(const double &zrot)
Get transform with zero translation and pure rotation about z axis.
Definition: SpatialAlgebraOperators.h:300
main
int main(int argc, char **argv)
Definition: RdlCalcAccelerationTests.cpp:222
RobotDynamics::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: rdl_eigenmath.h:337
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
TEST_F
TEST_F(FixedBase3DoF, TestCalcPointSimple)
Definition: RdlCalcAccelerationTests.cpp:33
RobotDynamics::Math
Math types such as vectors and matrices and utility functions.
Definition: ForceVector.hpp:21
RobotDynamics::Math::MatrixNd
Eigen::MatrixXd MatrixNd
Definition: rdl_eigenmath.h:26
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:27