GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/aba.cpp Lines: 130 130 100.0 %
Date: 2024-01-23 21:41:47 Branches: 457 906 50.4 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2016-2020 CNRS INRIA
3
//
4
5
#include "pinocchio/spatial/fwd.hpp"
6
#include "pinocchio/spatial/se3.hpp"
7
#include "pinocchio/multibody/visitor.hpp"
8
#include "pinocchio/multibody/model.hpp"
9
#include "pinocchio/multibody/data.hpp"
10
#include "pinocchio/algorithm/aba.hpp"
11
#include "pinocchio/algorithm/rnea.hpp"
12
#include "pinocchio/algorithm/jacobian.hpp"
13
#include "pinocchio/algorithm/joint-configuration.hpp"
14
#include "pinocchio/algorithm/crba.hpp"
15
#include "pinocchio/parsers/sample-models.hpp"
16
17
#include "pinocchio/algorithm/compute-all-terms.hpp"
18
#include "pinocchio/utils/timer.hpp"
19
20
#include <iostream>
21
22
#include <boost/test/unit_test.hpp>
23
#include <boost/utility/binary.hpp>
24
25
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
26
27
template<typename JointModel>
28
32
void test_joint_methods(const pinocchio::JointModelBase<JointModel> & jmodel)
29
{
30
  typedef typename pinocchio::JointModelBase<JointModel>::JointDataDerived JointData;
31
  typedef typename JointModel::ConfigVector_t ConfigVector_t;
32
  typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
33
34
32
  JointData jdata = jmodel.createData();
35
36

32
  ConfigVector_t ql(ConfigVector_t::Constant(jmodel.nq(),-M_PI));
37

32
  ConfigVector_t qu(ConfigVector_t::Constant(jmodel.nq(),M_PI));
38
39

32
  ConfigVector_t q = LieGroupType().randomConfiguration(ql,qu);
40

32
  pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Random().matrix());
41
32
  pinocchio::Inertia::Matrix6 I_check = I;
42
43
32
  jmodel.calc(jdata,q);
44
32
  jmodel.calc_aba(jdata,I,true);
45
46

64
  Eigen::MatrixXd S = jdata.S.matrix();
47

64
  Eigen::MatrixXd U_check = I_check*S;
48

64
  Eigen::MatrixXd D_check = S.transpose()*U_check;
49

64
  Eigen::MatrixXd Dinv_check = D_check.inverse();
50

64
  Eigen::MatrixXd UDinv_check = U_check*Dinv_check;
51


64
  Eigen::MatrixXd update_check = U_check*Dinv_check*U_check.transpose();
52
32
  I_check -= update_check;
53
54



32
  BOOST_CHECK(jdata.U.isApprox(U_check));
55



32
  BOOST_CHECK(jdata.Dinv.isApprox(Dinv_check));
56



32
  BOOST_CHECK(jdata.UDinv.isApprox(UDinv_check));
57
58
  // Checking the inertia was correctly updated
59
  // We use isApprox as usual, except for the freeflyer,
60
  // where the correct result is exacly zero and isApprox would fail.
61
  // Only for this single case, we use the infinity norm of the difference
62

32
  if(jmodel.shortname() == "JointModelFreeFlyer")
63




2
    BOOST_CHECK((I-I_check).lpNorm<Eigen::Infinity>() < Eigen::NumTraits<double>::dummy_precision());
64
  else
65



30
    BOOST_CHECK(I.isApprox(I_check));
66
32
}
67
68
struct TestJointMethods{
69
70
  template <typename JointModel>
71
28
  void operator()(const pinocchio::JointModelBase<JointModel> &) const
72
  {
73
28
    JointModel jmodel;
74
28
    jmodel.setIndexes(0,0,0);
75
76
28
    test_joint_methods(jmodel);
77
28
  }
78
79
  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelComposite> &) const
80
  {
81
    pinocchio::JointModelComposite jmodel_composite;
82
    jmodel_composite.addJoint(pinocchio::JointModelRX());
83
    jmodel_composite.addJoint(pinocchio::JointModelRY());
84
    jmodel_composite.setIndexes(0,0,0);
85
86
    //TODO: correct LieGroup
87
    //test_joint_methods(jmodel_composite);
88
89
  }
90
91
1
  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelRevoluteUnaligned> &) const
92
  {
93
1
    pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
94
1
    jmodel.setIndexes(0,0,0);
95
96
1
    test_joint_methods(jmodel);
97
1
  }
98
99
1
  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelPrismaticUnaligned> &) const
100
  {
101
1
    pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
102
1
    jmodel.setIndexes(0,0,0);
103
104
1
    test_joint_methods(jmodel);
105
1
  }
106
107
};
108
109
















4
BOOST_AUTO_TEST_CASE( test_joint_basic )
110
{
111
  using namespace pinocchio;
112
113
  typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned
114
  , JointModelSpherical, JointModelSphericalZYX
115
  , JointModelPX, JointModelPY, JointModelPZ
116
  , JointModelPrismaticUnaligned
117
  , JointModelFreeFlyer
118
  , JointModelPlanar
119
  , JointModelTranslation
120
  , JointModelRUBX, JointModelRUBY, JointModelRUBZ
121
  > Variant;
122
123
2
  boost::mpl::for_each<Variant::types>(TestJointMethods());
124
2
}
125
126
















4
BOOST_AUTO_TEST_CASE ( test_aba_simple )
127
{
128
  using namespace Eigen;
129
  using namespace pinocchio;
130
131

4
  pinocchio::Model model; buildModels::humanoidRandom(model);
132
133
4
  pinocchio::Data data(model);
134
4
  pinocchio::Data data_ref(model);
135
136

4
  VectorXd q = VectorXd::Ones(model.nq);
137

2
  q.segment<4>(3).normalize();
138

4
  VectorXd v = VectorXd::Ones(model.nv);
139

4
  VectorXd tau = VectorXd::Zero(model.nv);
140

4
  VectorXd a = VectorXd::Ones(model.nv);
141
142

2
  tau = rnea(model, data_ref, q, v, a);
143
2
  aba(model, data, q, v, tau);
144
145
56
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
146
  {
147



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



54
    BOOST_CHECK(data_ref.v[k].isApprox(data.v[k]));
149
  }
150
151



2
  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
152
153
2
}
154
155
















4
BOOST_AUTO_TEST_CASE ( test_aba_with_fext )
156
{
157
  using namespace Eigen;
158
  using namespace pinocchio;
159
160

4
  pinocchio::Model model; buildModels::humanoidRandom(model);
161
162
4
  pinocchio::Data data(model);
163
164

4
  VectorXd q = VectorXd::Random(model.nq);
165

2
  q.segment<4>(3).normalize();
166

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

4
  VectorXd a = VectorXd::Random(model.nv);
168
169

4
  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Random());
170
171
2
  crba(model, data, q);
172
2
  computeJointJacobians(model, data, q);
173
2
  nonLinearEffects(model, data, q, v);
174
2
  data.M.triangularView<Eigen::StrictlyLower>()
175

4
  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
176
177
178

4
  VectorXd tau = data.M * a + data.nle;
179

4
  Data::Matrix6x J = Data::Matrix6x::Zero(6, model.nv);
180
56
  for(Model::Index i=1;i<(Model::Index)model.njoints;++i) {
181
54
    getJointJacobian(model, data, i, LOCAL, J);
182


54
    tau -= J.transpose()*fext[i].toVector();
183
54
    J.setZero();
184
  }
185
2
  aba(model, data, q, v, tau, fext);
186
187



2
  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
188
2
}
189
190
















4
BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea )
191
{
192
  using namespace Eigen;
193
  using namespace pinocchio;
194
195

4
  pinocchio::Model model; buildModels::humanoidRandom(model);
196
197
4
  pinocchio::Data data(model);
198
4
  pinocchio::Data data_ref(model);
199
200

4
  VectorXd q = VectorXd::Ones(model.nq);
201

4
  VectorXd v = VectorXd::Ones(model.nv);
202

4
  VectorXd tau = VectorXd::Zero(model.nv);
203

4
  VectorXd a = VectorXd::Ones(model.nv);
204
205
2
  crba(model, data_ref, q);
206
2
  nonLinearEffects(model, data_ref, q, v);
207
2
  data_ref.M.triangularView<Eigen::StrictlyLower>()
208

4
  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
209
210

2
  tau = data_ref.M * a + data_ref.nle;
211
2
  aba(model, data, q, v, tau);
212
213

4
  VectorXd tau_ref = rnea(model, data_ref, q, v, a);
214



2
  BOOST_CHECK(tau_ref.isApprox(tau, 1e-12));
215
216
217



2
  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
218
219
2
}
220
221
















4
BOOST_AUTO_TEST_CASE ( test_computeMinverse )
222
{
223
  using namespace Eigen;
224
  using namespace pinocchio;
225
226
4
  pinocchio::Model model;
227
2
  buildModels::humanoidRandom(model);
228
2
  model.gravity.setZero();
229
230
4
  pinocchio::Data data(model);
231
4
  pinocchio::Data data_ref(model);
232
233

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

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

4
  VectorXd v = VectorXd::Random(model.nv);
237
238
2
  crba(model, data_ref, q);
239
2
  data_ref.M.triangularView<Eigen::StrictlyLower>()
240

4
  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
241

4
  MatrixXd Minv_ref(data_ref.M.inverse());
242
243
2
  computeMinverse(model, data, q);
244
245
246




2
  BOOST_CHECK(data.Minv.topRows<6>().isApprox(Minv_ref.topRows<6>()));
247
248
2
  data.Minv.triangularView<Eigen::StrictlyLower>()
249

4
  = data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
250
251



2
  BOOST_CHECK(data.Minv.isApprox(Minv_ref));
252
253
//  std::cout << "Minv:\n" << data.Minv.block<10,10>(0,0) << std::endl;
254
//  std::cout << "Minv_ref:\n" << Minv_ref.block<10,10>(0,0) << std::endl;
255
//
256
//  std::cout << "Minv:\n" << data.Minv.bottomRows<10>() << std::endl;
257
//  std::cout << "Minv_ref:\n" << Minv_ref.bottomRows<10>() << std::endl;
258
259
2
}
260
261
















4
BOOST_AUTO_TEST_CASE(test_multiple_calls)
262
{
263
  using namespace Eigen;
264
  using namespace pinocchio;
265
266
4
  Model model;
267
2
  buildModels::humanoidRandom(model);
268
269

4
  Data data1(model), data2(model);
270
271

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

2
  model.upperPositionLimit.head<3>().fill(1.);
273
4
  VectorXd q = randomConfiguration(model);
274
275
2
  computeMinverse(model,data1,q);
276
2
  data2 = data1;
277
278
42
  for(int k = 0; k < 20; ++k)
279
  {
280
40
    computeMinverse(model,data1,q);
281
  }
282
283



2
  BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
284
2
}
285
286
BOOST_AUTO_TEST_SUITE_END ()