GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/centroidal.cpp Lines: 227 228 99.6 %
Date: 2023-08-09 08:43:58 Branches: 828 1652 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.isApprox(dAg_ref,sqrt(alpha)));
206



2
    BOOST_CHECK(dAg.isApprox(dAg_ref_from_M,sqrt(alpha)));
207
  }
208
209
  // Compute tensor dAg/dq
210
  {
211

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

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

4
    Eigen::VectorXd v_fd(Eigen::VectorXd::Zero(model.nv));
214
2
    ccrba(model,data_fd,q,v);
215
2
    SE3 oMc_ref(SE3::Identity());
216

2
    oMc_ref.translation() = data_fd.com[0];
217
218

4
    Data::Matrix6x Ag0 = oMc_ref.toDualActionMatrix() * data_fd.Ag;
219
2
    const Force hg0 = oMc_ref.act(data_fd.hg);
220
221
4
    Data::Matrix6x Ag_fd(6,model.nv);
222
2
    Force hg_fd;
223
2
    const double alpha = 1e-8;
224
4
    Eigen::VectorXd q_plus(model.nq);
225
4
    Data::Matrix6x dhdq(6,model.nv);
226
72
    for(int k = 0; k < model.nv; ++k)
227
    {
228
70
      v_fd[k] = alpha;
229
70
      q_plus = integrate(model,q,v_fd);
230
70
      ccrba(model,data_fd,q_plus,v);
231
70
      SE3 oMc_fd(SE3::Identity());
232

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

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

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

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


70
      dhdq.col(k) = (hg_fd - hg0).toVector()/alpha;
237
70
      v_fd[k] = 0.;
238
    }
239
240

4
    Data::Matrix6x dAg_ref(6,model.nv); dAg_ref.setZero();
241
72
    for(int k = 0; k < model.nv; ++k)
242
    {
243

70
      dAg_ref += dAgdq[(size_t)k] * v[k];
244
    }
245
246

4
    Data::Matrix6x dAg_ref_bis(6,model.nv); dAg_ref_bis.setZero();
247
72
    for(int k = 0; k < model.nv; ++k)
248
    {
249

70
      dAg_ref_bis.col(k) = dAgdq[(size_t)k] * v;
250
    }
251
252
2
    dccrba(model, data, q, v);
253
2
    SE3 oMc(SE3::Identity());
254

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

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



2
    BOOST_CHECK(dAg.isApprox(dAg_ref,sqrt(alpha)));
257



2
    BOOST_CHECK(dhdq.isApprox(dAg_ref_bis,sqrt(alpha)));
258




2
    BOOST_CHECK((dAg*v).isApprox(dhdq*v,sqrt(alpha)));
259
260
  }
261
2
}
262
263
















4
BOOST_AUTO_TEST_CASE(test_centroidal_mapping_time_derivative)
264
{
265
4
  pinocchio::Model model;
266
2
  pinocchio::buildModels::humanoidRandom(model);
267

4
  pinocchio::Data data(model), data_ref(model);
268
269

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

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

4
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
273
274
2
  computeCentroidalMapTimeVariation(model, data, q, v);
275
2
  dccrba(model,data_ref,q,v);
276
277



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



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



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



2
  BOOST_CHECK(data.dAg.isApprox(data_ref.dAg));
281
282
2
  computeJointJacobiansTimeVariation(model,data_ref,q,v);
283
284



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



2
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
286
2
}
287
288
















4
BOOST_AUTO_TEST_CASE(test_computeCentroidalMomentum_computeCentroidalMomentumTimeVariation)
289
{
290
  using namespace pinocchio;
291
4
  Model model;
292
2
  buildModels::humanoidRandom(model);
293


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


4
  Data data(model), data_fk1(model), data_fk2(model), data_ref(model);
295
296

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

2
  model.upperPositionLimit.head<7>().fill( 1.);
298
299
4
  Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
300

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

4
  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
302
303
2
  ccrba(model,data_ref,q,v);
304
2
  forwardKinematics(model,data_ref,q,v);
305
2
  centerOfMass(model,data_ref,q,v,false);
306
2
  computeCentroidalMomentum(model,data,q,v);
307
308



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



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



2
  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
311
58
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
312
  {
313



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



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



56
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
316
  }
317
318
  // Check other signature
319
2
  forwardKinematics(model,data_fk1,q,v);
320
2
  computeCentroidalMomentum(model,data_fk1);
321
322



2
  BOOST_CHECK(data_fk1.hg.isApprox(data.hg));
323
324
2
  computeCentroidalMomentumTimeVariation(model,data,q,v,a);
325
2
  model.gravity.setZero();
326
2
  rnea(model,data_ref,q,v,a);
327
2
  dccrba(model,data_ref,q,v);
328


2
  const Force hgdot(data_ref.Ag * a + data_ref.dAg * v);
329
330



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



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



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



2
  BOOST_CHECK(data.dhg.isApprox(hgdot));
334
58
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
335
  {
336



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



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



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



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



56
    BOOST_CHECK(data.f[k].isApprox(data_ref.f[k]));
341
  }
342
343
  // Check other signature
344
2
  forwardKinematics(model,data_fk2,q,v,a);
345
2
  computeCentroidalMomentumTimeVariation(model,data_fk2);
346
347



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



2
  BOOST_CHECK(data_fk2.dhg.isApprox(data.dhg));
349
350
  // Check against finite differences
351
4
  Data data_fd(model);
352
2
  const double eps = 1e-8;
353

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

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

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

2
  const Force hg_plus = computeCentroidalMomentum(model,data_fd,q_plus,v_plus);
359
2
  const SE3::Vector3 com_plus = data_fd.com[0];
360
361
2
  SE3 transform(SE3::Identity());
362

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

2
  Force dhg_ref = (transform.act(hg_plus) - hg)/eps;
364
365



2
  BOOST_CHECK(data.dhg.isApprox(dhg_ref,sqrt(eps)));
366
2
}
367
368
BOOST_AUTO_TEST_SUITE_END()