rnea-derivatives.cpp 16.4 KB
Newer Older
1
//
2
// Copyright (c) 2017-2020 CNRS INRIA
3
4
5
//

#include "pinocchio/multibody/model.hpp"
jcarpent's avatar
jcarpent committed
6
#include "pinocchio/multibody/data.hpp"
7
8
9
10
11
12
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
13
#include "pinocchio/algorithm/crba.hpp"
14
15
16
17
18
19
20
21
22
23
24
25
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives)
{
  using namespace Eigen;
26
  using namespace pinocchio;
27
28

  Model model;
29
  buildModels::humanoidRandom(model);
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
  
  Data data(model), data_fd(model);
  
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Zero(model.nv));
  VectorXd a(VectorXd::Zero(model.nv));
  
  /// Check againt non-derivative algo
  MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero();
  computeGeneralizedGravityDerivatives(model,data,q,g_partial_dq);
  
  VectorXd g0 = computeGeneralizedGravity(model,data_fd,q);
  BOOST_CHECK(data.g.isApprox(g0));

  MatrixXd g_partial_dq_fd(model.nv,model.nv); g_partial_dq_fd.setZero();

  VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
  VectorXd q_plus(model.nq);
  VectorXd g_plus(model.nv);
  const double alpha = 1e-8;
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    g_plus = computeGeneralizedGravity(model,data_fd,q_plus);
    
    g_partial_dq_fd.col(k) = (g_plus - g0)/alpha;
    v_eps[k] -= alpha;
  }
  
  BOOST_CHECK(g_partial_dq.isApprox(g_partial_dq_fd,sqrt(alpha)));
63
64
}

65
66
67
68
69
70
71
72
73
74
75
76
77
78
BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives_fext)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);
  
  Data data(model), data_fd(model);
  
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);
  VectorXd q = randomConfiguration(model);

79
  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
  ForceVector fext((size_t)model.njoints);
  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
    (*it).setRandom();
  
  // Check againt non-derivative algo
  MatrixXd static_vec_partial_dq(model.nv,model.nv); static_vec_partial_dq.setZero();
  computeStaticTorqueDerivatives(model,data,q,fext,static_vec_partial_dq);
  
  VectorXd tau0 = computeStaticTorque(model,data_fd,q,fext);
  BOOST_CHECK(data.tau.isApprox(tau0));
  
  std::cout << "data.tau: " << data.tau.transpose() << std::endl;
  std::cout << "tau0: " << tau0.transpose() << std::endl;

  MatrixXd static_vec_partial_dq_fd(model.nv,model.nv);

  VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
  VectorXd q_plus(model.nq);
  VectorXd tau_plus(model.nv);
  const double alpha = 1e-8;
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    tau_plus = computeStaticTorque(model,data_fd,q_plus,fext);
    
    static_vec_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
    v_eps[k] = 0.;
  }
  
  BOOST_CHECK(static_vec_partial_dq.isApprox(static_vec_partial_dq_fd,sqrt(alpha)));
}

113
114
115
BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
{
  using namespace Eigen;
116
  using namespace pinocchio;
117
  
118
  Model model;
119
  buildModels::humanoidRandom(model);
120
  
121
  Data data(model), data_fd(model), data_ref(model);
122
  
123
124
125
126
127
128
129
130
131
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));
  
  /// Check againt computeGeneralizedGravityDerivatives
  MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
  MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
132
  MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();
133
  computeRNEADerivatives(model,data,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv),rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
134
  rnea(model,data_ref,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
135
  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
136
137
138
  {
    BOOST_CHECK(data.of[k].isApprox(data.oMi[k].act(data_ref.f[k])));
  }
139
140
141
142
  
  MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero();
  computeGeneralizedGravityDerivatives(model,data_ref,q,g_partial_dq);
  
143
  BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq));
144
145
146
  BOOST_CHECK(rnea_partial_dq.isApprox(g_partial_dq));
  BOOST_CHECK(data.tau.isApprox(data_ref.g));
  
147
  VectorXd tau0 = rnea(model,data_fd,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
148
149
150
151
152
153
154
155
156
157
  MatrixXd rnea_partial_dq_fd(model.nv,model.nv); rnea_partial_dq_fd.setZero();
  
  VectorXd v_eps(VectorXd::Zero(model.nv));
  VectorXd q_plus(model.nq);
  VectorXd tau_plus(model.nv);
  const double alpha = 1e-8;
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
158
    tau_plus = rnea(model,data_fd,q_plus,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
159
160
161
162
163
    
    rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
    v_eps[k] -= alpha;
  }
  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
164

165
166
167
168
169
170
171
172
  // Check with q and a non zero
  tau0 = rnea(model,data_fd,q,0*v,a);
  rnea_partial_dq_fd.setZero();

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
173
    tau_plus = rnea(model,data_fd,q_plus,VectorXd::Zero(model.nv),a);
174
175
176
177
178
179

    rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
    v_eps[k] -= alpha;
  }
  
  rnea_partial_dq.setZero();
180
181
  computeRNEADerivatives(model,data,q,VectorXd::Zero(model.nv),a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
  forwardKinematics(model,data_ref,q,VectorXd::Zero(model.nv),a);
182
183
184
185
186
187
  
  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
  {
    BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
jcarpent's avatar
jcarpent committed
188
    BOOST_CHECK(data.oh[k].isApprox(Force::Zero()));
189
190
191
192
193
194
195
196
  }
  
  BOOST_CHECK(data.tau.isApprox(tau0));
  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
  
  // Check with q and v non zero
  const Motion gravity(model.gravity);
  model.gravity.setZero();
197
  tau0 = rnea(model,data_fd,q,v,VectorXd::Zero(model.nv));
198
199
200
201
202
203
  rnea_partial_dq_fd.setZero();
  
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
204
    tau_plus = rnea(model,data_fd,q_plus,v,VectorXd::Zero(model.nv));
205
206
207
208
209
210
211
212
213
214
215
    
    rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
    v_eps[k] -= alpha;
  }
  
  VectorXd v_plus(v);
  MatrixXd rnea_partial_dv_fd(model.nv,model.nv); rnea_partial_dv_fd.setZero();
  
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
216
    tau_plus = rnea(model,data_fd,q,v_plus,VectorXd::Zero(model.nv));
217
218
219
220
221
222
223
    
    rnea_partial_dv_fd.col(k) = (tau_plus - tau0)/alpha;
    v_plus[k] -= alpha;
  }
  
  rnea_partial_dq.setZero();
  rnea_partial_dv.setZero();
224
225
  computeRNEADerivatives(model,data,q,v,VectorXd::Zero(model.nv),rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
  forwardKinematics(model,data_ref,q,v,VectorXd::Zero(model.nv));
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
  
  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
  {
    BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
  }
  
  BOOST_CHECK(data.tau.isApprox(tau0));
  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
  BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(alpha)));
  
//    std::cout << "rnea_partial_dv:\n" << rnea_partial_dv.block<10,10>(0,0) << std::endl;
//    std::cout << "rnea_partial_dv ref:\n" << rnea_partial_dv_fd.block<10,10>(0,0) << std::endl;
//    std::cout << "rnea_partial_dv:\n" << rnea_partial_dv.topRows<10>() << std::endl;
//    std::cout << "rnea_partial_dv ref:\n" << rnea_partial_dv_fd.topRows<10>() << std::endl;
  // Check with q, v and a non zero
  model.gravity = gravity;
  v_plus = v;
  tau0 = rnea(model,data_fd,q,v,a);
  rnea_partial_dq_fd.setZero();
  
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    tau_plus = rnea(model,data_fd,q_plus,v,a);
    
    rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
    v_eps[k] -= alpha;
  }
  
  rnea_partial_dv_fd.setZero();
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
    tau_plus = rnea(model,data_fd,q,v_plus,a);
    
    rnea_partial_dv_fd.col(k) = (tau_plus - tau0)/alpha;
    v_plus[k] -= alpha;
  }
  
  rnea_partial_dq.setZero();
  rnea_partial_dv.setZero();
270
  computeRNEADerivatives(model,data,q,v,a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
271
272
273
274
275
276
277
278
279
  forwardKinematics(model,data_ref,q,v,a);
  
  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
  {
    BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
  }
  
280
  computeJointJacobiansTimeVariation(model,data_ref,q,v);
281
282
283
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  crba(model,data_ref,q);
  
284
285
  rnea_partial_da.triangularView<Eigen::StrictlyLower>()
  = rnea_partial_da.transpose().triangularView<Eigen::StrictlyLower>();
286
287
  data_ref.M.triangularView<Eigen::StrictlyLower>()
  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
288
  BOOST_CHECK(rnea_partial_da.isApprox(data_ref.M));
289
290
291
292

  BOOST_CHECK(data.tau.isApprox(tau0));
  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
  BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(alpha)));
293
294
295
296
297
298
299
300
  
  Data data2(model);
  computeRNEADerivatives(model,data2,q,v,a);
  data2.M.triangularView<Eigen::StrictlyLower>()
  = data2.M.transpose().triangularView<Eigen::StrictlyLower>();

  BOOST_CHECK(rnea_partial_dq.isApprox(data2.dtau_dq));
  BOOST_CHECK(rnea_partial_dv.isApprox(data2.dtau_dv));
301
  BOOST_CHECK(rnea_partial_da.isApprox(data2.M));
302
  
303
}
304

305
306
307
BOOST_AUTO_TEST_CASE(test_rnea_derivatives_fext)
{
  using namespace Eigen;
308
  using namespace pinocchio;
309
310
  
  Model model;
311
  buildModels::humanoidRandom(model);
312
313
314
315
316
317
318
319
320
321
  typedef Model::Force Force;
  
  Data data(model), data_fd(model), data_ref(model);
  
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));
  
322
  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
  ForceVector fext((size_t)model.njoints);
  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
    (*it).setRandom();
  
  /// Check againt computeGeneralizedGravityDerivatives
  MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
  MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
  MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();
  
  computeRNEADerivatives(model,data,q,v,a,fext,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
  rnea(model,data_ref,q,v,a,fext);
  
  BOOST_CHECK(data.tau.isApprox(data_ref.tau));
  
  computeRNEADerivatives(model,data_ref,q,v,a);
  BOOST_CHECK(rnea_partial_dv.isApprox(data_ref.dtau_dv));
  BOOST_CHECK(rnea_partial_da.isApprox(data_ref.M));
  
  MatrixXd rnea_partial_dq_fd(model.nv,model.nv); rnea_partial_dq_fd.setZero();
  MatrixXd rnea_partial_dv_fd(model.nv,model.nv); rnea_partial_dv_fd.setZero();
  MatrixXd rnea_partial_da_fd(model.nv,model.nv); rnea_partial_da_fd.setZero();
  
  VectorXd v_eps(VectorXd::Zero(model.nv));
  VectorXd q_plus(model.nq);
  VectorXd tau_plus(model.nv);
  const double eps = 1e-8;
  
  const VectorXd tau_ref = rnea(model,data_ref,q,v,a,fext);
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] = eps;
    q_plus = integrate(model,q,v_eps);
    tau_plus = rnea(model,data_fd,q_plus,v,a,fext);
    
    rnea_partial_dq_fd.col(k) = (tau_plus - tau_ref) / eps;
    
    v_eps[k] = 0.;
  }
  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(eps)));
  
  VectorXd v_plus(v);
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += eps;
    
    tau_plus = rnea(model,data_fd,q,v_plus,a,fext);
    
    rnea_partial_dv_fd.col(k) = (tau_plus - tau_ref) / eps;
    
    v_plus[k] -= eps;
  }
  BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(eps)));
  
  VectorXd a_plus(a);
  for(int k = 0; k < model.nv; ++k)
  {
    a_plus[k] += eps;
    
    tau_plus = rnea(model,data_fd,q,v,a_plus,fext);
    
    rnea_partial_da_fd.col(k) = (tau_plus - tau_ref) / eps;
    
    a_plus[k] -= eps;
  }
  
  rnea_partial_da.triangularView<Eigen::Lower>() = rnea_partial_da.transpose().triangularView<Eigen::Lower>();
  BOOST_CHECK(rnea_partial_da.isApprox(rnea_partial_da_fd,sqrt(eps)));

  // test the shortcut
  Data data_shortcut(model);
  computeRNEADerivatives(model,data_shortcut,q,v,a,fext);
  BOOST_CHECK(data_shortcut.dtau_dq.isApprox(rnea_partial_dq));
  BOOST_CHECK(data_shortcut.dtau_dv.isApprox(rnea_partial_dv));
  data_shortcut.M.triangularView<Eigen::Lower>() = data_shortcut.M.transpose().triangularView<Eigen::Lower>();
  BOOST_CHECK(data_shortcut.M.isApprox(rnea_partial_da));
}

400
401
402
BOOST_AUTO_TEST_CASE(test_rnea_derivatives_vs_kinematics_derivatives)
{
  using namespace Eigen;
403
  using namespace pinocchio;
404
405
  
  Model model;
406
407
  buildModels::humanoidRandom(model);

408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
  Data data(model), data_ref(model);
  
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));
  
  /// Check againt computeGeneralizedGravityDerivatives
  MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
  MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
  MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();
  
  computeRNEADerivatives(model,data,q,v,a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
  computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
  
  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
426
427
428
429
430
431
432
  
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
    BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
    BOOST_CHECK(data.oa[k].isApprox(data_ref.oa[k]));
  }
433
434
}

435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
BOOST_AUTO_TEST_CASE(test_multiple_calls)
{
  using namespace Eigen;
  using namespace pinocchio;
  
  Model model;
  buildModels::humanoidRandom(model);
  
  Data data1(model), data2(model);
  
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));
  
  computeRNEADerivatives(model,data1,q,v,a);
  data2 = data1;
  
  for(int k = 0; k < 20; ++k)
  {
    computeRNEADerivatives(model,data1,q,v,a);
  }
  
  BOOST_CHECK(data1.J.isApprox(data2.J));
  BOOST_CHECK(data1.dJ.isApprox(data2.dJ));
  BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq));
  BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq));
  BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv));
  
  BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq));
  BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv));
  BOOST_CHECK(data1.dFda.isApprox(data2.dFda));
  
  BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq));
  BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv));
  BOOST_CHECK(data1.M.isApprox(data2.M));
}

474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
BOOST_AUTO_TEST_CASE(test_get_coriolis)
{
  using namespace Eigen;
  using namespace pinocchio;
  
  Model model;
  buildModels::humanoidRandom(model);
  
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);
  
  Data data_ref(model);
  Data data(model);
  
  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);
  VectorXd tau = VectorXd::Random(model.nv);
  
  computeCoriolisMatrix(model,data_ref,q,v);
  
  computeRNEADerivatives(model,data,q,v,tau);
  getCoriolisMatrix(model,data);
  
  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  BOOST_CHECK(data.Fcrb[0].isApprox(data_ref.dFdv));
  for(JointIndex k = 1; k < model.joints.size(); ++k)
  {
    BOOST_CHECK(data.vxI[k].isApprox(data_ref.vxI[k]));
    BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
  }
  
  BOOST_CHECK(data.C.isApprox(data_ref.C));
}

509
BOOST_AUTO_TEST_SUITE_END()