aba.cpp 8.13 KB
Newer Older
1
//
2
// Copyright (c) 2016-2020 CNRS INRIA
3
4
5
6
7
8
//

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
jcarpent's avatar
jcarpent committed
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"
jcarpent's avatar
jcarpent committed
18
#include "pinocchio/utils/timer.hpp"
19
20
21
22
23
24

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

25
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
26

27
template<typename JointModel>
28
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
35
36
37
38

  JointData jdata = jmodel.createData();

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

39
  ConfigVector_t q = LieGroupType().randomConfiguration(ql,qu);
40
41
  pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Random().matrix());
  pinocchio::Inertia::Matrix6 I_check = I;
42
43
44
45

  jmodel.calc(jdata,q);
  jmodel.calc_aba(jdata,I,true);

jcarpent's avatar
jcarpent committed
46
  Eigen::MatrixXd S = jdata.S.matrix();
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
  Eigen::MatrixXd U_check = I_check*S;
  Eigen::MatrixXd D_check = S.transpose()*U_check;
  Eigen::MatrixXd Dinv_check = D_check.inverse();
  Eigen::MatrixXd UDinv_check = U_check*Dinv_check;
  Eigen::MatrixXd update_check = U_check*Dinv_check*U_check.transpose();
  I_check -= update_check;

  BOOST_CHECK(jdata.U.isApprox(U_check));
  BOOST_CHECK(jdata.Dinv.isApprox(Dinv_check));
  BOOST_CHECK(jdata.UDinv.isApprox(UDinv_check));

  // Checking the inertia was correctly updated
  // We use isApprox as usual, except for the freeflyer,
  // where the correct result is exacly zero and isApprox would fail.
  // Only for this single case, we use the infinity norm of the difference
  if(jmodel.shortname() == "JointModelFreeFlyer")
    BOOST_CHECK((I-I_check).lpNorm<Eigen::Infinity>() < Eigen::NumTraits<double>::dummy_precision());
  else
    BOOST_CHECK(I.isApprox(I_check));
}

struct TestJointMethods{

  template <typename JointModel>
71
  void operator()(const pinocchio::JointModelBase<JointModel> &) const
72
73
74
75
76
77
78
  {
    JointModel jmodel;
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

79
  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelComposite> &) const
80
  {
81
82
83
    pinocchio::JointModelComposite jmodel_composite;
    jmodel_composite.addJoint(pinocchio::JointModelRX());
    jmodel_composite.addJoint(pinocchio::JointModelRY());
84
85
    jmodel_composite.setIndexes(0,0,0);

86
87
    //TODO: correct LieGroup
    //test_joint_methods(jmodel_composite);
88
89
90

  }

91
  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelRevoluteUnaligned> &) const
92
  {
93
    pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
94
95
96
97
98
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

99
  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelPrismaticUnaligned> &) const
100
  {
101
    pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
102
103
104
105
106
107
108
109
110
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

};

BOOST_AUTO_TEST_CASE( test_joint_basic )
{
111
  using namespace pinocchio;
112
113
114
115
116
117
118
119
120
121
122
123

  typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned
  , JointModelSpherical, JointModelSphericalZYX
  , JointModelPX, JointModelPY, JointModelPZ
  , JointModelPrismaticUnaligned
  , JointModelFreeFlyer
  , JointModelPlanar
  , JointModelTranslation
  , JointModelRUBX, JointModelRUBY, JointModelRUBZ
  > Variant;

  boost::mpl::for_each<Variant::types>(TestJointMethods());
124
125
}

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

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

  VectorXd q = VectorXd::Ones(model.nq);
137
  q.segment<4>(3).normalize();
138
139
140
141
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);
  
142
  tau = rnea(model, data_ref, q, v, a);
143
144
  aba(model, data, q, v, tau);
  
145
146
147
148
149
150
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data_ref.liMi[k].isApprox(data.liMi[k]));
    BOOST_CHECK(data_ref.v[k].isApprox(data.v[k]));
  }
  
151
152
153
154
  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
  
}

155
156
157
BOOST_AUTO_TEST_CASE ( test_aba_with_fext )
{
  using namespace Eigen;
158
  using namespace pinocchio;
159
  
160
  pinocchio::Model model; buildModels::humanoidRandom(model);
161
  
162
  pinocchio::Data data(model);
163
164
165
166
167
168
  
  VectorXd q = VectorXd::Random(model.nq);
  q.segment<4>(3).normalize();
  VectorXd v = VectorXd::Random(model.nv);
  VectorXd a = VectorXd::Random(model.nv);

169
  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Random());
170
171
  
  crba(model, data, q);
172
  computeJointJacobians(model, data, q);
173
174
175
176
177
178
179
180
  nonLinearEffects(model, data, q, v);
  data.M.triangularView<Eigen::StrictlyLower>()
  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
  

  VectorXd tau = data.M * a + data.nle;
  Data::Matrix6x J = Data::Matrix6x::Zero(6, model.nv);
  for(Model::Index i=1;i<(Model::Index)model.njoints;++i) {
181
    getJointJacobian(model, data, i, LOCAL, J);
182
183
184
185
186
187
188
189
    tau -= J.transpose()*fext[i].toVector();
    J.setZero();
  }
  aba(model, data, q, v, tau, fext);
  
  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
}

190
191
192
BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea )
{
  using namespace Eigen;
193
  using namespace pinocchio;
194
  
195
  pinocchio::Model model; buildModels::humanoidRandom(model);
196
  
197
198
  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
  
  VectorXd q = VectorXd::Ones(model.nq);
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);
  
  crba(model, data_ref, q);
  nonLinearEffects(model, data_ref, q, v);
  data_ref.M.triangularView<Eigen::StrictlyLower>()
  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
  
  tau = data_ref.M * a + data_ref.nle;
  aba(model, data, q, v, tau);
  
  VectorXd tau_ref = rnea(model, data_ref, q, v, a);
  BOOST_CHECK(tau_ref.isApprox(tau, 1e-12));
  
  
217
218
  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
  
219
220
221
222
223
}

BOOST_AUTO_TEST_CASE ( test_computeMinverse )
{
  using namespace Eigen;
224
  using namespace pinocchio;
225
  
226
  pinocchio::Model model;
227
  buildModels::humanoidRandom(model);
228
229
  model.gravity.setZero();
  
230
231
  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
  
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);

  crba(model, data_ref, q);
  data_ref.M.triangularView<Eigen::StrictlyLower>()
  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
  MatrixXd Minv_ref(data_ref.M.inverse());

  computeMinverse(model, data, q);

  
  BOOST_CHECK(data.Minv.topRows<6>().isApprox(Minv_ref.topRows<6>()));
  
  data.Minv.triangularView<Eigen::StrictlyLower>()
  = data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
  
  BOOST_CHECK(data.Minv.isApprox(Minv_ref));
  
//  std::cout << "Minv:\n" << data.Minv.block<10,10>(0,0) << std::endl;
//  std::cout << "Minv_ref:\n" << Minv_ref.block<10,10>(0,0) << std::endl;
//
//  std::cout << "Minv:\n" << data.Minv.bottomRows<10>() << std::endl;
//  std::cout << "Minv_ref:\n" << Minv_ref.bottomRows<10>() << std::endl;
  
259
}
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285

BOOST_AUTO_TEST_CASE(test_multiple_calls)
{
  using namespace Eigen;
  using namespace pinocchio;
  
  Model model;
  buildModels::humanoidRandom(model);
  
  Data data1(model), data2(model);
  
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  
  computeMinverse(model,data1,q);
  data2 = data1;
  
  for(int k = 0; k < 20; ++k)
  {
    computeMinverse(model,data1,q);
  }
  
  BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
}

286
BOOST_AUTO_TEST_SUITE_END ()