test_lpf.cpp 29.7 KB
Newer Older
1
2
3
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
4
5
// Copyright (C) 2019-2021, LAAS-CNRS, New York University, Max Planck
// Gesellschaft
6
7
8
9
10
11
12
13
//                          University of Edinburgh, INRIA
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API

14
15
#include <crocoddyl/core/integrator/euler.hpp>

16
#include "common.hpp"
17
18
#include "factory/diff-action.hpp"
#include "factory/lpf.hpp"
19
20
21
22
23
24

using namespace boost::unit_test;
using namespace sobec::unittest;

//----------------------------------------------------------------------------//

25
26
27
28
29
void test_check_data(
    ActionModelLPFTypes::Type iam_type,
    DifferentialActionModelTypes::Type dam_type,
    PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
    ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) {
30
31
  // create the model
  ActionModelLPFFactory factory_iam;
Sébastien Kleff's avatar
Sébastien Kleff committed
32
33
  const boost::shared_ptr<sobec::IntegratedActionModelLPF>& model =
      factory_iam.create(iam_type, dam_type, ref_type, mask_type);
34
35
36
37
38
39

  // Run the print function
  std::ostringstream tmp;
  tmp << *model;

  // create the corresponding data object
40
41
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data =
      model->createData();
Sébastien Kleff's avatar
Sébastien Kleff committed
42

43
44
45
  BOOST_CHECK(model->checkData(data));
}

46
47
48
49
50
void test_calc_returns_state(
    ActionModelLPFTypes::Type iam_type,
    DifferentialActionModelTypes::Type dam_type,
    PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
    ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) {
51
52
  // create the model
  ActionModelLPFFactory factory_iam;
Sébastien Kleff's avatar
Sébastien Kleff committed
53
54
  const boost::shared_ptr<sobec::IntegratedActionModelLPF>& model =
      factory_iam.create(iam_type, dam_type, ref_type, mask_type);
55
  // create the corresponding data object
56
57
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data =
      model->createData();
Sébastien Kleff's avatar
Sébastien Kleff committed
58

59
60
  // Generating random state and control vectors
  const Eigen::VectorXd& x = model->get_state()->rand();
61
  const Eigen::VectorXd& u = Eigen::VectorXd::Random(model->get_nw());
62
63
64
65

  // Getting the state dimension from calc() call
  model->calc(data, x, u);

66
67
  BOOST_CHECK(static_cast<std::size_t>(data->xnext.size()) ==
              model->get_state()->get_nx());
68
69
}

70
71
72
73
74
void test_calc_returns_a_cost(
    ActionModelLPFTypes::Type iam_type,
    DifferentialActionModelTypes::Type dam_type,
    PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
    ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) {
75
76
  // create the model
  ActionModelLPFFactory factory_iam;
Sébastien Kleff's avatar
Sébastien Kleff committed
77
78
  const boost::shared_ptr<sobec::IntegratedActionModelLPF>& model =
      factory_iam.create(iam_type, dam_type, ref_type, mask_type);
79
80

  // create the corresponding data object and set the cost to nan
81
82
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data =
      model->createData();
83
84
85
86
87

  data->cost = nan("");

  // Getting the cost value computed by calc()
  const Eigen::VectorXd& x = model->get_state()->rand();
88
  const Eigen::VectorXd& u = Eigen::VectorXd::Random(model->get_nw());
89
90
91
92
93
94
  model->calc(data, x, u);

  // Checking that calc returns a cost value
  BOOST_CHECK(!std::isnan(data->cost));
}

95
96
void test_partial_derivatives_against_numdiff(
    const boost::shared_ptr<sobec::IntegratedActionModelLPF>& model) {
97
  // create the corresponding data object and set the cost to nan
98
99
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data =
      model->createData();
100
101

  crocoddyl::ActionModelNumDiff model_num_diff(model);
102
103
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data_num_diff =
      model_num_diff.createData();
104
105
106

  // Generating random values for the state and control
  Eigen::VectorXd x = model->get_state()->rand();
107
  const Eigen::VectorXd& u = Eigen::VectorXd::Random(model->get_nw());
108
109
110
111
112
113
114
115
116
117
118
119

  // Computing the action derivatives
  model->calc(data, x, u);
  model->calcDiff(data, x, u);

  model_num_diff.calc(data_num_diff, x, u);
  model_num_diff.calcDiff(data_num_diff, x, u);

  // Checking the partial derivatives against NumDiff
  double tol = sqrt(model_num_diff.get_disturbance());

  // const std::size_t nv = model->get_state()->get_nv();
120
  // const std::size_t nu = model->get_differential()->get_nu();
121
  // std::cout << " Fx - Fx_nd [q]: " << std::endl;
122
123
  // std::cout << (data->Fx -
  // data_num_diff->Fx).leftCols(nv).lpNorm<Eigen::Infinity>() << std::endl;
124
  // std::cout << " Fx - Fx_nd [v]: " << std::endl;
125
126
127
128
  // std::cout << (data->Fx - data_num_diff->Fx).block(0, nv, 2*nv+nu,
  // nv).lpNorm<Eigen::Infinity>() << std::endl; std::cout << " Fx - Fx_nd
  // [tau]: " << std::endl; std::cout << (data->Fx -
  // data_num_diff->Fx).rightCols(nu).lpNorm<Eigen::Infinity>() << std::endl;
129

130
131
132
133
134
  BOOST_CHECK((data->Fx - data_num_diff->Fx).isZero(NUMDIFF_MODIFIER * tol));
  BOOST_CHECK((data->Fu - data_num_diff->Fu).isZero(NUMDIFF_MODIFIER * tol));
  BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(NUMDIFF_MODIFIER * tol));
  BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(NUMDIFF_MODIFIER * tol));
  if (model_num_diff.get_with_gauss_approx()) {
135
136
137
138
139
140
    BOOST_CHECK(
        (data->Lxx - data_num_diff->Lxx).isZero(NUMDIFF_MODIFIER * tol));
    BOOST_CHECK(
        (data->Lxu - data_num_diff->Lxu).isZero(NUMDIFF_MODIFIER * tol));
    BOOST_CHECK(
        (data->Luu - data_num_diff->Luu).isZero(NUMDIFF_MODIFIER * tol));
141
142
143
144
145
146
147
  } else {
    BOOST_CHECK((data_num_diff->Lxx).isZero(tol));
    BOOST_CHECK((data_num_diff->Lxu).isZero(tol));
    BOOST_CHECK((data_num_diff->Luu).isZero(tol));
  }
}

148
149
150
151
152
void test_partial_derivatives_action_model(
    ActionModelLPFTypes::Type iam_type,
    DifferentialActionModelTypes::Type dam_type,
    PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
    ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) {
153
154
  // create the model
  ActionModelLPFFactory factory;
Sébastien Kleff's avatar
Sébastien Kleff committed
155
156
  const boost::shared_ptr<sobec::IntegratedActionModelLPF>& model =
      factory.create(iam_type, dam_type, ref_type, mask_type);
157
158
159
  test_partial_derivatives_against_numdiff(model);
}

160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215


void test_partial_derivatives_against_numdiff_terminal(
    const boost::shared_ptr<sobec::IntegratedActionModelLPF>& model) {
  // create the corresponding data object and set the cost to nan
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data =
      model->createData();

  crocoddyl::ActionModelNumDiff model_num_diff(model);
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data_num_diff =
      model_num_diff.createData();

  // Generating random values for the state and control
  Eigen::VectorXd x = model->get_state()->rand();

  // Computing the action derivatives
  model->calc(data, x);
  model->calcDiff(data, x);
  model_num_diff.calc(data_num_diff, x);
  model_num_diff.calcDiff(data_num_diff, x);

  // Checking the partial derivatives against NumDiff
  double tol = sqrt(model_num_diff.get_disturbance());
  // Checking the partial derivatives against NumDiff
  BOOST_CHECK((data->Fx - data_num_diff->Fx).isZero(NUMDIFF_MODIFIER * tol));
  BOOST_CHECK((data->Fu - data_num_diff->Fu).isZero(NUMDIFF_MODIFIER * tol));
  BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(NUMDIFF_MODIFIER * tol));
  BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(NUMDIFF_MODIFIER * tol));
  if (model_num_diff.get_with_gauss_approx()) {
    BOOST_CHECK(
        (data->Lxx - data_num_diff->Lxx).isZero(NUMDIFF_MODIFIER * tol));
    BOOST_CHECK(
        (data->Lxu - data_num_diff->Lxu).isZero(NUMDIFF_MODIFIER * tol));
    BOOST_CHECK(
        (data->Luu - data_num_diff->Luu).isZero(NUMDIFF_MODIFIER * tol));
  } else {
    BOOST_CHECK((data_num_diff->Lxx).isZero(tol));
    BOOST_CHECK((data_num_diff->Lxu).isZero(tol));
    BOOST_CHECK((data_num_diff->Luu).isZero(tol));
  }
}

void test_partial_derivatives_action_model_terminal(
    ActionModelLPFTypes::Type iam_type,
    DifferentialActionModelTypes::Type dam_type,
    PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
    ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) {
  // create the model
  ActionModelLPFFactory factory;
  const boost::shared_ptr<sobec::IntegratedActionModelLPF>& model =
      factory.create(iam_type, dam_type, ref_type, mask_type);
  model->set_dt(0);
  test_partial_derivatives_against_numdiff_terminal(model);
}


216
void test_calc_alpha0_equivalent_euler(
217
218
219
    ActionModelLPFTypes::Type iam_type,
    DifferentialActionModelTypes::Type dam_type,
    PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
220
    ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) {
221
222
223
224
  // Create IAM LPF
  ActionModelLPFFactory factory_iam;
  const boost::shared_ptr<sobec::IntegratedActionModelLPF>& modelLPF =
      factory_iam.create(iam_type, dam_type, ref_type, mask_type);
225
226
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataLPF =
      modelLPF->createData();
227

228
  // Create IAM Euler from iamLPF.DAM and iamLPF.dt (with cost residual)
229
  boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> modelEuler = 
230
    boost::make_shared<crocoddyl::IntegratedActionModelEuler>(modelLPF->get_differential(), modelLPF->get_dt(), true);
231
232
233
234
235
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataEuler = modelEuler->createData();

  // Generating random values for the state and control
  std::size_t nx = modelEuler->get_state()->get_nx();
  std::size_t ndx = modelEuler->get_state()->get_ndx();
236
  std::size_t ntau = boost::static_pointer_cast<sobec::IntegratedActionModelLPF>(modelLPF)->get_ntau();
237
  std::size_t ntau_state = boost::static_pointer_cast<sobec::StateLPF>(modelLPF->get_state())->get_ntau();
238
239
240
241
  const Eigen::VectorXd y = modelLPF->get_state()->rand();
  const Eigen::VectorXd& w = Eigen::VectorXd::Random(modelLPF->get_nw());
  const Eigen::VectorXd x = y.head(nx);
  const Eigen::VectorXd tau = y.tail(ntau);
242

243
244
245
246
247
248
249
250
251
  // Check stuff
  BOOST_CHECK(ntau == modelEuler->get_nu());
  BOOST_CHECK(ntau_state == modelEuler->get_nu());
  const std::vector<int>& lpf_torque_ids = modelLPF->get_lpf_torque_ids();
  const std::vector<int>& non_lpf_torque_ids = modelLPF->get_non_lpf_torque_ids();
  BOOST_CHECK(lpf_torque_ids.size() == modelEuler->get_nu());
  BOOST_CHECK(non_lpf_torque_ids.size() == 0);
  BOOST_CHECK(non_lpf_torque_ids.size() + lpf_torque_ids.size() == modelEuler->get_nu());

252
  // Checking the partial derivatives against NumDiff
253
  double tol = 1e-6;
254

255
256
257
258
259
260
  // Computing the action 
  modelLPF->calc(dataLPF, y, w);
  modelEuler->calc(dataEuler, x, tau);
  // Test perfect actuation and state integration
  BOOST_CHECK((dataLPF->xnext.tail(ntau) - w).isZero(tol));
  BOOST_CHECK((dataLPF->xnext.head(nx) - dataEuler->xnext).isZero(tol));
261
262
  BOOST_CHECK((dataLPF->r.head(modelEuler->get_nr()) - dataEuler->r).isZero(tol));
  BOOST_CHECK( (dataLPF->cost - dataEuler->cost) <= tol);
263
264
265
266
267
268
269
270

  // Test terminal calc
  modelLPF->set_dt(0.);
  modelEuler->set_dt(0.);
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataLPFTerminal = modelLPF->createData();
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataEulerTerminal = modelEuler->createData();
  modelLPF->calc(dataLPFTerminal, y);
  modelEuler->calc(dataEulerTerminal, x);
Sébastien Kleff's avatar
Sébastien Kleff committed
271
  BOOST_CHECK((dataLPFTerminal->r.head(modelEuler->get_nr()) - dataEulerTerminal->r).isZero(tol));
272
  BOOST_CHECK((dataLPFTerminal->cost - dataEulerTerminal->cost) <= tol);
273
274
}

275
void test_calc_NONE_equivalent_euler(
276
277
278
    ActionModelLPFTypes::Type iam_type,
    DifferentialActionModelTypes::Type dam_type,
    PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
279
    ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) {
280
281
282
283
  // Create IAM LPF
  ActionModelLPFFactory factory_iam;
  const boost::shared_ptr<sobec::IntegratedActionModelLPF>& modelLPF =
      factory_iam.create(iam_type, dam_type, ref_type, mask_type);
284
285
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataLPF =
      modelLPF->createData();
286
287
288

  // Create IAM Euler from DAM and iamLPF.dt (with cost residual)
  boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> modelEuler = 
289
    boost::make_shared<crocoddyl::IntegratedActionModelEuler>(modelLPF->get_differential(), modelLPF->get_dt(), true);
290
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataEuler = modelEuler->createData();
291

292
293
294
  // Generating random values for the state and control
  std::size_t nx = modelEuler->get_state()->get_nx();
  std::size_t ndx = modelEuler->get_state()->get_ndx();
295
  // std::size_t nv = modelEuler->get_state()->get_nv();
296
297
298
299
300
301
  std::size_t ntau =
      boost::static_pointer_cast<sobec::IntegratedActionModelLPF>(modelLPF)
          ->get_ntau();
  std::size_t ntau_state =
      boost::static_pointer_cast<sobec::StateLPF>(modelLPF->get_state())
          ->get_ntau();
302
303
304
  BOOST_CHECK(ntau == 0);
  BOOST_CHECK(ntau_state == 0);
  const std::vector<int>& lpf_torque_ids = modelLPF->get_lpf_torque_ids();
305
  const std::vector<int>& non_lpf_torque_ids = modelLPF->get_non_lpf_torque_ids();
306
  BOOST_CHECK(lpf_torque_ids.size() == 0);
307
308
309
  BOOST_CHECK(non_lpf_torque_ids.size() + lpf_torque_ids.size() == modelEuler->get_nu());
  BOOST_CHECK(non_lpf_torque_ids.size() == modelEuler->get_nu() );

310
311
312
313
  const Eigen::VectorXd y = modelLPF->get_state()->rand();
  BOOST_CHECK(y.size() == nx);
  const Eigen::VectorXd& w = Eigen::VectorXd::Random(modelLPF->get_nw());
  BOOST_CHECK(w.size() == modelEuler->get_nu());
314

315
316
  // Checking the partial derivatives against NumDiff
  double tol = 1e-6;
317
  // Computing the action
318
  modelLPF->calc(dataLPF, y, w);
319
320
321
  modelEuler->calc(dataEuler, y, w);
  // Test perfect actuation and state integration
  BOOST_CHECK((dataLPF->xnext - dataEuler->xnext).isZero(tol));
322
323
  BOOST_CHECK((dataLPF->r - dataEuler->r).isZero(tol));
  BOOST_CHECK( (dataLPF->cost - dataEuler->cost) <= tol);
324
325
326
327
328
329
330
331
332
333

  // Test terminal calc
  modelLPF->set_dt(0.);
  modelEuler->set_dt(0.);
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataLPFTerminal = modelLPF->createData();
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataEulerTerminal = modelEuler->createData();
  modelLPF->calc(dataLPFTerminal, y);
  modelEuler->calc(dataEulerTerminal, y);
  BOOST_CHECK((dataLPFTerminal->r - dataEulerTerminal->r).isZero(tol));
  BOOST_CHECK((dataLPFTerminal->cost - dataEulerTerminal->cost) <= tol);
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
// void test_calcDiff_NONE_equivalent_euler( 
//     ActionModelLPFTypes::Type iam_type,
//     DifferentialActionModelTypes::Type dam_type,
//     PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
//     ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z){
    
//   // Create IAM LPF
//   ActionModelLPFFactory factory_iam;
//   const boost::shared_ptr<sobec::IntegratedActionModelLPF>& modelLPF =
//       factory_iam.create(iam_type, dam_type, ref_type, mask_type);
//   const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataLPF = modelLPF->createData();

//   // Create IAM Euler from DAM and iamLPF.dt (with cost residual)
//   boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> modelEuler = 
//     boost::make_shared<crocoddyl::IntegratedActionModelEuler>(modelLPF->get_differential(), modelLPF->get_dt(), true);
//   const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataEuler = modelEuler->createData();

//   // Generating random values for the state and control
//   std::size_t nx = modelEuler->get_state()->get_nx();
//   std::size_t ndx = modelEuler->get_state()->get_ndx();
//   std::size_t nv = modelEuler->get_state()->get_nv();
//   std::size_t ntau = boost::static_pointer_cast<sobec::IntegratedActionModelLPF>(modelLPF)->get_ntau();
//   std::size_t nu = modelEuler->get_nu();
//   const Eigen::VectorXd y = modelLPF->get_state()->rand();
//   const Eigen::VectorXd& w = Eigen::VectorXd::Random(modelLPF->get_nw());
//   const Eigen::VectorXd x = y.head(nx);
//   Eigen::VectorXd tau = w; 
//   const std::vector<int>& lpf_torque_ids = modelLPF->get_lpf_torque_ids();
//   const std::vector<int>& non_lpf_torque_ids = modelLPF->get_non_lpf_torque_ids();
//   BOOST_CHECK(ntau == lpf_torque_ids.size());
//   for(std::size_t i=0; i<lpf_torque_ids.size();i++){
//     tau(lpf_torque_ids[i]) = y.tail(ntau)[i];
//   }
//   BOOST_CHECK(non_lpf_torque_ids.size() + lpf_torque_ids.size() == nu );

//   // Checking the partial derivatives against NumDiff
//   double tol = 1e-6; 

//   // Computing the action 
//   modelLPF->calc(dataLPF, y, w);
//   modelEuler->calc(dataEuler, x, tau); 
//   // Computing the derivatives
//   modelLPF->calcDiff(dataLPF, y, w);
//   modelEuler->calcDiff(dataEuler, x, tau); 

//   // Case no joint is LPF
//   BOOST_CHECK((dataLPF->Fx - dataEuler->Fx).isZero(tol));
//   BOOST_CHECK((dataLPF->Fu - dataEuler->Fu).isZero(tol));
//   BOOST_CHECK((dataLPF->Lx - dataEuler->Lx).isZero(tol));
//   BOOST_CHECK((dataLPF->Lu - dataEuler->Lu).isZero(tol));
//   BOOST_CHECK((dataLPF->Lxx - dataEuler->Lxx).isZero(tol));
//   BOOST_CHECK((dataLPF->Lxu - dataEuler->Lxu).isZero(tol));
//   BOOST_CHECK((dataLPF->Luu - dataEuler->Luu).isZero(tol));
// }


393
394
395
396
void test_calcDiff_explicit_equivalent_euler( 
    ActionModelLPFTypes::Type iam_type,
    DifferentialActionModelTypes::Type dam_type,
    PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
397
    ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) {
398
399
400
401
  // Create IAM LPF
  ActionModelLPFFactory factory_iam;
  const boost::shared_ptr<sobec::IntegratedActionModelLPF>& modelLPF =
      factory_iam.create(iam_type, dam_type, ref_type, mask_type);
402
403
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataLPF =
      modelLPF->createData();
404
405
406

  // Create IAM Euler from DAM and iamLPF.dt (with cost residual)
  boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> modelEuler = 
407
    boost::make_shared<crocoddyl::IntegratedActionModelEuler>(modelLPF->get_differential(), modelLPF->get_dt(), true);
408
409
410
411
412
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataEuler = modelEuler->createData();

  // Generating random values for the state and control
  std::size_t nx = modelEuler->get_state()->get_nx();
  std::size_t ndx = modelEuler->get_state()->get_ndx();
413
  // std::size_t nv = modelEuler->get_state()->get_nv();
414
415
416
  std::size_t ntau =
      boost::static_pointer_cast<sobec::IntegratedActionModelLPF>(modelLPF)
          ->get_ntau();
417
418
419
420
  std::size_t nu = modelEuler->get_nu();
  const Eigen::VectorXd y = modelLPF->get_state()->rand();
  const Eigen::VectorXd& w = Eigen::VectorXd::Random(modelLPF->get_nw());
  const Eigen::VectorXd x = y.head(nx);
421
  Eigen::VectorXd tau = w;
422
  const std::vector<int>& lpf_torque_ids = modelLPF->get_lpf_torque_ids();
423
  const std::vector<int>& non_lpf_torque_ids = modelLPF->get_non_lpf_torque_ids();
424
  BOOST_CHECK(ntau == lpf_torque_ids.size());
425
426
  for(std::size_t i=0; i<lpf_torque_ids.size();i++){
    tau(lpf_torque_ids[i]) = y.tail(ntau)[i];
427
  }
428
  BOOST_CHECK(non_lpf_torque_ids.size() + lpf_torque_ids.size() == nu );
429

430
  // Checking the partial derivatives against NumDiff
431
  double tol = 1e-6; 
432

433
  // Computing the action
434
  modelLPF->calc(dataLPF, y, w);
435
  modelEuler->calc(dataEuler, x, tau); 
436
437
  // Computing the derivatives
  modelLPF->calcDiff(dataLPF, y, w);
438
439
  modelEuler->calcDiff(dataEuler, x, tau); 

440
  // All or some joint are LPF
441
442
443
444
445
  // Size varying stuff  
  const Eigen::MatrixXd& Fu_LPF = dataLPF->Fx.topRightCorner(ndx, ntau);
  const Eigen::MatrixXd& Lu_LPF = dataLPF->Lx.tail(ntau);
  const Eigen::MatrixXd& Lxu_LPF = dataLPF->Lxx.topRightCorner(ndx, ntau);
  const Eigen::MatrixXd& Luu_LPF = dataLPF->Lxx.bottomRightCorner(ntau, ntau);
446
447
448
  for (std::size_t i = 0; i < lpf_torque_ids.size(); i++) {
    BOOST_CHECK(
        (Fu_LPF.col(i) - dataEuler->Fu.col(lpf_torque_ids[i])).isZero(tol));
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
    BOOST_CHECK((Lu_LPF(i) - dataEuler->Lu(lpf_torque_ids[i])) <= tol);
    BOOST_CHECK((Lxu_LPF.col(i) - dataEuler->Lxu.col(lpf_torque_ids[i])).isZero(tol));
    for(std::size_t j=0; j<lpf_torque_ids.size();j++){
      BOOST_CHECK( (Luu_LPF(i,j)- dataEuler->Luu(lpf_torque_ids[i], lpf_torque_ids[j]) ) <= tol );
    }
  }
  // Fixed size stuff
  const Eigen::MatrixXd& Fx_LPF = dataLPF->Fx.topLeftCorner(ndx, ndx);
  const Eigen::MatrixXd& Lx_LPF = dataLPF->Lx.head(ndx);
  const Eigen::MatrixXd& Lxx_LPF = dataLPF->Lxx.topLeftCorner(ndx, ndx);
  // Testing the partials w.r.t. u match blocks in partial w.r.t. augmented state y
  BOOST_CHECK((Fx_LPF - dataEuler->Fx).isZero(tol));
  BOOST_CHECK((Lx_LPF - dataEuler->Lx).isZero(tol));
  BOOST_CHECK((Lxx_LPF - dataEuler->Lxx).isZero(tol));

  // Non LPF dimensions
  // Size varying stuff  
  const Eigen::MatrixXd& Fu_nonLPF = dataLPF->Fu.topRows(ndx);
  const Eigen::MatrixXd& Lu_nonLPF = dataLPF->Lu;
468
  const Eigen::MatrixXd& Lxu_nonLPF = dataLPF->Lxu;
469
470
471
472
  const Eigen::MatrixXd& Luu_nonLPF = dataLPF->Luu;
  for(std::size_t i=0; i<non_lpf_torque_ids.size();i++){
    BOOST_CHECK((Fu_nonLPF.col(non_lpf_torque_ids[i]) - dataEuler->Fu.col(non_lpf_torque_ids[i])).isZero(tol));
    BOOST_CHECK((Lu_nonLPF(non_lpf_torque_ids[i]) - dataEuler->Lu(non_lpf_torque_ids[i])) <= tol);
473
474
    BOOST_CHECK((Lxu_nonLPF.topRows(ndx).col(non_lpf_torque_ids[i]) - dataEuler->Lxu.col(non_lpf_torque_ids[i])).isZero(tol));
    for(std::size_t j=0; j<non_lpf_torque_ids.size();j++){
475
      BOOST_CHECK((Luu_nonLPF(non_lpf_torque_ids[i],non_lpf_torque_ids[j]) - dataEuler->Luu(non_lpf_torque_ids[i], non_lpf_torque_ids[j])) <= tol );
476
    }
477
478
479
    for(std::size_t j=0; j<lpf_torque_ids.size();j++){
      BOOST_CHECK((Lxu_nonLPF.bottomRows(ntau)(j, non_lpf_torque_ids[i]) - dataEuler->Luu(lpf_torque_ids[j], non_lpf_torque_ids[i])) <= tol );
    }
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
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537


  // Test terminal calcDiff
  // Computing the action
  modelLPF->set_dt(0.);
  modelEuler->set_dt(0.);
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataLPFTerminal = modelLPF->createData();
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataEulerTerminal = modelEuler->createData();
  modelLPF->calc(dataLPFTerminal, y);
  modelEuler->calc(dataEulerTerminal, x); 
  modelLPF->calcDiff(dataLPFTerminal, y);
  modelEuler->calcDiff(dataEulerTerminal, x); 
  // All or some joint are LPF
  // Size varying stuff  
  const Eigen::MatrixXd& Fu_LPF_term = dataLPFTerminal->Fx.topRightCorner(ndx, ntau);
  const Eigen::MatrixXd& Lu_LPF_term = dataLPFTerminal->Lx.tail(ntau);
  const Eigen::MatrixXd& Lxu_LPF_term = dataLPFTerminal->Lxx.topRightCorner(ndx, ntau);
  const Eigen::MatrixXd& Luu_LPF_term = dataLPFTerminal->Lxx.bottomRightCorner(ntau, ntau);
  for (std::size_t i = 0; i < lpf_torque_ids.size(); i++) {
    BOOST_CHECK(
        (Fu_LPF_term.col(i) - dataEulerTerminal->Fu.col(lpf_torque_ids[i])).isZero(tol));
    BOOST_CHECK((Lu_LPF_term(i) - dataEulerTerminal->Lu(lpf_torque_ids[i])) <= tol);
    BOOST_CHECK((Lxu_LPF_term.col(i) - dataEulerTerminal->Lxu.col(lpf_torque_ids[i])).isZero(tol));
    for(std::size_t j=0; j<lpf_torque_ids.size();j++){
      BOOST_CHECK( (Luu_LPF_term(i,j)- dataEulerTerminal->Luu(lpf_torque_ids[i], lpf_torque_ids[j]) ) <= tol );
    }
  }
  // Fixed size stuff
  const Eigen::MatrixXd& Fx_LPF_term = dataLPFTerminal->Fx.topLeftCorner(ndx, ndx);
  const Eigen::MatrixXd& Lx_LPF_term = dataLPFTerminal->Lx.head(ndx);
  const Eigen::MatrixXd& Lxx_LPF_term = dataLPFTerminal->Lxx.topLeftCorner(ndx, ndx);
  // Testing the partials w.r.t. u match blocks in partial w.r.t. augmented state y
  // if(!(Fx_LPF_term - dataEulerTerminal->Fx).isZero(tol)){
  //   std::cout << " Fx_lpf - Fx_euler terminal = " << std::endl;
  //   std::cout << (Fx_LPF_term - dataEulerTerminal->Fx) << std::endl;
  // }
  BOOST_CHECK((Fx_LPF_term - dataEulerTerminal->Fx).isZero(tol));
  BOOST_CHECK((Lx_LPF_term - dataEulerTerminal->Lx).isZero(tol));
  BOOST_CHECK((Lxx_LPF_term - dataEulerTerminal->Lxx).isZero(tol));

  // Non LPF dimensions
  // Size varying stuff  
  const Eigen::MatrixXd& Fu_nonLPF_term = dataLPFTerminal->Fu.topRows(ndx);
  const Eigen::MatrixXd& Lu_nonLPF_term = dataLPFTerminal->Lu;
  const Eigen::MatrixXd& Lxu_nonLPF_term = dataLPFTerminal->Lxu;
  const Eigen::MatrixXd& Luu_nonLPF_term = dataLPFTerminal->Luu;
  for(std::size_t i=0; i<non_lpf_torque_ids.size();i++){
    BOOST_CHECK((Fu_nonLPF_term.col(non_lpf_torque_ids[i]) - dataEulerTerminal->Fu.col(non_lpf_torque_ids[i])).isZero(tol));
    BOOST_CHECK((Lu_nonLPF_term(non_lpf_torque_ids[i]) - dataEulerTerminal->Lu(non_lpf_torque_ids[i])) <= tol);
    BOOST_CHECK((Lxu_nonLPF_term.topRows(ndx).col(non_lpf_torque_ids[i]) - dataEulerTerminal->Lxu.col(non_lpf_torque_ids[i])).isZero(tol));
    for(std::size_t j=0; j<non_lpf_torque_ids.size();j++){
      BOOST_CHECK((Luu_nonLPF_term(non_lpf_torque_ids[i],non_lpf_torque_ids[j]) - dataEulerTerminal->Luu(non_lpf_torque_ids[i], non_lpf_torque_ids[j])) <= tol );
    }
    for(std::size_t j=0; j<lpf_torque_ids.size();j++){
      BOOST_CHECK((Lxu_nonLPF_term.bottomRows(ntau)(j, non_lpf_torque_ids[i]) - dataEulerTerminal->Luu(lpf_torque_ids[j], non_lpf_torque_ids[i])) <= tol );
    }
  }
538
539
}

540
541
//----------------------------------------------------------------------------//

542
543
544
545
546
void register_action_model_unit_tests(
    ActionModelLPFTypes::Type iam_type,
    DifferentialActionModelTypes::Type dam_type,
    PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
    ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) {
547
  boost::test_tools::output_test_stream test_name;
548
549
550
551
552
553
  if (dam_type == DifferentialActionModelTypes::
                      DifferentialActionModelContact1DFwdDynamics_TalosArm ||
      dam_type == DifferentialActionModelTypes::
                      DifferentialActionModelContact1DFwdDynamics_HyQ) {
    test_name << "test_" << iam_type << "_" << dam_type << "_" << ref_type
              << "_" << mask_type;
Sébastien Kleff's avatar
Sébastien Kleff committed
554
  } else {
555
556
    test_name << "test_" << iam_type << "_" << dam_type << "_" << ref_type;
  }
557
558
  std::cout << "Running " << test_name.str() << std::endl;
  test_suite* ts = BOOST_TEST_SUITE(test_name.str());
559
  // ts->add(BOOST_TEST_CASE(
560
561
  //     boost::bind(&test_check_data, iam_type, dam_type, ref_type,
  //     mask_type)));
562
  
563
564
565
566
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_state, iam_type,
                                      dam_type, ref_type, mask_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_cost, iam_type,
                                      dam_type, ref_type, mask_type)));
567
568
  ts->add(
      BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_action_model,
569
                                  iam_type, dam_type, ref_type, mask_type)));
570
571
572
573
  // seems incompatible with euler equivalence test
  // ts->add(
  //     BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_action_model_terminal,
  //                                 iam_type, dam_type, ref_type, mask_type))); 
574
  // Equivalence with Euler when alpha=0 or ntau=0
575
576
577
578
579
  if(iam_type == ActionModelLPFTypes::Type::IntegratedActionModelLPF_alpha0){
    ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_alpha0_equivalent_euler, iam_type, dam_type, ref_type, mask_type)));
  }
  if(iam_type == ActionModelLPFTypes::Type::IntegratedActionModelLPF_NONE){
    ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_NONE_equivalent_euler, iam_type, dam_type, ref_type, mask_type)));
580
    // ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff_NONE_equivalent_euler, iam_type, dam_type, ref_type, mask_type)));
581
  }
582
583
584
  ts->add(
      BOOST_TEST_CASE(boost::bind(&test_calcDiff_explicit_equivalent_euler,
                                  iam_type, dam_type, ref_type, mask_type)));
585
586
587
588
  framework::master_test_suite().add(ts);
}

bool init_function() {
589
  // free
Sébastien Kleff's avatar
format    
Sébastien Kleff committed
590
  // register_action_model_unit_tests(ActionModelLPFTypes::IntegratedActionModelLPF,
591
  //                                  DifferentialActionModelTypes::DifferentialActionModelFreeFwdDynamics_TalosArm);
592
593
  for (size_t i = 0; i < ActionModelLPFTypes::all.size(); ++i) {
    for (size_t j = 0; j < DifferentialActionModelTypes::all.size(); ++j) {
Sébastien Kleff's avatar
Sébastien Kleff committed
594
      if (DifferentialActionModelTypes::all[j] ==
595
596
              DifferentialActionModelTypes::
                  DifferentialActionModelFreeFwdDynamics_TalosArm ||
Sébastien Kleff's avatar
Sébastien Kleff committed
597
          DifferentialActionModelTypes::all[j] ==
598
599
600
601
              DifferentialActionModelTypes::
                  DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed) {
        register_action_model_unit_tests(ActionModelLPFTypes::all[i],
                                         DifferentialActionModelTypes::all[j]);
602
603
604
      }
    }
  }
605
606
607
  // 3D contact
  for (size_t i = 0; i < ActionModelLPFTypes::all.size(); ++i) {
    for (size_t j = 0; j < DifferentialActionModelTypes::all.size(); ++j) {
Sébastien Kleff's avatar
Sébastien Kleff committed
608
      if (DifferentialActionModelTypes::all[j] ==
609
610
              DifferentialActionModelTypes::
                  DifferentialActionModelContact3DFwdDynamics_TalosArm ||
Sébastien Kleff's avatar
Sébastien Kleff committed
611
          DifferentialActionModelTypes::all[j] ==
612
613
              DifferentialActionModelTypes::
                  DifferentialActionModelContact3DFwdDynamics_HyQ) {
Sébastien Kleff's avatar
Sébastien Kleff committed
614
        for (size_t k = 0; k < PinocchioReferenceTypes::all.size(); ++k) {
615
616
          register_action_model_unit_tests(ActionModelLPFTypes::all[i],
                                           DifferentialActionModelTypes::all[j],
Sébastien Kleff's avatar
Sébastien Kleff committed
617
                                           PinocchioReferenceTypes::all[k]);
618
619
620
621
        }
      }
    }
  }
622
623
624
  // 1D contact
  for (size_t i = 0; i < ActionModelLPFTypes::all.size(); ++i) {
    for (size_t j = 0; j < DifferentialActionModelTypes::all.size(); ++j) {
Sébastien Kleff's avatar
Sébastien Kleff committed
625
      if (DifferentialActionModelTypes::all[j] ==
626
627
              DifferentialActionModelTypes::
                  DifferentialActionModelContact1DFwdDynamics_TalosArm ||
Sébastien Kleff's avatar
Sébastien Kleff committed
628
          DifferentialActionModelTypes::all[j] ==
629
630
              DifferentialActionModelTypes::
                  DifferentialActionModelContact1DFwdDynamics_HyQ) {
Sébastien Kleff's avatar
Sébastien Kleff committed
631
632
        for (size_t k = 0; k < PinocchioReferenceTypes::all.size(); ++k) {
          for (size_t l = 0; l < ContactModelMaskTypes::all.size(); ++l) {
633
634
635
636
            register_action_model_unit_tests(
                ActionModelLPFTypes::all[i],
                DifferentialActionModelTypes::all[j],
                PinocchioReferenceTypes::all[k], ContactModelMaskTypes::all[l]);
637
638
639
640
641
          }
        }
      }
    }
  }
642

643
644
645
  return true;
}

646
647
648
int main(int argc, char** argv) {
  return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
}