kinematics-derivatives.hxx 12.4 KB
Newer Older
1
//
jcarpent's avatar
jcarpent committed
2
// Copyright (c) 2017-2018 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_kinematics_derivatives_hxx__
#define __se3_kinematics_derivatives_hxx__

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

namespace se3
{
  
  struct ForwardKinematicsDerivativesForwardStep : public fusion::JointVisitor<ForwardKinematicsDerivativesForwardStep>
  {
    typedef boost::fusion::vector<const se3::Model &,
    se3::Data &,
    const Eigen::VectorXd &,
    const Eigen::VectorXd &,
    const Eigen::VectorXd &
    > ArgsType;
    
    JOINT_VISITOR_INIT(ForwardKinematicsDerivativesForwardStep);
    
    template<typename JointModel>
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
                     se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const se3::Model & model,
                     se3::Data & data,
                     const Eigen::VectorXd & q,
                     const Eigen::VectorXd & v,
                     const Eigen::VectorXd & a)
    {
      const Model::JointIndex & i = jmodel.id();
      const Model::JointIndex & parent = model.parents[i];
      SE3 & oMi = data.oMi[i];
      Motion & vi = data.v[i];
      Motion & ai = data.a[i];
      Motion & ov = data.ov[i];
      Motion & oa = data.oa[i];
      
      jmodel.calc(jdata.derived(),q,v);
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      vi = jdata.v();

      if(parent>0)
      {
        oMi = data.oMi[parent]*data.liMi[i];
        vi += data.liMi[i].actInv(data.v[parent]);
      }
      else
        oMi = data.liMi[i];
      
      ai = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (vi ^ jdata.v());
      if(parent>0)
        ai += data.liMi[i].actInv(data.a[parent]);

      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
      ColsBlock dJcols = jmodel.jointCols(data.dJ);
      ColsBlock Jcols = jmodel.jointCols(data.J);
      
      Jcols = oMi.act(jdata.S());
      ov = oMi.act(vi); // Spatial velocity of joint i expressed in the global frame o
78
      motionSet::motionAction(ov,Jcols,dJcols);
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
      oa = oMi.act(ai); // Spatial acceleration of joint i expressed in the global frame o
    }
    
  };
  
  inline void
  computeForwardKinematicsDerivatives(const Model & model, Data & data,
                                      const Eigen::VectorXd & q,
                                      const Eigen::VectorXd & v,
                                      const Eigen::VectorXd & a)
  {
    assert(q.size() == model.nq && "The configuration vector is not of right size");
    assert(v.size() == model.nv && "The velocity vector is not of right size");
    assert(a.size() == model.nv && "The acceleration vector is not of right size");
    assert(model.check(data) && "data is not consistent with model.");
    
    data.v[0].setZero();
    data.a[0].setZero();
    
    for(Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i)
    {
      ForwardKinematicsDerivativesForwardStep::run(model.joints[i],data.joints[i],
                                                   ForwardKinematicsDerivativesForwardStep::ArgsType(model,data,q,v,a));
    }
  }
  
  template<ReferenceFrame rf>
  struct JointVelocityDerivativesBackwardStep : public fusion::JointModelVisitor< JointVelocityDerivativesBackwardStep<rf> >
  {
    typedef boost::fusion::vector<const se3::Model &,
    se3::Data &,
    const SE3 &,
    const Motion &,
    Data::Matrix6x &,
    Data::Matrix6x &
    > ArgsType;
    
    JOINT_MODEL_VISITOR_INIT(JointVelocityDerivativesBackwardStep);
    
    template<typename JointModel>
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
                     const se3::Model & model,
                     se3::Data & data,
                     const SE3 & oMlast,
                     const Motion & vlast,
                     Data::Matrix6x & partial_dq,
                     Data::Matrix6x & partial_dv)
    {
      const Model::JointIndex & i = jmodel.id();
      const Model::JointIndex & parent = model.parents[i];
      const SE3 & oMi = data.oMi[i];
      const Motion & vi = data.v[i];
      const Motion & ov = data.ov[i];
      Motion & vtmp = data.ov[0]; // Temporary variable

      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
      ColsBlock Jcols = jmodel.jointCols(data.J);
      
      // dvec/dv
      ColsBlock partial_dv_cols = jmodel.jointCols(partial_dv);
      if(rf == WORLD)
        partial_dv_cols = Jcols;
      else
        motionSet::se3ActionInverse(oMlast,Jcols,partial_dv_cols);

      // dvec/dq
      ColsBlock partial_dq_cols = jmodel.jointCols(partial_dq);
      if(rf == WORLD)
      {
        if(parent > 0)
          vtmp = data.ov[parent] - vlast;
        else
          vtmp = -vlast;
152
        motionSet::motionAction(vtmp,Jcols,partial_dq_cols);
153
154
155
156
157
158
      }
      else
      {
        if(parent > 0)
        {
          vtmp = oMlast.actInv(data.ov[parent]);
159
          motionSet::motionAction(vtmp,partial_dv_cols,partial_dq_cols);
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
        }
      }
      
      
    }
    
  };
  
  template<ReferenceFrame rf>
  inline void getJointVelocityDerivatives(const Model & model,
                                          Data & data,
                                          const Model::JointIndex jointId,
                                          Data::Matrix6x & partial_dq,
                                          Data::Matrix6x & partial_dv)
  {
    assert( partial_dq.cols() ==  model.nv );
    assert( partial_dv.cols() ==  model.nv );
    assert(model.check(data) && "data is not consistent with model.");
    
    const SE3 & oMlast = data.oMi[jointId];
    const Motion & vlast = data.ov[jointId];
    
    for(Model::JointIndex i = jointId; i > 0; i = model.parents[i])
    {
      JointVelocityDerivativesBackwardStep<rf>::run(model.joints[i],
                                                    typename JointVelocityDerivativesBackwardStep<rf>::ArgsType(model,data,
                                                                                                                oMlast,vlast,
                                                                                                                partial_dq,
                                                                                                                partial_dv));
    }
  
    data.ov[0].setZero();
  }
  
  template<ReferenceFrame rf>
  struct JointAccelerationDerivativesBackwardStep
  : public fusion::JointModelVisitor< JointAccelerationDerivativesBackwardStep<rf> >
  {
    typedef boost::fusion::vector<const se3::Model &,
    se3::Data &,
    const Model::JointIndex,
    Data::Matrix6x &,
    Data::Matrix6x &,
    Data::Matrix6x &,
    Data::Matrix6x &
    > ArgsType;
    
    JOINT_MODEL_VISITOR_INIT(JointAccelerationDerivativesBackwardStep);
    
    template<typename JointModel>
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
                     const se3::Model & model,
                     se3::Data & data,
                     const Model::JointIndex jointId,
                     Data::Matrix6x & v_partial_dq,
                     Data::Matrix6x & a_partial_dq,
                     Data::Matrix6x & a_partial_dv,
                     Data::Matrix6x & a_partial_da)
    {
      const Model::JointIndex & i = jmodel.id();
      const Model::JointIndex & parent = model.parents[i];
      const SE3 & oMi = data.oMi[i];
      const Motion & vi = data.v[i];
      const Motion & ov = data.ov[i];
      Motion & vtmp = data.ov[0]; // Temporary variable
      Motion & atmp = data.oa[0]; // Temporary variable
      
      const SE3 & oMlast = data.oMi[jointId];
      const Motion & vlast = data.ov[jointId];
      const Motion & alast = data.oa[jointId];

      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
      ColsBlock dJcols = jmodel.jointCols(data.dJ);
      ColsBlock Jcols = jmodel.jointCols(data.J);
      
      ColsBlock v_partial_dq_cols = jmodel.jointCols(v_partial_dq);
      ColsBlock a_partial_dq_cols = jmodel.jointCols(a_partial_dq);
      ColsBlock a_partial_dv_cols = jmodel.jointCols(a_partial_dv);
      ColsBlock a_partial_da_cols = jmodel.jointCols(a_partial_da);
      
      // dacc/da
      if(rf == WORLD)
        a_partial_da_cols = Jcols;
      else
        motionSet::se3ActionInverse(oMlast,Jcols,a_partial_da_cols);
      
      // dacc/dv
      if(rf == WORLD)
      {
        if(parent > 0)
          vtmp = data.ov[parent] - vlast;
        else
          vtmp = -vlast;
        
        /// also computes dvec/dq
255
        motionSet::motionAction(vtmp,Jcols,v_partial_dq_cols);
256
257
258
259
260
261
262
263
264
        
        a_partial_dv_cols = v_partial_dq_cols + dJcols;
      }
      else
      {
       /// also computes dvec/dq
        if(parent > 0)
        {
          vtmp = oMlast.actInv(data.ov[parent]);
265
          motionSet::motionAction(vtmp,a_partial_da_cols,v_partial_dq_cols);
266
267
268
269
270
271
272
        }
        
        if(parent > 0)
          vtmp -= data.v[jointId];
        else
          vtmp = -data.v[jointId];
        
273
        motionSet::motionAction(vtmp,a_partial_da_cols,a_partial_dv_cols);
jcarpent's avatar
jcarpent committed
274
        motionSet::se3ActionInverse<ADDTO>(oMlast,dJcols,a_partial_dv_cols);
275
276
277
278
279
280
281
282
283
      }
      
      // dacc/dq
      if(rf == WORLD)
      {
        if(parent > 0)
          atmp = data.oa[parent] - alast;
        else
          atmp = -alast;
284
        motionSet::motionAction(atmp,Jcols,a_partial_dq_cols);
285
286
        
        if(parent >0)
jcarpent's avatar
jcarpent committed
287
          motionSet::motionAction<ADDTO>(vtmp,dJcols,a_partial_dq_cols);
288
289
290
291
292
293
      }
      else
      {
        if(parent > 0)
        {
          atmp = oMlast.actInv(data.oa[parent]);
294
          motionSet::motionAction(atmp,a_partial_da_cols,a_partial_dq_cols);
295
296
        }
        
jcarpent's avatar
jcarpent committed
297
        motionSet::motionAction<ADDTO>(vtmp,v_partial_dq_cols,a_partial_dq_cols);
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
      }

      
    }
    
  };
  
  template<ReferenceFrame rf>
  inline void getJointAccelerationDerivatives(const Model & model,
                                              Data & data,
                                              const Model::JointIndex jointId,
                                              Data::Matrix6x & v_partial_dq,
                                              Data::Matrix6x & a_partial_dq,
                                              Data::Matrix6x & a_partial_dv,
                                              Data::Matrix6x & a_partial_da)
  {
    assert( v_partial_dq.cols() ==  model.nv );
    assert( a_partial_dq.cols() ==  model.nv );
    assert( a_partial_dv.cols() ==  model.nv );
    assert( a_partial_da.cols() ==  model.nv );
    assert(model.check(data) && "data is not consistent with model.");
    
    for(Model::JointIndex i = jointId; i > 0; i = model.parents[i])
    {
      JointAccelerationDerivativesBackwardStep<rf>::run(model.joints[i],
                                                        typename JointAccelerationDerivativesBackwardStep<rf>::ArgsType(model,data,
                                                                                                                        jointId,
                                                                                                                        v_partial_dq,
                                                                                                                        a_partial_dq,
                                                                                                                        a_partial_dv,
                                                                                                                        a_partial_da));
      
    }
    
    // Set Zero to temporary spatial variables
    data.ov[0].setZero();
    data.oa[0].setZero();
  }

} // namespace se3

#endif // ifndef __se3_kinematics_derivatives_hxx__