GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/algorithm/jacobian.hxx Lines: 116 118 98.3 %
Date: 2023-08-09 08:43:58 Branches: 103 451 22.8 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2020 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_jacobian_hxx__
6
#define __pinocchio_jacobian_hxx__
7
8
#include "pinocchio/multibody/visitor.hpp"
9
#include "pinocchio/algorithm/check.hpp"
10
11
/// @cond DEV
12
13
namespace pinocchio
14
{
15
16
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
17
  struct JointJacobiansForwardStep
18
  : public fusion::JointUnaryVisitorBase< JointJacobiansForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> >
19
  {
20
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
21
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
22
23
    typedef boost::fusion::vector<const Model &,
24
                                  Data &,
25
                                  const ConfigVectorType &,
26
                                  Matrix6xLike &> ArgsType;
27
28
    template<typename JointModel>
29
14358
    static void algo(const JointModelBase<JointModel> & jmodel,
30
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
31
                     const Model & model,
32
                     Data & data,
33
                     const Eigen::MatrixBase<ConfigVectorType> & q,
34
                     const Eigen::MatrixBase<Matrix6xLike> & J)
35
    {
36
      typedef typename Model::JointIndex JointIndex;
37
38
14358
      const JointIndex & i = jmodel.id();
39
14358
      const JointIndex & parent = model.parents[i];
40
41
14358
      jmodel.calc(jdata.derived(),q.derived());
42
43
14358
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
44
14358
      if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
45
562
      else         data.oMi[i] = data.liMi[i];
46
47
14358
      Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
48

14358
      jmodel.jointCols(J_) = data.oMi[i].act(jdata.S());
49
    }
50
51
  };
52
53
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
54
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
55
285
  computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
56
                        DataTpl<Scalar,Options,JointCollectionTpl> & data,
57
                        const Eigen::MatrixBase<ConfigVectorType> & q)
58
  {
59
285
    assert(model.check(data) && "data is not consistent with model.");
60






285
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
61
62
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
63
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
64
    typedef typename Model::JointIndex JointIndex;
65
    typedef typename Data::Matrix6x Matrix6x;
66
67
    typedef JointJacobiansForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6x> Pass;
68
    typedef typename Pass::ArgsType ArgsType;
69
7460
    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
70
    {
71
7175
      Pass::run(model.joints[i],data.joints[i],
72
                ArgsType(model,data,q.derived(),
73
7175
                         PINOCCHIO_EIGEN_CONST_CAST(Matrix6x,data.J)));
74
    }
75
76
285
    return data.J;
77
  }
78
79
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
80
  struct JointJacobiansForwardStep2
81
  : public fusion::JointUnaryVisitorBase< JointJacobiansForwardStep2<Scalar,Options,JointCollectionTpl> >
82
  {
83
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
84
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
85
86
    typedef boost::fusion::vector<Data &> ArgsType;
87
88
    template<typename JointModel>
89
54
    static void algo(const JointModelBase<JointModel> & jmodel,
90
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
91
                     Data & data)
92
    {
93
      typedef typename Model::JointIndex JointIndex;
94
95
54
      const JointIndex & i = jmodel.id();
96

54
      jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
97
    }
98
99
  };
100
101
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
102
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
103
1
  computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
104
                        DataTpl<Scalar,Options,JointCollectionTpl> & data)
105
  {
106
1
    assert(model.check(data) && "data is not consistent with model.");
107
108
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
109
    typedef typename Model::JointIndex JointIndex;
110
111
    typedef JointJacobiansForwardStep2<Scalar,Options,JointCollectionTpl> Pass;
112
28
    for(JointIndex i=1; i< (JointIndex)model.njoints; ++i)
113
    {
114
27
      Pass::run(model.joints[i],data.joints[i],
115
                typename Pass::ArgsType(data));
116
    }
117
118
1
    return data.J;
119
  }
120
121
  namespace details
122
  {
123
    template<typename Scalar, int Options, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
124
140
    void translateJointJacobian(const SE3Tpl<Scalar,Options> & placement,
125
                                const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
126
                                const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
127
    {
128






140
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
129







140
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), Jout.cols());
130






140
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
131
132
140
      Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);
133
134
      typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
135
      typedef const MotionRef<ConstColXprIn> MotionIn;
136
137
      typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
138
      typedef MotionRef<ColXprOut> MotionOut;
139
140
420
      for(Eigen::DenseIndex j=0; j < Jin.cols(); ++j)
141
      {
142

280
        MotionIn v_in(Jin.col(j));
143

280
        MotionOut v_out(Jout_.col(j));
144
145
280
        v_out = v_in;
146


280
        v_out.linear() -= placement.translation().cross(v_in.angular());
147
      }
148
    }
149
150
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
151
363
    void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
152
                                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
153
                                const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex joint_id,
154
                                const ReferenceFrame rf,
155
                                const SE3Tpl<Scalar,Options> & placement,
156
                                const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
157
                                const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
158
    {
159
363
      assert(model.check(data) && "data is not consistent with model.");
160
161






363
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
162






363
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nv);
163
164






363
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
165






363
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv);
166
167
363
      Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);
168
169
      typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
170
      typedef const MotionRef<ConstColXprIn> MotionIn;
171
172
      typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
173
      typedef MotionRef<ColXprOut> MotionOut;
174
175
363
      const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
176

363
      switch(rf)
177
      {
178
53
        case WORLD:
179
        {
180
617
          for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
181
          {
182

564
            MotionIn v_in(Jin.col(j));
183

564
            MotionOut v_out(Jout_.col(j));
184
185
564
            v_out = v_in;
186
          }
187
53
          break;
188
        }
189
46
        case LOCAL_WORLD_ALIGNED:
190
        {
191
514
          for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
192
          {
193

468
            MotionIn v_in(Jin.col(j));
194

468
            MotionOut v_out(Jout_.col(j));
195
196
468
            v_out = v_in;
197


468
            v_out.linear() -= placement.translation().cross(v_in.angular());
198
          }
199
46
          break;
200
        }
201
264
        case LOCAL:
202
        {
203
3121
          for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
204
          {
205

2857
            MotionIn v_in(Jin.col(j));
206

2857
            MotionOut v_out(Jout_.col(j));
207
208

2857
            v_out = placement.actInv(v_in);
209
          }
210
264
          break;
211
        }
212
        default:
213
          PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "must never happened");
214
          break;
215
      }
216
363
    }
217
218
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
219
337
    void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
220
                                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
221
                                const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex joint_id,
222
                                const ReferenceFrame rf,
223
                                const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
224
                                const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
225
    {
226
      typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
227
337
      const typename Data::SE3 & oMjoint = data.oMi[joint_id];
228
229
337
      translateJointJacobian(model,data,joint_id,rf,oMjoint,Jin,Jout);
230
337
    }
231
  } // namespace details
232
233
  /* Return the jacobian of the output frame attached to joint <jointId> in the
234
   world frame or in the local frame depending on the template argument. The
235
   function computeJacobians should have been called first. */
236
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
237
331
  inline void getJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
238
                               const DataTpl<Scalar,Options,JointCollectionTpl> & data,
239
                               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex jointId,
240
                               const ReferenceFrame rf,
241
                               const Eigen::MatrixBase<Matrix6xLike> & J)
242
  {
243
331
    assert(model.check(data) && "data is not consistent with model.");
244
245
331
    details::translateJointJacobian(model,data,jointId,rf,
246
331
                                    data.J,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
247
331
  }
248
249
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
250
  struct JointJacobianForwardStep
251
  : public fusion::JointUnaryVisitorBase< JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> >
252
  {
253
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
254
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
255
256
    typedef boost::fusion::vector<const Model &,
257
                                  Data &,
258
                                  const ConfigVectorType &,
259
                                  Matrix6xLike &
260
                                  > ArgsType;
261
262
    template<typename JointModel>
263
2264
    static void algo(const JointModelBase<JointModel> & jmodel,
264
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
265
                     const Model & model,
266
                     Data & data,
267
                     const Eigen::MatrixBase<ConfigVectorType> & q,
268
                     const Eigen::MatrixBase<Matrix6xLike> & J)
269
    {
270
      typedef typename Model::JointIndex JointIndex;
271
2264
      const JointIndex & i = jmodel.id();
272
2264
      const JointIndex & parent = model.parents[i];
273
274
2264
      jmodel.calc(jdata.derived(),q.derived());
275
276
2264
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
277
2264
      data.iMf[parent] = data.liMi[i]*data.iMf[i];
278
279
2264
      Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
280

2264
      jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S());
281
    }
282
283
  };
284
285
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
286
183
  inline void computeJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
287
                                   DataTpl<Scalar,Options,JointCollectionTpl> & data,
288
                                   const Eigen::MatrixBase<ConfigVectorType> & q,
289
                                   const JointIndex jointId,
290
                                   const Eigen::MatrixBase<Matrix6xLike> & J)
291
  {
292
183
    assert(model.check(data) && "data is not consistent with model.");
293






183
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
294
295
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
296
    typedef typename Model::JointIndex JointIndex;
297
298
183
    data.iMf[jointId].setIdentity();
299
    typedef JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> Pass;
300
1289
    for(JointIndex i=jointId; i>0; i=model.parents[i])
301
    {
302
1106
      Pass::run(model.joints[i],data.joints[i],
303
1106
                typename Pass::ArgsType(model,data,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J)));
304
    }
305
183
  }
306
307
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
308
  struct JointJacobiansTimeVariationForwardStep
309
  : public fusion::JointUnaryVisitorBase< JointJacobiansTimeVariationForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
310
  {
311
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
312
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
313
314
    typedef boost::fusion::vector<const Model &,
315
                                  Data &,
316
                                  const ConfigVectorType &,
317
                                  const TangentVectorType &> ArgsType;
318
319
    template<typename JointModel>
320
702
    static void algo(const JointModelBase<JointModel> & jmodel,
321
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
322
                     const Model & model,
323
                     Data & data,
324
                     const Eigen::MatrixBase<ConfigVectorType> & q,
325
                     const Eigen::MatrixBase<TangentVectorType> & v)
326
    {
327
      typedef typename Model::JointIndex JointIndex;
328
      typedef typename Data::SE3 SE3;
329
      typedef typename Data::Motion Motion;
330
331
702
      const JointIndex & i = (JointIndex) jmodel.id();
332
702
      const JointIndex & parent = model.parents[i];
333
334
702
      SE3 & oMi = data.oMi[i];
335
702
      Motion & vJ = data.v[i];
336
337
702
      jmodel.calc(jdata.derived(),q.derived(),v.derived());
338
339

702
      vJ = jdata.v();
340
341

702
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
342
702
      if(parent>0)
343
      {
344
676
        oMi = data.oMi[parent]*data.liMi[i];
345

676
        vJ += data.liMi[i].actInv(data.v[parent]);
346
      }
347
      else
348
      {
349
26
        oMi = data.liMi[i];
350
      }
351
352


702
      jmodel.jointCols(data.J) = oMi.act(jdata.S());
353
354
      // Spatial velocity of joint i expressed in the global frame o
355
702
      data.ov[i] = oMi.act(vJ);
356
357
      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
358
702
      ColsBlock dJcols = jmodel.jointCols(data.dJ);
359
702
      ColsBlock Jcols = jmodel.jointCols(data.J);
360
361
702
      motionSet::motionAction(data.ov[i],Jcols,dJcols);
362
    }
363
364
  };
365
366
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
367
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
368
13
  computeJointJacobiansTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
369
                                     DataTpl<Scalar,Options,JointCollectionTpl> & data,
370
                                     const Eigen::MatrixBase<ConfigVectorType> & q,
371
                                     const Eigen::MatrixBase<TangentVectorType> & v)
372
  {
373
13
    assert(model.check(data) && "data is not consistent with model.");
374






13
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
375






13
    PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
376
377
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
378
    typedef typename Model::JointIndex JointIndex;
379
380
    typedef JointJacobiansTimeVariationForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Pass;
381
364
    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
382
    {
383
351
      Pass::run(model.joints[i],data.joints[i],
384
                typename Pass::ArgsType(model,data,q.derived(),v.derived()));
385
    }
386
387
13
    return data.dJ;
388
  }
389
390
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
391
6
  inline void getJointJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
392
                                            const DataTpl<Scalar,Options,JointCollectionTpl> & data,
393
                                            const JointIndex jointId,
394
                                            const ReferenceFrame rf,
395
                                            const Eigen::MatrixBase<Matrix6xLike> & dJ)
396
  {
397
6
    details::translateJointJacobian(model,data,jointId,rf,
398
6
                                    data.dJ,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
399
6
  }
400
401
} // namespace pinocchio
402
403
/// @endcond
404
405
#endif // ifndef __pinocchio_jacobian_hxx__