jacobian.hxx 15.9 KB
Newer Older
1
//
2
// Copyright (c) 2015-2020 CNRS INRIA
3
4
//

5
6
#ifndef __pinocchio_jacobian_hxx__
#define __pinocchio_jacobian_hxx__
7
8

#include "pinocchio/multibody/visitor.hpp"
9
#include "pinocchio/algorithm/check.hpp"
10

jcarpent's avatar
jcarpent committed
11
12
/// @cond DEV

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
21
22
23
24
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    
    typedef boost::fusion::vector<const Model &,
                                  Data &,
25
26
                                  const ConfigVectorType &,
                                  Matrix6xLike &> ArgsType;
27
28
    
    template<typename JointModel>
29
30
31
32
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data,
33
34
                     const Eigen::MatrixBase<ConfigVectorType> & q,
                     const Eigen::MatrixBase<Matrix6xLike> & J)
35
    {
36
      typedef typename Model::JointIndex JointIndex;
37
      
38
39
40
41
      const JointIndex & i = jmodel.id();
      const JointIndex & parent = model.parents[i];
      
      jmodel.calc(jdata.derived(),q.derived());
42
43
44
45
46
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
      else         data.oMi[i] = data.liMi[i];
      
47
48
      Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
      jmodel.jointCols(J_) = data.oMi[i].act(jdata.S());
49
50
51
52
    }
  
  };
  
53
54
55
56
57
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
  computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<ConfigVectorType> & q)
58
  {
59
    assert(model.check(data) && "data is not consistent with model.");
Justin Carpentier's avatar
Justin Carpentier committed
60
    PINOCCHIO_CHECK_INPUT_ARGUMENT(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
68
    typedef JointJacobiansForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6x> Pass;
    typedef typename Pass::ArgsType ArgsType;
69
    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
70
    {
71
      Pass::run(model.joints[i],data.joints[i],
72
73
                ArgsType(model,data,q.derived(),
                         PINOCCHIO_EIGEN_CONST_CAST(Matrix6x,data.J)));
74
75
76
77
78
    }
  
    return data.J;
  }
  
79
80
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  struct JointJacobiansForwardStep2
81
  : public fusion::JointUnaryVisitorBase< JointJacobiansForwardStep2<Scalar,Options,JointCollectionTpl> >
82
  {
83
84
85
86
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    
    typedef boost::fusion::vector<Data &> ArgsType;
87
88
    
    template<typename JointModel>
89
90
91
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     Data & data)
92
    {
93
94
95
      typedef typename Model::JointIndex JointIndex;
      
      const JointIndex & i = jmodel.id();
96
97
98
99
100
      jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
    }
    
  };
  
101
102
103
104
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
  computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        DataTpl<Scalar,Options,JointCollectionTpl> & data)
105
  {
106
    assert(model.check(data) && "data is not consistent with model.");
107
    
108
109
110
111
112
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
    
    typedef JointJacobiansForwardStep2<Scalar,Options,JointCollectionTpl> Pass;
    for(JointIndex i=1; i< (JointIndex)model.njoints; ++i)
113
    {
114
115
      Pass::run(model.joints[i],data.joints[i],
                typename Pass::ArgsType(data));
116
117
118
119
120
    }
    
    return data.J;
  }
  
121
  namespace details
122
  {
123
124
125
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
    void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
126
                                const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex joint_id,
127
                                const ReferenceFrame rf,
128
                                const SE3Tpl<Scalar,Options> & placement,
129
130
                                const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
                                const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
131
    {
132
133
      assert(model.check(data) && "data is not consistent with model.");
      
Justin Carpentier's avatar
Justin Carpentier committed
134
135
      PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.rows() == 6);
      PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.cols() == model.nv);
136
      
Justin Carpentier's avatar
Justin Carpentier committed
137
138
      PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.rows() == 6);
      PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.cols() == model.nv);
139
      
140
141
142
143
144
145
146
147
      Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);

      typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
      typedef const MotionRef<ConstColXprIn> MotionIn;

      typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
      typedef MotionRef<ColXprOut> MotionOut;
      
148
      const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
149
      switch(rf)
Justin Carpentier's avatar
Justin Carpentier committed
150
      {
151
        case WORLD:
152
153
154
155
156
157
158
159
        {
          for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
          {
            MotionIn v_in(Jin.col(j));
            MotionOut v_out(Jout_.col(j));

            v_out = v_in;
          }
160
          break;
161
        }
162
        case LOCAL_WORLD_ALIGNED:
163
164
165
166
167
168
169
        {
          for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
          {
            MotionIn v_in(Jin.col(j));
            MotionOut v_out(Jout_.col(j));
            
            v_out = v_in;
170
            v_out.linear() -= placement.translation().cross(v_in.angular());
171
          }
172
          break;
173
        }
174
        case LOCAL:
175
176
177
178
179
180
        {
          for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
          {
            MotionIn v_in(Jin.col(j));
            MotionOut v_out(Jout_.col(j));
            
181
            v_out = placement.actInv(v_in);
182
          }
183
          break;
184
        }
185
        default:
Justin Carpentier's avatar
Justin Carpentier committed
186
          PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "must never happened");
187
          break;
188
      }
189
    }
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
  
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
    void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex joint_id,
                                const ReferenceFrame rf,
                                const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
                                const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
    {
      typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
      const typename Data::SE3 & oMjoint = data.oMi[joint_id];
      
      translateJointJacobian(model,data,joint_id,rf,oMjoint,Jin,Jout);
    }
  } // namespace details
205
206
207
208
209
210
211
212
213
214
215

  /* Return the jacobian of the output frame attached to joint <jointId> in the
   world frame or in the local frame depending on the template argument. The
   function computeJacobians should have been called first. */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
  inline void getJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                               const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex jointId,
                               const ReferenceFrame rf,
                               const Eigen::MatrixBase<Matrix6xLike> & J)
  {
216
    assert(model.check(data) && "data is not consistent with model.");
217
218
219
220

    details::translateJointJacobian(model,data,jointId,rf,
                                    data.J,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
  }
221
  
222
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
223
  struct JointJacobianForwardStep
224
  : public fusion::JointUnaryVisitorBase< JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> >
225
  {
226
227
228
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    
229
230
    typedef boost::fusion::vector<const Model &,
                                  Data &,
231
                                  const ConfigVectorType &,
232
                                  Matrix6xLike &
233
234
235
                                  > ArgsType;
    
    template<typename JointModel>
236
237
238
239
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data,
240
                     const Eigen::MatrixBase<ConfigVectorType> & q,
241
                     const Eigen::MatrixBase<Matrix6xLike> & J)
242
    {
243
244
245
      typedef typename Model::JointIndex JointIndex;
      const JointIndex & i = jmodel.id();
      const JointIndex & parent = model.parents[i];
246
      
247
      jmodel.calc(jdata.derived(),q.derived());
248
249
250
251
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      data.iMf[parent] = data.liMi[i]*data.iMf[i];
      
252
      Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
253
      jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S());
254
255
256
257
    }
  
  };
  
258
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
259
260
261
262
263
  inline void computeJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                   DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                   const Eigen::MatrixBase<ConfigVectorType> & q,
                                   const JointIndex jointId,
                                   const Eigen::MatrixBase<Matrix6xLike> & J)
264
  {
265
    assert(model.check(data) && "data is not consistent with model.");
Justin Carpentier's avatar
Justin Carpentier committed
266
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
267
268
269
    
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
270
    
271
    data.iMf[jointId].setIdentity();
272
    typedef JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> Pass;
273
    for(JointIndex i=jointId; i>0; i=model.parents[i])
274
    {
275
      Pass::run(model.joints[i],data.joints[i],
276
                typename Pass::ArgsType(model,data,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J)));
277
278
279
    }
  }
  
280
281
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  struct JointJacobiansTimeVariationForwardStep
282
  : public fusion::JointUnaryVisitorBase< JointJacobiansTimeVariationForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
283
  {
284
285
286
287
288
289
290
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    
    typedef boost::fusion::vector<const Model &,
                                  Data &,
                                  const ConfigVectorType &,
                                  const TangentVectorType &> ArgsType;
291
292
    
    template<typename JointModel>
293
294
295
296
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data,
297
298
                     const Eigen::MatrixBase<ConfigVectorType> & q,
                     const Eigen::MatrixBase<TangentVectorType> & v)
299
    {
300
301
302
303
304
305
      typedef typename Model::JointIndex JointIndex;
      typedef typename Data::SE3 SE3;
      typedef typename Data::Motion Motion;
      
      const JointIndex & i = (JointIndex) jmodel.id();
      const JointIndex & parent = model.parents[i];
306
307
308
309
      
      SE3 & oMi = data.oMi[i];
      Motion & vJ = data.v[i];
      
310
      jmodel.calc(jdata.derived(),q.derived(),v.derived());
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
      
      vJ = jdata.v();
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      if(parent>0)
      {
        oMi = data.oMi[parent]*data.liMi[i];
        vJ += data.liMi[i].actInv(data.v[parent]);
      }
      else
      {
        oMi = data.liMi[i];
      }
      
      jmodel.jointCols(data.J) = oMi.act(jdata.S());
      
      // Spatial velocity of joint i expressed in the global frame o
328
      data.ov[i] = oMi.act(vJ);
329
      
330
      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
331
332
333
      ColsBlock dJcols = jmodel.jointCols(data.dJ);
      ColsBlock Jcols = jmodel.jointCols(data.J);
      
334
      motionSet::motionAction(data.ov[i],Jcols,dJcols);
335
336
337
338
    }
    
  };
  
339
340
341
342
343
344
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
  computeJointJacobiansTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                     DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                     const Eigen::MatrixBase<ConfigVectorType> & q,
                                     const Eigen::MatrixBase<TangentVectorType> & v)
345
  {
346
    assert(model.check(data) && "data is not consistent with model.");
Justin Carpentier's avatar
Justin Carpentier committed
347
348
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The velocity vector is not of right size");
349
350
351
    
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
352
    
353
354
    typedef JointJacobiansTimeVariationForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Pass;
    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
355
    {
356
357
      Pass::run(model.joints[i],data.joints[i],
                typename Pass::ArgsType(model,data,q.derived(),v.derived()));
358
359
360
361
362
    }
    
    return data.dJ;
  }
  
363
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
364
365
366
  inline void getJointJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                            const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                            const JointIndex jointId,
367
                                            const ReferenceFrame rf,
368
                                            const Eigen::MatrixBase<Matrix6xLike> & dJ)
369
  {
370
371
    details::translateJointJacobian(model,data,jointId,rf,
                                    data.dJ,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
372
373
  }
  
374
} // namespace pinocchio
375

jcarpent's avatar
jcarpent committed
376
377
/// @endcond

378
#endif // ifndef __pinocchio_jacobian_hxx__