GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/rnea.cpp Lines: 156 156 100.0 %
Date: 2024-04-26 13:14:21 Branches: 527 1048 50.3 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2020 CNRS INRIA
3
//
4
5
/*
6
 * Unittest of the RNE algorithm. The code simply test that the algorithm does
7
 * not cause any serious errors. The numerical values are not cross validated
8
 * in any way.
9
 *
10
 */
11
12
#include "pinocchio/spatial/fwd.hpp"
13
#include "pinocchio/multibody/model.hpp"
14
#include "pinocchio/multibody/data.hpp"
15
#include "pinocchio/algorithm/rnea.hpp"
16
#include "pinocchio/algorithm/jacobian.hpp"
17
#include "pinocchio/algorithm/center-of-mass.hpp"
18
#include "pinocchio/algorithm/joint-configuration.hpp"
19
#include "pinocchio/algorithm/crba.hpp"
20
#include "pinocchio/algorithm/centroidal.hpp"
21
#include "pinocchio/parsers/sample-models.hpp"
22
#include "pinocchio/utils/timer.hpp"
23
24
#include <iostream>
25
26
//#define __SSE3__
27
#include <fenv.h>
28
29
#ifdef __SSE3__
30
#include <pmmintrin.h>
31
#endif
32
33
#include <boost/test/unit_test.hpp>
34
#include <boost/utility/binary.hpp>
35
36
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
37
38
















4
BOOST_AUTO_TEST_CASE ( test_rnea )
39
{
40
  #ifdef __SSE3__
41
    _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
42
  #endif
43
  using namespace Eigen;
44
  using namespace pinocchio;
45
46

4
  pinocchio::Model model; buildModels::humanoidRandom(model);
47
48

2
  model.lowerPositionLimit.head<3>().fill(-1.);
49

2
  model.upperPositionLimit.head<3>().fill( 1.);
50
51
4
  pinocchio::Data data(model);
52
2
  data.v[0] = Motion::Zero();
53
2
  data.a[0] = -model.gravity;
54
55
4
  VectorXd q = randomConfiguration(model);
56

4
  VectorXd v = VectorXd::Random(model.nv);
57

4
  VectorXd a = VectorXd::Random(model.nv);
58
59
  #ifdef NDEBUG
60
    const size_t NBT = 10000;
61
  #else
62
2
    const size_t NBT = 1;
63
2
    std::cout << "(the time score in debug mode is not relevant)  " ;
64
  #endif
65
66

4
  PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
67
4
  SMOOTH(NBT)
68
    {
69
2
      rnea(model,data,q,v,a);
70
    }
71
2
  timer.toc(std::cout,NBT);
72
73
2
}
74
75
















4
BOOST_AUTO_TEST_CASE ( test_nle_vs_rnea )
76
{
77
  using namespace Eigen;
78
  using namespace pinocchio;
79
80

4
  pinocchio::Model model; buildModels::humanoidRandom(model);
81
82

2
  model.lowerPositionLimit.head<3>().fill(-1.);
83

2
  model.upperPositionLimit.head<3>().fill( 1.);
84
85
4
  pinocchio::Data data_nle(model);
86
4
  pinocchio::Data data_rnea(model);
87
88
4
  VectorXd q = randomConfiguration(model);
89

4
  VectorXd v = VectorXd::Random(model.nv);
90
91

4
  VectorXd tau_nle (VectorXd::Zero (model.nv));
92

4
  VectorXd tau_rnea (VectorXd::Zero (model.nv));
93
94
  // -------
95

2
  q.tail(model.nq-7).setZero();
96
2
  v.setZero();
97
98

2
  tau_nle = nonLinearEffects(model,data_nle,q,v);
99

2
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
100
101



2
  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
102
103
  // -------
104

2
  q.tail(model.nq-7).setZero();
105
2
  v.setOnes();
106
107

2
  tau_nle = nonLinearEffects(model,data_nle,q,v);
108

2
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
109
110



2
  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
111
112
  // -------
113

2
  q.tail(model.nq-7).setOnes();
114
2
  v.setOnes();
115
116

2
  tau_nle = nonLinearEffects(model,data_nle,q,v);
117

2
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
118
119



2
  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
120
121
  // -------
122
2
  q = randomConfiguration(model);
123
2
  v.setRandom();
124
125

2
  tau_nle = nonLinearEffects(model,data_nle,q,v);
126

2
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
127
128



2
  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
129
2
}
130
131
















4
BOOST_AUTO_TEST_CASE (test_rnea_with_fext)
132
{
133
  using namespace Eigen;
134
  using namespace pinocchio;
135
136
4
  Model model;
137
2
  buildModels::humanoidRandom(model);
138
139

2
  model.lowerPositionLimit.head<3>().fill(-1.);
140

2
  model.upperPositionLimit.head<3>().fill( 1.);
141
142
4
  Data data_rnea_fext(model);
143
4
  Data data_rnea(model);
144
145
4
  VectorXd q = randomConfiguration(model);
146
147

4
  VectorXd v (VectorXd::Random(model.nv));
148

4
  VectorXd a (VectorXd::Random(model.nv));
149
150

4
  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Zero());
151
152

2
  JointIndex rf = model.getJointId("rleg6_joint"); Force Frf = Force::Random();
153
2
  fext[rf] = Frf;
154

2
  JointIndex lf = model.getJointId("lleg6_joint"); Force Flf = Force::Random();
155
2
  fext[lf] = Flf;
156
157
2
  rnea(model,data_rnea,q,v,a);
158
4
  VectorXd tau_ref(data_rnea.tau);
159

4
  Data::Matrix6x Jrf(Data::Matrix6x::Zero(6,model.nv));
160
2
  computeJointJacobian(model,data_rnea,q,rf,Jrf);
161


2
  tau_ref -= Jrf.transpose() * Frf.toVector();
162
163

4
  Data::Matrix6x Jlf(Data::Matrix6x::Zero(6,model.nv));
164
2
  computeJointJacobian(model,data_rnea,q,lf,Jlf);
165


2
  tau_ref -= Jlf.transpose() * Flf.toVector();
166
167
2
  rnea(model,data_rnea_fext,q,v,a,fext);
168
169



2
  BOOST_CHECK(tau_ref.isApprox(data_rnea_fext.tau));
170
2
}
171
172
















4
BOOST_AUTO_TEST_CASE(test_compute_gravity)
173
{
174
  using namespace Eigen;
175
  using namespace pinocchio;
176
177
4
  Model model;
178
2
  buildModels::humanoidRandom(model);
179
180

2
  model.lowerPositionLimit.head<3>().fill(-1.);
181

2
  model.upperPositionLimit.head<3>().fill( 1.);
182
183
4
  Data data_rnea(model);
184
4
  Data data(model);
185
186
4
  VectorXd q = randomConfiguration(model);
187
188

2
  rnea(model,data_rnea,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
189
2
  computeGeneralizedGravity(model,data,q);
190
191



2
  BOOST_CHECK(data_rnea.tau.isApprox(data.g));
192
193
  // Compare with Jcom
194
2
  crba(model,data_rnea,q);
195

4
  Data::Matrix3x Jcom = getJacobianComFromCrba(model,data_rnea);
196
197


4
  VectorXd g_ref(-data_rnea.mass[0]*Jcom.transpose()*Model::gravity981);
198
199



2
  BOOST_CHECK(g_ref.isApprox(data.g));
200
2
}
201
202
















4
BOOST_AUTO_TEST_CASE(test_compute_static_torque)
203
{
204
  using namespace Eigen;
205
  using namespace pinocchio;
206
207
4
  Model model;
208
2
  buildModels::humanoidRandom(model);
209
210

2
  model.lowerPositionLimit.head<3>().fill(-1.);
211

2
  model.upperPositionLimit.head<3>().fill( 1.);
212
213
4
  Data data_rnea(model);
214
4
  Data data(model);
215
216
4
  VectorXd q = randomConfiguration(model);
217
218
  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
219
4
  ForceVector fext((size_t)model.njoints);
220
58
  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
221
56
    (*it).setRandom();
222
223

2
  rnea(model,data_rnea,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv),fext);
224
2
  computeStaticTorque(model,data,q,fext);
225
226



2
  BOOST_CHECK(data_rnea.tau.isApprox(data.tau));
227
228
  // Compare with Jcom + Jacobian of joint
229
2
  crba(model,data_rnea,q);
230

4
  Data::Matrix3x Jcom = getJacobianComFromCrba(model,data_rnea);
231
232


4
  VectorXd static_torque_ref = -data_rnea.mass[0]*Jcom.transpose()*Model::gravity981;
233
2
  computeJointJacobians(model,data_rnea,q);
234
235
4
  Data::Matrix6x J_local(6,model.nv);
236
56
  for(JointIndex joint_id = 1; joint_id < (JointIndex)(model.njoints); ++joint_id)
237
  {
238
54
    J_local.setZero();
239
54
    getJointJacobian(model, data_rnea, joint_id, LOCAL, J_local);
240


54
    static_torque_ref -= J_local.transpose() * fext[joint_id].toVector();
241
  }
242
243



2
  BOOST_CHECK(static_torque_ref.isApprox(data.tau));
244
2
}
245
246
















4
BOOST_AUTO_TEST_CASE(test_compute_coriolis)
247
{
248
  using namespace Eigen;
249
  using namespace pinocchio;
250
251
2
  const double prec = Eigen::NumTraits<double>::dummy_precision();
252
253
4
  Model model;
254
2
  buildModels::humanoidRandom(model);
255
256

2
  model.lowerPositionLimit.head<3>().fill(-1.);
257

2
  model.upperPositionLimit.head<3>().fill( 1.);
258
259
4
  Data data_ref(model);
260
4
  Data data(model);
261
262
4
  VectorXd q = randomConfiguration(model);
263
264

4
  VectorXd v (VectorXd::Random(model.nv));
265

2
  computeCoriolisMatrix(model,data,q,Eigen::VectorXd::Zero(model.nv));
266



2
  BOOST_CHECK(data.C.isZero(prec));
267
268
269
2
  model.gravity.setZero();
270

2
  rnea(model,data_ref,q,v,VectorXd::Zero(model.nv));
271
2
  computeJointJacobiansTimeVariation(model,data_ref,q,v);
272
2
  computeCoriolisMatrix(model,data,q,v);
273
274



2
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
275



2
  BOOST_CHECK(data.J.isApprox(data_ref.J));
276
277

4
  VectorXd tau = data.C * v;
278



2
  BOOST_CHECK(tau.isApprox(data_ref.tau,prec));
279
280
2
  dccrba(model,data_ref,q,v);
281
2
  crba(model,data_ref,q);
282
283
2
  const Data::Vector3 & com = data_ref.com[0];
284

2
  Motion vcom(data_ref.vcom[0],Data::Vector3::Zero());
285

2
  SE3 cM1(data.oMi[1]); cM1.translation() -= com;
286
287





2
  BOOST_CHECK((cM1.toDualActionMatrix()*data_ref.M.topRows<6>()).isApprox(data_ref.Ag,prec));
288
289

2
  Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
290

2
  Force dh(data_ref.dAg * v);
291



2
  BOOST_CHECK(dh.isApprox(dh_ref,prec));
292
293
  {
294

4
    Data data_ref(model), data_ref_plus(model);
295

4
    Eigen::MatrixXd dM(data.C + data.C.transpose());
296
297
2
    const double alpha = 1e-8;
298
4
    Eigen::VectorXd q_plus(model.nq);
299

2
    q_plus = integrate(model,q,alpha*v);
300
301
2
    crba(model,data_ref,q);
302


2
    data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
303
2
    crba(model,data_ref_plus,q_plus);
304


2
    data_ref_plus.M.triangularView<Eigen::StrictlyLower>() = data_ref_plus.M.transpose().triangularView<Eigen::StrictlyLower>();
305
306

4
    Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M)/alpha;
307



2
    BOOST_CHECK(dM.isApprox(dM_ref,sqrt(alpha)));
308
  }
309
310
2
}
311
312
BOOST_AUTO_TEST_SUITE_END()