GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/algorithm/rnea-second-order-derivatives.hxx Lines: 206 206 100.0 %
Date: 2024-04-26 13:14:21 Branches: 380 1095 34.7 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2017-2020 CNRS INRIA
3
4
#ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hxx__
5
#define __pinocchio_algorithm_rnea_second_order_derivatives_hxx__
6
7
#include "pinocchio/algorithm/check.hpp"
8
#include "pinocchio/multibody/visitor.hpp"
9
10
namespace pinocchio {
11
12
template <typename Scalar, int Options,
13
          template <typename, int> class JointCollectionTpl,
14
          typename ConfigVectorType, typename TangentVectorType1,
15
          typename TangentVectorType2>
16
struct ComputeRNEASecondOrderDerivativesForwardStep
17
    : public fusion::JointUnaryVisitorBase<ComputeRNEASecondOrderDerivativesForwardStep<
18
          Scalar, Options, JointCollectionTpl, ConfigVectorType,
19
          TangentVectorType1, TangentVectorType2>> {
20
  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
21
  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
22
23
  typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &,
24
                                const TangentVectorType1 &,
25
                                const TangentVectorType2 &>
26
      ArgsType;
27
28
  template <typename JointModel>
29
216
  static void algo(const JointModelBase<JointModel> &jmodel,
30
                   JointDataBase<typename JointModel::JointDataDerived> &jdata,
31
                   const Model &model, Data &data,
32
                   const Eigen::MatrixBase<ConfigVectorType> &q,
33
                   const Eigen::MatrixBase<TangentVectorType1> &v,
34
                   const Eigen::MatrixBase<TangentVectorType2> &a) {
35
    typedef typename Model::JointIndex JointIndex;
36
    typedef typename Data::Motion Motion;
37
    typedef typename Data::Inertia Inertia;
38
39
216
    const JointIndex i = jmodel.id();
40
216
    const JointIndex parent = model.parents[i];
41
216
    Motion &ov = data.ov[i];
42
216
    Motion &oa = data.oa[i];
43
216
    Motion &vJ = data.v[i];
44
45
216
    jmodel.calc(jdata.derived(), q.derived(), v.derived());
46
47

216
    data.liMi[i] = model.jointPlacements[i] * jdata.M();
48
216
    if (parent > 0) {
49
208
      data.oMi[i] = data.oMi[parent] * data.liMi[i];
50
208
      ov = data.ov[parent];
51
208
      oa = data.oa[parent];
52
    } else {
53
8
      data.oMi[i] = data.liMi[i];
54
8
      ov.setZero();
55
8
      oa = -model.gravity;
56
    }
57
58
    typedef typename SizeDepType<JointModel::NV>::template ColsReturn<
59
        typename Data::Matrix6x>::Type ColsBlock;
60
216
    ColsBlock J_cols = jmodel.jointCols(
61
216
        data.J); // data.J has all the phi (in world frame) stacked in columns
62
216
    ColsBlock psid_cols =
63
216
        jmodel.jointCols(data.psid); // psid_cols is the psi_dot in world frame
64
216
    ColsBlock psidd_cols = jmodel.jointCols(
65
216
        data.psidd); // psidd_cols is the psi_dotdot in world frame
66
216
    ColsBlock dJ_cols =
67
216
        jmodel.jointCols(data.dJ); // This here is phi_dot in world frame
68
69


216
    J_cols.noalias() = data.oMi[i].act(
70
        jdata.S()); // J_cols is just the phi in world frame for a joint
71

216
    vJ = data.oMi[i].act(jdata.v());
72
216
    motionSet::motionAction(
73
        ov, J_cols, psid_cols); // This ov here is v(p(i)), psi_dot calcs
74
216
    motionSet::motionAction(
75
        oa, J_cols, psidd_cols); // This oa here is a(p(i)) , psi_dotdot calcs
76
216
    motionSet::motionAction<ADDTO>(
77
        ov, psid_cols,
78
        psidd_cols); // This ov here is v(p(i)) , psi_dotdot calcs
79
216
    ov += vJ;
80


216
    oa += (ov ^ vJ) +
81


216
          data.oMi[i].act(jdata.S() * jmodel.jointVelocitySelector(a) +
82
                          jdata.c());
83
216
    motionSet::motionAction(
84
        ov, J_cols, dJ_cols); // This here is phi_dot, here ov used is v(p(i)) +
85
                              // vJ Composite rigid body inertia
86
216
    Inertia &oY = data.oYcrb[i];
87
88

216
    oY = data.oMi[i].act(model.inertias[i]);
89

216
    data.oh[i] = oY * ov;
90
91


216
    data.of[i] = oY * oa + oY.vxiv(ov); // f_i in world frame
92
93
216
    data.doYcrb[i] = oY.variation(ov);
94
216
    addForceCrossMatrix(data.oh[i], data.doYcrb[i]); // BC{i}
95
  }
96
  template <typename ForceDerived, typename M6>
97
216
  static void addForceCrossMatrix(const ForceDense<ForceDerived> &f,
98
                                  const Eigen::MatrixBase<M6> &mout) {
99
216
    M6 &mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout);
100

216
    addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR,
101
                                                    ForceDerived::ANGULAR));
102

216
    addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR,
103
                                                    ForceDerived::LINEAR));
104

216
    addSkew(-f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR,
105
                                                     ForceDerived::ANGULAR));
106
216
  }
107
};
108
109
template <typename Scalar, int Options,
110
          template <typename, int> class JointCollectionTpl,
111
          typename Tensor1, typename Tensor2, typename Tensor3,
112
          typename Tensor4>
113
struct ComputeRNEASecondOrderDerivativesBackwardStep
114
    : public fusion::JointUnaryVisitorBase<ComputeRNEASecondOrderDerivativesBackwardStep<
115
          Scalar, Options, JointCollectionTpl, Tensor1, Tensor2,
116
          Tensor3, Tensor4>> {
117
  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
118
  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
119
120
  typedef boost::fusion::vector<const Model &, Data &, const Tensor1 &,
121
                                const Tensor2 &, const Tensor3 &,
122
                                const Tensor4 &>
123
      ArgsType;
124
125
  template <typename JointModel>
126
216
  static void algo(const JointModelBase<JointModel> &jmodel, const Model &model,
127
                   Data &data, const Tensor1 &d2tau_dqdq,
128
                   const Tensor2 &d2tau_dvdv, const Tensor3 &dtau_dqdv,
129
                   const Tensor3 &dtau_dadq) {
130
    typedef typename Data::Motion Motion;
131
    typedef typename Data::Force Force;
132
    typedef typename Data::Inertia Inertia;
133
    typedef typename Model::JointIndex JointIndex;
134
    typedef typename Motion::ActionMatrixType ActionMatrixType;
135
    typedef typename Data::Matrix6 Matrix6;
136
    typedef typename Data::Vector6r Vector6r;
137
    typedef typename Data::Vector6c Vector6c;
138
139
216
    const JointIndex i = jmodel.id();
140
216
    const JointIndex parent = model.parents[i];
141
142
216
    const Inertia &oYcrb = data.oYcrb[i];  // IC{i}
143
216
    const Matrix6 &oBcrb = data.doYcrb[i]; // BC{i}
144
145
216
    Tensor1 &d2tau_dqdq_ = const_cast<Tensor1 &>(d2tau_dqdq);
146
216
    Tensor2 &d2tau_dvdv_ = const_cast<Tensor2 &>(d2tau_dvdv);
147
216
    Tensor3 &dtau_dqdv_ = const_cast<Tensor3 &>(dtau_dqdv);
148
216
    Tensor4 &dtau_dadq_ = const_cast<Tensor4 &>(dtau_dadq);
149
150
216
    Vector6r u1;
151
216
    Vector6r u2;
152
216
    Vector6c u3;
153
216
    Vector6c u4;
154
216
    Vector6c u5;
155
216
    Vector6c u6;
156
216
    Vector6c u7;
157
216
    Vector6c u8;
158
216
    Vector6c u9;
159
216
    Vector6c u10;
160
216
    Vector6r u11;
161
216
    Vector6r u12;
162
216
    Vector6c u13;
163
164
216
    Matrix6 Bicphii;
165
216
    Matrix6 oBicpsidot;
166
167
    Scalar p1, p2, p3, p4, p5, p6;
168
169




216
    Matrix6 r0, r1, r2, r3, r4, r5, r6, r7;
170
171
472
    for (int p = 0; p < model.nvs[i]; p++) {
172
256
      const Eigen::DenseIndex ip = model.idx_vs[i] + p;
173
174

256
      const MotionRef<typename Data::Matrix6x::ColXpr> S_i = data.J.col(ip);          // S{i}(:,p)
175
256
      const ActionMatrixType S_iA = S_i.toActionMatrix(); //(S{i}(:,p) )x matrix
176

256
      const MotionRef<typename Data::Matrix6x::ColXpr> psid_dm = data.psid.col(ip);   // psi_dot for p DOF
177

256
      const MotionRef<typename Data::Matrix6x::ColXpr> psidd_dm = data.psidd.col(ip); // psi_ddot for p DOF
178

256
      const MotionRef<typename Data::Matrix6x::ColXpr> phid_dm = data.dJ.col(ip);     // phi_dot for p DOF
179
180

256
      r1 = Bicphii = oYcrb.variation(S_i);  // S{i}(p)x*IC{i} - IC{i} S{i}(p)x
181

256
      oBicpsidot = oYcrb.variation(psid_dm); // new Bicpsidot in world frame
182
183
256
      Force f_tmp = oYcrb * S_i; // IC{i}S{i}(:,p)
184
256
      ForceCrossMatrix(f_tmp, r0);                   // cmf_bar(IC{i}S{i}(:,p))
185
256
      Bicphii += r0;
186
187

256
      f_tmp = oYcrb * psid_dm; // IC{i}S{i}(:,p)
188
256
      addForceCrossMatrix(f_tmp, oBicpsidot); // cmf_bar(IC{i}S{i}(:,p))
189
190


256
      r2.noalias() = 2 * r0 - Bicphii;
191
192


256
      r3.noalias() =
193

256
      oBicpsidot - S_iA.transpose() * oBcrb -
194
256
          oBcrb * S_iA; // Bicpsidot + S{i}(p)x*BC{i}- BC {i}S{i}(p)x
195
196
      // r4
197



256
      f_tmp.toVector().noalias() = oBcrb.transpose() * S_i.toVector();
198
256
      ForceCrossMatrix(f_tmp, r4); // cmf_bar(BC{i}.'S{i}(:,p))
199
      // r5
200


256
      f_tmp.toVector().noalias() = oBcrb * psid_dm.toVector();
201

256
      f_tmp += S_i.cross(data.of[i]);
202

256
      motionSet::inertiaAction<ADDTO>(oYcrb, psidd_dm.toVector(), f_tmp.toVector());
203
256
      ForceCrossMatrix(
204
          f_tmp,
205
          r5); //  cmf_bar(BC{i}psi_dot{i}(:,p)+IC{i}psi_ddot{i}(:,p)+S{i}(:,p)x*f{i})
206
207
      // S{i}(:,p)x* IC{i} + r0
208


256
      r6 = r0 + oYcrb.vxi(S_i);
209
210
      // r7
211


256
      f_tmp.toVector().noalias() = oBcrb * S_i.toVector();
212

256
      f_tmp += oYcrb * (psid_dm + phid_dm);
213
256
      ForceCrossMatrix(f_tmp, r7); // cmf_bar(BC{i}S{i}(:,p) +
214
                                  // IC{i}(psi_dot{i}(:,p)+phi_dot{i}(:,p)))
215
216
256
      JointIndex j = i;
217
218
1400
      while (j > 0) {
219
220
3568
        for (int q = 0; q < model.nvs[j]; q++) {
221
2424
          const Eigen::DenseIndex jq = model.idx_vs[j] + q;
222
223

2424
          const MotionRef<typename Data::Matrix6x::ColXpr> S_j = data.J.col(jq);
224

2424
          const MotionRef<typename Data::Matrix6x::ColXpr> psid_dm = data.psid.col(jq);   // psi_dot{j}(:,q)
225

2424
          const MotionRef<typename Data::Matrix6x::ColXpr> psidd_dm = data.psidd.col(jq); // psi_ddot{j}(:,q)
226

2424
          const MotionRef<typename Data::Matrix6x::ColXpr> phid_dm = data.dJ.col(jq);     // phi_dot{j}(:,q)
227
228


2424
          u1.noalias() = S_j.toVector().transpose() * r3;
229


2424
          u2.noalias() = S_j.toVector().transpose() * r1;
230





2424
          u3.noalias() = r3 * psid_dm.toVector() + r1 * psidd_dm.toVector() + r5 * S_j.toVector();
231


2424
          u4.noalias()  = r6 * S_j.toVector();
232


2424
          u5.noalias()  = r2 * psid_dm.toVector();
233


2424
          u6.noalias()  = Bicphii * psid_dm.toVector();
234


2424
          u6.noalias()  += r7 * S_j.toVector();
235




2424
          u7.noalias()  = r3 * S_j.toVector() + r1 * (psid_dm.toVector() + phid_dm.toVector());
236


2424
          u8.noalias()  = r4 * S_j.toVector();
237


2424
          u9.noalias()  = r0 * S_j.toVector();
238


2424
          u10.noalias() = Bicphii * S_j.toVector();
239


2424
          u11.noalias() = S_j.toVector().transpose() * Bicphii;
240


2424
          u12.noalias() = psid_dm.toVector().transpose() * Bicphii;
241


2424
          u13.noalias() = r1 * S_j.toVector();
242
243
2424
          JointIndex k = j;
244
245
7632
          while (k > 0) {
246
247
22536
            for (int r = 0; r < model.nvs[k]; r++) {
248
17328
              const Eigen::DenseIndex kr = model.idx_vs[k] + r;
249
250

17328
              const MotionRef<typename Data::Matrix6x::ColXpr> S_k(data.J.col(kr));
251

17328
              const MotionRef<typename Data::Matrix6x::ColXpr> psid_dm = data.psid.col(kr);   // psi_dot{k}(:,r)
252

17328
              const MotionRef<typename Data::Matrix6x::ColXpr> psidd_dm = data.psidd.col(kr); // psi_ddot{k}(:,r)
253

17328
              const MotionRef<typename Data::Matrix6x::ColXpr> phid_dm = data.dJ.col(kr);     // phi_dot{k}(:,r)
254
255

17328
              p1 = u11 * psid_dm.toVector();
256

17328
              p2 = u9.dot(psidd_dm.toVector());
257



17328
              p2 += (-u12 + u8.transpose()) * psid_dm.toVector();
258
259
17328
              d2tau_dqdq_(ip, jq, kr) = p2;
260
17328
              dtau_dqdv_(ip, kr, jq) = -p1;
261
262
17328
              if (j != i) {
263


13464
                p3 = -u11 * S_k.toVector();
264

13464
                p4 = S_k.toVector().dot(u13);
265


13464
                d2tau_dqdq_(jq, kr, ip) = u1 * psid_dm.toVector();
266


13464
                d2tau_dqdq_(jq, kr, ip) += u2 * psidd_dm.toVector();
267

13464
                d2tau_dqdq_(jq, ip, kr) = d2tau_dqdq_(jq, kr, ip);
268
13464
                dtau_dqdv_(jq, kr, ip) = p1;
269


13464
                dtau_dqdv_(jq, ip, kr) = u1 * S_k.toVector();
270



13464
                dtau_dqdv_(jq, ip, kr) += u2 * (psid_dm.toVector() + phid_dm.toVector());
271
13464
                d2tau_dvdv_(jq, kr, ip) = -p3;
272
13464
                d2tau_dvdv_(jq, ip, kr) = -p3;
273
13464
                dtau_dadq_(kr, jq, ip) = p4;
274
13464
                dtau_dadq_(jq, kr, ip) = p4;
275
              }
276
277
17328
              if (k != j) {
278


7224
                p3 = -u11 * S_k.toVector();
279

7224
                p5 = S_k.toVector().dot(u9);
280
7224
                d2tau_dqdq_(ip, kr, jq) = p2;
281

7224
                d2tau_dqdq_(kr, ip, jq) = S_k.toVector().dot(u3);
282
7224
                d2tau_dvdv_(ip, jq, kr) = p3;
283
7224
                d2tau_dvdv_(ip, kr, jq) = p3;
284


7224
                dtau_dqdv_(ip, jq, kr) = S_k.toVector().dot(u5 + u8);
285


7224
                dtau_dqdv_(ip, jq, kr) += u9.dot(psid_dm.toVector() + phid_dm.toVector());
286

7224
                dtau_dqdv_(kr, jq, ip) = S_k.toVector().dot(u6);
287
7224
                dtau_dadq_(kr, ip, jq) = p5;
288
7224
                dtau_dadq_(ip, kr, jq) = p5;
289
7224
                if (j != i) {
290

5296
                  p6 = S_k.toVector().dot(u10);
291

5296
                  d2tau_dqdq_(kr, jq, ip) = d2tau_dqdq_(kr, ip, jq);
292
5296
                  d2tau_dvdv_(kr, ip, jq) = p6;
293
5296
                  d2tau_dvdv_(kr, jq, ip) = p6;
294

5296
                  dtau_dqdv_(kr, ip, jq) = S_k.toVector().dot(u7);
295
296
                } else {
297

1928
                  d2tau_dvdv_(kr, jq, ip) = S_k.toVector().dot(u4);
298
                }
299
300
              } else {
301


10104
                d2tau_dvdv_(ip, jq, kr) = -u2 * S_k.toVector();
302
              }
303
            }
304
305
5208
            k = model.parents[k];
306
          }
307
        }
308
1144
        j = model.parents[j];
309
      }
310
    }
311
312
216
    if (parent > 0) {
313
208
      data.oYcrb[parent] += data.oYcrb[i];
314
208
      data.doYcrb[parent] += data.doYcrb[i];
315
208
      data.of[parent] += data.of[i];
316
    }
317
  }
318
319
  // Function for cmf_bar operator
320
  template <typename ForceDerived, typename M6>
321
512
  static void ForceCrossMatrix(const ForceDense<ForceDerived> &f,
322
                               const Eigen::MatrixBase<M6> &mout) {
323
512
    M6 &mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout);
324
512
    mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::LINEAR)
325
        .setZero();
326
512
    mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR) =
327
        mout_.template block<3, 3>(ForceDerived::ANGULAR,
328


512
                                   ForceDerived::LINEAR) = skew(-f.linear());
329


512
    mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR) =
330
        skew(-f.angular());
331
512
  }
332
  template <typename ForceDerived, typename M6>
333
334
128
  static void addForceCrossMatrix(const ForceDense<ForceDerived> &f,
335
                                  const Eigen::MatrixBase<M6> &mout) {
336
128
    M6 &mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout);
337

128
    addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR,
338
                                                    ForceDerived::ANGULAR));
339

128
    addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR,
340
                                                    ForceDerived::LINEAR));
341

128
    addSkew(-f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR,
342
                                                     ForceDerived::ANGULAR));
343
128
  }
344
};
345
346
template <typename Scalar, int Options,
347
          template <typename, int> class JointCollectionTpl,
348
          typename ConfigVectorType, typename TangentVectorType1,
349
          typename TangentVectorType2, typename Tensor1,
350
          typename Tensor2, typename Tensor3, typename Tensor4>
351
8
inline void ComputeRNEASecondOrderDerivatives(
352
    const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
353
    DataTpl<Scalar, Options, JointCollectionTpl> &data,
354
    const Eigen::MatrixBase<ConfigVectorType> &q,
355
    const Eigen::MatrixBase<TangentVectorType1> &v,
356
    const Eigen::MatrixBase<TangentVectorType2> &a, const Tensor1 &d2tau_dqdq,
357
    const Tensor2 &d2tau_dvdv, const Tensor3 &dtau_dqdv,
358
    const Tensor4 &dtau_dadq) {
359
  // Extra safety here
360






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(
361
      q.size(), model.nq,
362
      "The joint configuration vector is not of right size");
363






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(
364
      v.size(), model.nv, "The joint velocity vector is not of right size");
365






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(
366
      a.size(), model.nv, "The joint acceleration vector is not of right size");
367






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dqdq.dimension(0), model.nv);
368






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dqdq.dimension(1), model.nv);
369






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dqdq.dimension(2), model.nv);
370






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dvdv.dimension(0), model.nv);
371






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dvdv.dimension(1), model.nv);
372






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dvdv.dimension(2), model.nv);
373






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dqdv.dimension(0), model.nv);
374






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dqdv.dimension(1), model.nv);
375






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dqdv.dimension(2), model.nv);
376






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(0), model.nv);
377






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(1), model.nv);
378






8
  PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(2), model.nv);
379
8
  assert(model.check(data) && "data is not consistent with model.");
380
381
  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
382
  typedef typename Model::JointIndex JointIndex;
383
384
  typedef ComputeRNEASecondOrderDerivativesForwardStep<
385
      Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1,
386
      TangentVectorType2>
387
      Pass1;
388
224
  for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) {
389
216
    Pass1::run(model.joints[i], data.joints[i],
390
               typename Pass1::ArgsType(model, data, q.derived(), v.derived(),
391
                                        a.derived()));
392
  }
393
394
  typedef ComputeRNEASecondOrderDerivativesBackwardStep<
395
      Scalar, Options, JointCollectionTpl, Tensor1, Tensor2,
396
      Tensor3, Tensor4>
397
      Pass2;
398
224
  for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) {
399
216
    Pass2::run(model.joints[i],
400
               typename Pass2::ArgsType(model, data,
401
                                        const_cast<Tensor1 &>(d2tau_dqdq),
402
                                        const_cast<Tensor2 &>(d2tau_dvdv),
403
                                        const_cast<Tensor3 &>(dtau_dqdv),
404
                                        const_cast<Tensor4 &>(dtau_dadq)));
405
  }
406
8
}
407
408
} // namespace pinocchio
409
410
#endif // ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hxx__