GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/centroidal.cpp Lines: 229 230 99.6 %
Date: 2024-04-26 13:14:21 Branches: 819 1636 50.1 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
3
//
4
5
#include "pinocchio/algorithm/crba.hpp"
6
#include "pinocchio/algorithm/centroidal.hpp"
7
#include "pinocchio/algorithm/rnea.hpp"
8
#include "pinocchio/algorithm/jacobian.hpp"
9
#include "pinocchio/algorithm/center-of-mass.hpp"
10
#include "pinocchio/algorithm/joint-configuration.hpp"
11
12
#include "pinocchio/parsers/sample-models.hpp"
13
14
#include "pinocchio/utils/timer.hpp"
15
16
#include <iostream>
17
18
#include <boost/test/unit_test.hpp>
19
#include <boost/utility/binary.hpp>
20
21
template<typename JointModel>
22
2
static void addJointAndBody(pinocchio::Model & model,
23
                            const pinocchio::JointModelBase<JointModel> & joint,
24
                            const std::string & parent_name,
25
                            const std::string & name,
26
                            const pinocchio::SE3 placement = pinocchio::SE3::Random(),
27
                            bool setRandomLimits = true)
28
{
29
  using namespace pinocchio;
30
  typedef typename JointModel::ConfigVector_t CV;
31
  typedef typename JointModel::TangentVector_t TV;
32
33
  Model::JointIndex idx;
34
35
2
  if(setRandomLimits)
36










2
    idx = model.addJoint(model.getJointId(parent_name),joint,
37
                         SE3::Random(),
38
                         name + "_joint",
39
                         TV::Random() + TV::Constant(1),
40
                         TV::Random() + TV::Constant(1),
41
                         CV::Random() - CV::Constant(1),
42
                         CV::Random() + CV::Constant(1)
43
                         );
44
  else
45
    idx = model.addJoint(model.getJointId(parent_name),joint,
46
                         placement, name + "_joint");
47
48
2
  model.addJointFrame(idx);
49
50

2
  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
51

2
  model.addBodyFrame(name + "_body", idx);
52
2
}
53
54
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
55
56
















4
BOOST_AUTO_TEST_CASE(test_ccrba)
57
{
58
4
  pinocchio::Model model;
59
2
  pinocchio::buildModels::humanoidRandom(model);
60

4
  pinocchio::Data data(model), data_ref(model);
61
62

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

2
  model.upperPositionLimit.head<3>().fill( 1.);
64
4
  Eigen::VectorXd q = randomConfiguration(model);
65

4
  Eigen::VectorXd v = Eigen::VectorXd::Ones(model.nv);
66
67
2
  crba(model,data_ref,q);
68


2
  data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
69

2
  data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]);
70
71


2
  pinocchio::SE3 cMo (pinocchio::SE3::Matrix3::Identity(), -getComFromCrba(model, data_ref));
72
73
2
  ccrba(model, data, q, v);
74




2
  BOOST_CHECK(data.com[0].isApprox(-cMo.translation(),1e-12));
75




2
  BOOST_CHECK(data.oYcrb[0].matrix().isApprox(data_ref.Ycrb[0].matrix(),1e-12));
76
77
2
  pinocchio::Inertia Ig_ref (cMo.act(data.oYcrb[0]));
78




2
  BOOST_CHECK(data.Ig.matrix().isApprox(Ig_ref.matrix(),1e-12));
79
80
2
  pinocchio::SE3 oM1 (data_ref.liMi[1]);
81
2
  pinocchio::SE3 cM1 (cMo * oM1);
82
83



4
  pinocchio::Data::Matrix6x Ag_ref (cM1.inverse().toActionMatrix().transpose() * data_ref.M.topRows <6> ());
84



2
  BOOST_CHECK(data.Ag.isApprox(Ag_ref,1e-12));
85
2
}
86
87
















4
BOOST_AUTO_TEST_CASE(test_centroidal_mapping)
88
{
89
4
  pinocchio::Model model;
90
2
  pinocchio::buildModels::humanoidRandom(model);
91

4
  pinocchio::Data data(model), data_ref(model);
92
93

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

2
  model.upperPositionLimit.head<3>().fill( 1.);
95
4
  Eigen::VectorXd q = randomConfiguration(model);
96

4
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
97
98
2
  computeCentroidalMap(model, data, q);
99
2
  ccrba(model,data_ref,q,v);
100
101



2
  BOOST_CHECK(data.J.isApprox(data_ref.J));
102



2
  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
103
104
2
  computeJointJacobians(model,data_ref,q);
105
106



2
  BOOST_CHECK(data.J.isApprox(data_ref.J));
107
2
}
108
109
















4
BOOST_AUTO_TEST_CASE(test_dccrb)
110
{
111
  using namespace pinocchio;
112
4
  Model model;
113
2
  buildModels::humanoidRandom(model);
114


2
  addJointAndBody(model,JointModelSpherical(),"larm6_joint","larm7");
115

4
  Data data(model), data_ref(model);
116
117

2
  model.lowerPositionLimit.head<7>().fill(-1.);
118

2
  model.upperPositionLimit.head<7>().fill(1.);
119
120
4
  Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
121

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

4
  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
123
124


4
  const Eigen::VectorXd g = rnea(model,data_ref,q,0*v,0*a);
125
2
  rnea(model,data_ref,q,v,a);
126
127
2
  crba(model,data_ref,q);
128


2
  data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
129
130
2
  SE3 cMo(SE3::Identity());
131


2
  cMo.translation() = -getComFromCrba(model, data_ref);
132
133
2
  SE3 oM1 (data_ref.liMi[1]);
134
2
  SE3 cM1 (cMo * oM1);
135


4
  Data::Matrix6x Ag_ref (cM1.toDualActionMatrix() * data_ref.M.topRows <6> ());
136
137


2
  Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>())));
138
139
2
  ccrba(model,data_ref,q,v);
140
2
  dccrba(model,data,q,v);
141



2
  BOOST_CHECK(data.Ag.isApprox(Ag_ref));
142



2
  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
143



2
  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
144
145
2
  centerOfMass(model,data_ref,q,v,a);
146





2
  BOOST_CHECK(data_ref.vcom[0].isApprox(data.hg.linear()/data_ref.M(0,0)));
147



2
  BOOST_CHECK(data_ref.vcom[0].isApprox(data.vcom[0]));
148





2
  BOOST_CHECK(data_ref.acom[0].isApprox(hdot_ref.linear()/data_ref.M(0,0)));
149
150


2
  Force hdot(data.Ag * a + data.dAg * v);
151



2
  BOOST_CHECK(hdot.isApprox(hdot_ref));
152
153

2
  dccrba(model,data,q,0*v);
154



2
  BOOST_CHECK(data.dAg.isZero());
155
156
  // Check that dYcrb is equal to doYcrb
157
  {
158
    // Compute dYcrb
159

4
    Data data_ref(model), data_ref_plus(model), data(model);
160
161
2
    const double alpha = 1e-8;
162
4
    Eigen::VectorXd q_plus(model.nq);
163

2
    q_plus = integrate(model,q,alpha*v);
164
165
2
    forwardKinematics(model,data_ref,q);
166
2
    crba(model,data_ref,q);
167
2
    crba(model,data_ref_plus,q_plus);
168
2
    forwardKinematics(model,data_ref_plus,q_plus);
169
2
    dccrba(model,data,q,v);
170
171
58
    for(size_t i = 1; i < (size_t)model.njoints; ++i)
172
    {
173

56
      Inertia::Matrix6 dYcrb = (data_ref_plus.oMi[i].act(data_ref_plus.Ycrb[i]).matrix() -
174


112
                                data_ref.oMi[i].act(data_ref.Ycrb[i]).matrix())/alpha;
175



56
      BOOST_CHECK(data.doYcrb[i].isApprox(dYcrb,sqrt(alpha)));
176
    }
177
  }
178
179
  {
180
4
    Data data(model);
181
2
    ccrba(model,data_ref,q,v);
182
2
    SE3 oMc_ref(SE3::Identity());
183

2
    oMc_ref.translation() = data_ref.com[0];
184

4
    const Data::Matrix6x Ag_ref = oMc_ref.toDualActionMatrix() * data_ref.Ag;
185
2
    crba(model,data_ref,q);
186


4
    const Data::Matrix6x Ag_ref_from_M = data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>();
187
188
2
    const double alpha = 1e-8;
189
4
    Eigen::VectorXd q_plus(model.nq);
190

2
    q_plus = integrate(model,q,alpha*v);
191
2
    ccrba(model,data_ref,q_plus,v);
192
2
    SE3 oMc_ref_plus(SE3::Identity());
193

2
    oMc_ref_plus.translation() = data_ref.com[0];
194

4
    const Data::Matrix6x Ag_plus_ref = oMc_ref_plus.toDualActionMatrix() * data_ref.Ag;
195
2
    crba(model,data_ref,q_plus);
196


4
    const Data::Matrix6x Ag_plus_ref_from_M = data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>();
197

4
    const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref)/alpha;
198

4
    const Data::Matrix6x dAg_ref_from_M = (Ag_plus_ref_from_M - Ag_ref_from_M)/alpha;
199
200
2
    dccrba(model, data, q, v);
201
2
    SE3 oMc(SE3::Identity());
202

2
    oMc.translation() = data.com[0];
203

4
    Data::Matrix6x dAg = oMc.toDualActionMatrix() * data.dAg;
204



2
    BOOST_CHECK(oMc.isApprox(oMc_ref));
205



2
    BOOST_CHECK(dAg_ref.isApprox(dAg_ref_from_M, sqrt(alpha)));
206
  }
207
208
  // Check for dAg/dt
209
    {
210
4
    Data data(model);
211
2
    ccrba(model,data_ref,q,v);
212
4
    const Data::Matrix6x Ag_ref = data_ref.Ag;
213
214
2
    const double alpha = 1e-8;
215
4
    Eigen::VectorXd q_plus(model.nq);
216

2
    q_plus = integrate(model,q,alpha*v);
217
2
    ccrba(model,data_ref,q_plus,v);
218
4
    const Data::Matrix6x Ag_plus_ref = data_ref.Ag;
219

4
    const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref)/alpha;
220
221
2
    dccrba(model, data, q, v);
222



2
    BOOST_CHECK(data.dAg.isApprox(dAg_ref,sqrt(alpha)));
223
  }
224
225
  // Compute tensor dAg/dq
226
  {
227

6
    std::vector<Data::Matrix6x> dAgdq((size_t)model.nv,Data::Matrix6x::Zero(6,model.nv));
228

4
    Data data(model), data_fd(model);
229

4
    Eigen::VectorXd v_fd(Eigen::VectorXd::Zero(model.nv));
230
2
    ccrba(model,data_fd,q,v);
231
2
    SE3 oMc_ref(SE3::Identity());
232

2
    oMc_ref.translation() = data_fd.com[0];
233
234

4
    Data::Matrix6x Ag0 = oMc_ref.toDualActionMatrix() * data_fd.Ag;
235
2
    const Force hg0 = oMc_ref.act(data_fd.hg);
236
237
4
    Data::Matrix6x Ag_fd(6,model.nv);
238
2
    Force hg_fd;
239
2
    const double alpha = 1e-8;
240
4
    Eigen::VectorXd q_plus(model.nq);
241
4
    Data::Matrix6x dhdq(6,model.nv);
242
72
    for(int k = 0; k < model.nv; ++k)
243
    {
244
70
      v_fd[k] = alpha;
245
70
      q_plus = integrate(model,q,v_fd);
246
70
      ccrba(model,data_fd,q_plus,v);
247
70
      SE3 oMc_fd(SE3::Identity());
248

70
      oMc_fd.translation() = data_fd.com[0];
249

70
      Ag_fd = oMc_fd.toDualActionMatrix() * data_fd.Ag;
250

70
      hg_fd = oMc_fd.act(data_fd.hg);
251

70
      dAgdq[(size_t)k] = (Ag_fd - Ag0)/alpha;
252


70
      dhdq.col(k) = (hg_fd - hg0).toVector()/alpha;
253
70
      v_fd[k] = 0.;
254
    }
255
256

4
    Data::Matrix6x dAg_ref(6,model.nv); dAg_ref.setZero();
257
72
    for(int k = 0; k < model.nv; ++k)
258
    {
259

70
      dAg_ref.col(k) = dAgdq[(size_t)k] * v;
260
    }
261
262



2
    BOOST_CHECK(dhdq.isApprox(dAg_ref, sqrt(alpha)));
263




2
    BOOST_CHECK((dAg_ref * v).isApprox(dhdq * v, sqrt(alpha)));
264
  }
265
2
}
266
267
















4
BOOST_AUTO_TEST_CASE(test_centroidal_mapping_time_derivative)
268
{
269
4
  pinocchio::Model model;
270
2
  pinocchio::buildModels::humanoidRandom(model);
271

4
  pinocchio::Data data(model), data_ref(model);
272
273

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

2
  model.upperPositionLimit.head<3>().fill( 1.);
275
4
  Eigen::VectorXd q = randomConfiguration(model);
276

4
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
277
278
2
  computeCentroidalMapTimeVariation(model, data, q, v);
279
2
  dccrba(model,data_ref,q,v);
280
281



2
  BOOST_CHECK(data.J.isApprox(data_ref.J));
282



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



2
  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
284



2
  BOOST_CHECK(data.dAg.isApprox(data_ref.dAg));
285
286
2
  computeJointJacobiansTimeVariation(model,data_ref,q,v);
287
288



2
  BOOST_CHECK(data.J.isApprox(data_ref.J));
289



2
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
290
2
}
291
292
















4
BOOST_AUTO_TEST_CASE(test_computeCentroidalMomentum_computeCentroidalMomentumTimeVariation)
293
{
294
  using namespace pinocchio;
295
4
  Model model;
296
2
  buildModels::humanoidRandom(model);
297


2
  addJointAndBody(model,JointModelSpherical(),"larm6_joint","larm7");
298


4
  Data data(model), data_fk1(model), data_fk2(model), data_ref(model);
299
300

2
  model.lowerPositionLimit.head<7>().fill(-1.);
301

2
  model.upperPositionLimit.head<7>().fill( 1.);
302
303
4
  Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
304

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

4
  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
306
307
2
  ccrba(model,data_ref,q,v);
308
2
  forwardKinematics(model,data_ref,q,v);
309
2
  centerOfMass(model,data_ref,q,v,false);
310
2
  computeCentroidalMomentum(model,data,q,v);
311
312



2
  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
313



2
  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
314



2
  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
315
58
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
316
  {
317



56
    BOOST_CHECK(data.mass[k] == data_ref.mass[k]);
318



56
    BOOST_CHECK(data.com[k].isApprox(data_ref.com[k]));
319



56
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
320
  }
321
322
  // Check other signature
323
2
  forwardKinematics(model,data_fk1,q,v);
324
2
  computeCentroidalMomentum(model,data_fk1);
325
326



2
  BOOST_CHECK(data_fk1.hg.isApprox(data.hg));
327
328
2
  computeCentroidalMomentumTimeVariation(model,data,q,v,a);
329
2
  model.gravity.setZero();
330
2
  rnea(model,data_ref,q,v,a);
331
2
  dccrba(model,data_ref,q,v);
332


2
  const Force hgdot(data_ref.Ag * a + data_ref.dAg * v);
333
334



2
  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
335



2
  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
336



2
  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
337



2
  BOOST_CHECK(data.dhg.isApprox(hgdot));
338
58
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
339
  {
340



56
    BOOST_CHECK(data.mass[k] == data_ref.mass[k]);
341



56
    BOOST_CHECK(data.com[k].isApprox(data_ref.com[k]));
342



56
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
343



56
    BOOST_CHECK(data.a[k].isApprox(data_ref.a_gf[k]));
344



56
    BOOST_CHECK(data.f[k].isApprox(data_ref.f[k]));
345
  }
346
347
  // Check other signature
348
2
  forwardKinematics(model,data_fk2,q,v,a);
349
2
  computeCentroidalMomentumTimeVariation(model,data_fk2);
350
351



2
  BOOST_CHECK(data_fk2.hg.isApprox(data.hg));
352



2
  BOOST_CHECK(data_fk2.dhg.isApprox(data.dhg));
353
354
  // Check against finite differences
355
4
  Data data_fd(model);
356
2
  const double eps = 1e-8;
357

4
  Eigen::VectorXd v_plus = v + eps * a;
358

4
  Eigen::VectorXd q_plus = integrate(model,q,eps*v);
359
360

2
  const Force hg = computeCentroidalMomentum(model,data_fd,q,v);
361
2
  const SE3::Vector3 com = data_fd.com[0];
362

2
  const Force hg_plus = computeCentroidalMomentum(model,data_fd,q_plus,v_plus);
363
2
  const SE3::Vector3 com_plus = data_fd.com[0];
364
365
2
  SE3 transform(SE3::Identity());
366

2
  transform.translation() = com_plus - com;
367

2
  Force dhg_ref = (transform.act(hg_plus) - hg)/eps;
368
369



2
  BOOST_CHECK(data.dhg.isApprox(dhg_ref,sqrt(eps)));
370
2
}
371
372
BOOST_AUTO_TEST_SUITE_END()