GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/aba-derivatives.cpp Lines: 201 201 100.0 %
Date: 2024-04-26 13:14:21 Branches: 777 1534 50.7 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2018-2020 CNRS INRIA
3
//
4
5
#include "pinocchio/multibody/model.hpp"
6
#include "pinocchio/multibody/data.hpp"
7
#include "pinocchio/algorithm/jacobian.hpp"
8
#include "pinocchio/algorithm/joint-configuration.hpp"
9
#include "pinocchio/algorithm/kinematics.hpp"
10
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
11
#include "pinocchio/algorithm/rnea.hpp"
12
#include "pinocchio/algorithm/rnea-derivatives.hpp"
13
#include "pinocchio/algorithm/aba.hpp"
14
#include "pinocchio/algorithm/aba-derivatives.hpp"
15
#include "pinocchio/algorithm/crba.hpp"
16
#include "pinocchio/parsers/sample-models.hpp"
17
18
#include <iostream>
19
20
#include <boost/test/unit_test.hpp>
21
#include <boost/utility/binary.hpp>
22
23
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
24
25
















4
BOOST_AUTO_TEST_CASE(test_aba_derivatives)
26
{
27
  using namespace Eigen;
28
  using namespace pinocchio;
29
30
4
  Model model;
31
2
  buildModels::humanoidRandom(model);
32
33

4
  Data data(model), data_ref(model);
34
35

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

2
  model.upperPositionLimit.head<3>().fill(1.);
37
4
  VectorXd q = randomConfiguration(model);
38

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

4
  VectorXd tau(VectorXd::Random(model.nv));
40

4
  VectorXd a(aba(model,data_ref,q,v,tau));
41
42

4
  MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
43

4
  MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
44

4
  Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
45
46
2
  computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
47
2
  computeRNEADerivatives(model,data_ref,q,v,a);
48
56
  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
49
  {
50



54
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
51



54
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
52



54
    BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
53



54
    BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa_gf[k]));
54



54
    BOOST_CHECK(data.of[k].isApprox(data_ref.of[k]));
55



54
    BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
56



54
    BOOST_CHECK(data.doYcrb[k].isApprox(data_ref.doYcrb[k]));
57
  }
58
59
2
  computeJointJacobians(model,data_ref,q);
60



2
  BOOST_CHECK(data.J.isApprox(data_ref.J));
61
62
2
  aba(model,data_ref,q,v,tau);
63



2
  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
64
65
2
  computeMinverse(model,data_ref,q);
66
2
  data_ref.Minv.triangularView<Eigen::StrictlyLower>()
67

4
  = data_ref.Minv.transpose().triangularView<Eigen::StrictlyLower>();
68
69



2
  BOOST_CHECK(aba_partial_dtau.isApprox(data_ref.Minv));
70
71



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



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



2
  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
74



2
  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
75



2
  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
76



2
  BOOST_CHECK(data.dtau_dq.isApprox(data_ref.dtau_dq));
77



2
  BOOST_CHECK(data.dtau_dv.isApprox(data_ref.dtau_dv));
78
79

4
  MatrixXd aba_partial_dq_fd(model.nv,model.nv); aba_partial_dq_fd.setZero();
80

4
  MatrixXd aba_partial_dv_fd(model.nv,model.nv); aba_partial_dv_fd.setZero();
81

4
  MatrixXd aba_partial_dtau_fd(model.nv,model.nv); aba_partial_dtau_fd.setZero();
82
83
4
  Data data_fd(model);
84

4
  VectorXd a0 = aba(model,data_fd,q,v,tau);
85

4
  VectorXd v_eps(VectorXd::Zero(model.nv));
86
4
  VectorXd q_plus(model.nq);
87
4
  VectorXd a_plus(model.nv);
88
2
  const double alpha = 1e-8;
89
66
  for(int k = 0; k < model.nv; ++k)
90
  {
91
64
    v_eps[k] += alpha;
92
64
    q_plus = integrate(model,q,v_eps);
93

64
    a_plus = aba(model,data_fd,q_plus,v,tau);
94
95


64
    aba_partial_dq_fd.col(k) = (a_plus - a0)/alpha;
96
64
    v_eps[k] -= alpha;
97
  }
98



2
  BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_fd,sqrt(alpha)));
99
100
4
  VectorXd v_plus(v);
101
66
  for(int k = 0; k < model.nv; ++k)
102
  {
103
64
    v_plus[k] += alpha;
104

64
    a_plus = aba(model,data_fd,q,v_plus,tau);
105
106


64
    aba_partial_dv_fd.col(k) = (a_plus - a0)/alpha;
107
64
    v_plus[k] -= alpha;
108
  }
109



2
  BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_fd,sqrt(alpha)));
110
111
4
  VectorXd tau_plus(tau);
112
66
  for(int k = 0; k < model.nv; ++k)
113
  {
114
64
    tau_plus[k] += alpha;
115

64
    a_plus = aba(model,data_fd,q,v,tau_plus);
116
117


64
    aba_partial_dtau_fd.col(k) = (a_plus - a0)/alpha;
118
64
    tau_plus[k] -= alpha;
119
  }
120



2
  BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_fd,sqrt(alpha)));
121
2
}
122
123
















4
BOOST_AUTO_TEST_CASE(test_aba_minimal_argument)
124
{
125
  using namespace Eigen;
126
  using namespace pinocchio;
127
128
4
  Model model;
129
2
  buildModels::humanoidRandom(model);
130
131

4
  Data data(model), data_ref(model);
132
133

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

2
  model.upperPositionLimit.head<3>().fill(1.);
135
4
  VectorXd q = randomConfiguration(model);
136

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

4
  VectorXd tau(VectorXd::Random(model.nv));
138

4
  VectorXd a(aba(model,data_ref,q,v,tau));
139
140

4
  MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
141

4
  MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
142

4
  Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
143
144
2
  computeABADerivatives(model, data_ref, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
145
146
2
  computeABADerivatives(model, data, q, v, tau);
147
148



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



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



2
  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
151



2
  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
152



2
  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
153



2
  BOOST_CHECK(data.dtau_dq.isApprox(data_ref.dtau_dq));
154



2
  BOOST_CHECK(data.dtau_dv.isApprox(data_ref.dtau_dv));
155



2
  BOOST_CHECK(data.Minv.isApprox(aba_partial_dtau));
156



2
  BOOST_CHECK(data.ddq_dq.isApprox(aba_partial_dq));
157



2
  BOOST_CHECK(data.ddq_dv.isApprox(aba_partial_dv));
158
2
}
159
160
















4
BOOST_AUTO_TEST_CASE(test_aba_derivatives_fext)
161
{
162
  using namespace Eigen;
163
  using namespace pinocchio;
164
165
4
  Model model;
166
2
  buildModels::humanoidRandom(model);
167
168

4
  Data data(model), data_ref(model);
169
170

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

2
  model.upperPositionLimit.head<3>().fill(1.);
172
4
  VectorXd q = randomConfiguration(model);
173

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

4
  VectorXd tau(VectorXd::Random(model.nv));
175

4
  VectorXd a(aba(model,data_ref,q,v,tau));
176
177
  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
178
4
  ForceVector fext((size_t)model.njoints);
179
58
  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
180
56
    (*it).setRandom();
181
182

4
  MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
183

4
  MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
184

4
  Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
185
186
2
  computeABADerivatives(model, data, q, v, tau, fext,
187
                        aba_partial_dq, aba_partial_dv, aba_partial_dtau);
188
189
2
  aba(model,data_ref,q,v,tau,fext);
190
//  updateGlobalPlacements(model, data_ref);
191
//  for(size_t k =1; k < (size_t)model.njoints; ++k)
192
//  {
193
//    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
194
//    BOOST_CHECK(daita.of[k].isApprox(data_ref.oMi[k].act(data.f[k])));
195
//
196
//  }
197



2
  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
198
199
2
  computeABADerivatives(model,data_ref,q,v,tau);
200



2
  BOOST_CHECK(aba_partial_dv.isApprox(data_ref.ddq_dv));
201



2
  BOOST_CHECK(aba_partial_dtau.isApprox(data_ref.Minv));
202
203

4
  MatrixXd aba_partial_dq_fd(model.nv,model.nv); aba_partial_dq_fd.setZero();
204

4
  MatrixXd aba_partial_dv_fd(model.nv,model.nv); aba_partial_dv_fd.setZero();
205

4
  MatrixXd aba_partial_dtau_fd(model.nv,model.nv); aba_partial_dtau_fd.setZero();
206
207
4
  Data data_fd(model);
208

4
  const VectorXd a0 = aba(model,data_fd,q,v,tau,fext);
209

4
  VectorXd v_eps(VectorXd::Zero(model.nv));
210
4
  VectorXd q_plus(model.nq);
211
4
  VectorXd a_plus(model.nv);
212
2
  const double alpha = 1e-8;
213
66
  for(int k = 0; k < model.nv; ++k)
214
  {
215
64
    v_eps[k] += alpha;
216
64
    q_plus = integrate(model,q,v_eps);
217

64
    a_plus = aba(model,data_fd,q_plus,v,tau,fext);
218
219


64
    aba_partial_dq_fd.col(k) = (a_plus - a0)/alpha;
220
64
    v_eps[k] -= alpha;
221
  }
222



2
  BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_fd,sqrt(alpha)));
223
224
4
  VectorXd v_plus(v);
225
66
  for(int k = 0; k < model.nv; ++k)
226
  {
227
64
    v_plus[k] += alpha;
228

64
    a_plus = aba(model,data_fd,q,v_plus,tau,fext);
229
230


64
    aba_partial_dv_fd.col(k) = (a_plus - a0)/alpha;
231
64
    v_plus[k] -= alpha;
232
  }
233



2
  BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_fd,sqrt(alpha)));
234
235
4
  VectorXd tau_plus(tau);
236
66
  for(int k = 0; k < model.nv; ++k)
237
  {
238
64
    tau_plus[k] += alpha;
239

64
    a_plus = aba(model,data_fd,q,v,tau_plus,fext);
240
241


64
    aba_partial_dtau_fd.col(k) = (a_plus - a0)/alpha;
242
64
    tau_plus[k] -= alpha;
243
  }
244



2
  BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_fd,sqrt(alpha)));
245
246
  // test the shortcut
247
4
  Data data_shortcut(model);
248
2
  computeABADerivatives(model,data_shortcut,q,v,tau,fext);
249



2
  BOOST_CHECK(data_shortcut.ddq_dq.isApprox(aba_partial_dq));
250



2
  BOOST_CHECK(data_shortcut.ddq_dv.isApprox(aba_partial_dv));
251



2
  BOOST_CHECK(data_shortcut.Minv.isApprox(aba_partial_dtau));
252
2
}
253
254
















4
BOOST_AUTO_TEST_CASE(test_multiple_calls)
255
{
256
  using namespace Eigen;
257
  using namespace pinocchio;
258
259
4
  Model model;
260
2
  buildModels::humanoidRandom(model);
261
262

4
  Data data1(model), data2(model);
263
264

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

2
  model.upperPositionLimit.head<3>().fill(1.);
266
4
  VectorXd q = randomConfiguration(model);
267

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

4
  VectorXd tau(VectorXd::Random(model.nv));
269
270
2
  computeABADerivatives(model,data1,q,v,tau);
271
2
  data2 = data1;
272
273
42
  for(int k = 0; k < 20; ++k)
274
  {
275
40
    computeABADerivatives(model,data1,q,v,tau);
276
  }
277
278



2
  BOOST_CHECK(data1.J.isApprox(data2.J));
279



2
  BOOST_CHECK(data1.dJ.isApprox(data2.dJ));
280



2
  BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq));
281



2
  BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq));
282



2
  BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv));
283
284



2
  BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq));
285



2
  BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv));
286
287



2
  BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq));
288



2
  BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv));
289
290



2
  BOOST_CHECK(data1.ddq_dq.isApprox(data2.ddq_dq));
291



2
  BOOST_CHECK(data1.ddq_dv.isApprox(data2.ddq_dv));
292



2
  BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
293
2
}
294
295
















4
BOOST_AUTO_TEST_CASE(test_aba_derivatives_vs_kinematics_derivatives)
296
{
297
  using namespace Eigen;
298
  using namespace pinocchio;
299
300
4
  Model model;
301
2
  buildModels::humanoidRandom(model);
302
303

4
  Data data(model), data_ref(model);
304
305

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

2
  model.upperPositionLimit.head<3>().fill(1.);
307
4
  VectorXd q = randomConfiguration(model);
308

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

4
  VectorXd a(VectorXd::Random(model.nv));
310
311

4
  VectorXd tau = rnea(model,data_ref,q,v,a);
312
313
  /// Check againt computeGeneralizedGravityDerivatives
314

4
  MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
315

4
  MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
316

4
  MatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
317
318
2
  computeABADerivatives(model,data,q,v,tau,aba_partial_dq,aba_partial_dv,aba_partial_dtau);
319
2
  computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
320
321



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



2
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
323
324
56
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
325
  {
326



54
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
327



54
    BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
328



54
    BOOST_CHECK(data.oa[k].isApprox(data_ref.oa[k]));
329
  }
330
2
}
331
332
BOOST_AUTO_TEST_SUITE_END()