liegroup-algo.hxx 20.6 KB
Newer Older
1
//
2
// Copyright (c) 2018 CNRS, INRIA
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// 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/>.

18
19
#ifndef __pinocchio_lie_group_algo_hxx__
#define __pinocchio_lie_group_algo_hxx__
20
21

#include "pinocchio/multibody/visitor.hpp"
22
#include "pinocchio/multibody/joint/joint-composite.hpp"
23

24
namespace pinocchio
25
26
27
28
29
30
31
{
  
  namespace details
  {
    template<typename Algo>
    struct Dispatch
    {
32
33
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsType>
      static void run(const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel,
34
                      ArgsType args)
35
36
37
38
39
40
41
42
43
44
45
46
      {
        for (size_t i = 0; i < jmodel.joints.size(); ++i)
          Algo::run(jmodel.joints[i], args);
      }
    };
    
#define SE3_DETAILS_WRITE_ARGS_0(JM)                               const JointModelBase<JM> & jmodel
#define SE3_DETAILS_WRITE_ARGS_1(JM) SE3_DETAILS_WRITE_ARGS_0(JM), typename boost::fusion::result_of::at_c<ArgsType, 0>::type a0
#define SE3_DETAILS_WRITE_ARGS_2(JM) SE3_DETAILS_WRITE_ARGS_1(JM), typename boost::fusion::result_of::at_c<ArgsType, 1>::type a1
#define SE3_DETAILS_WRITE_ARGS_3(JM) SE3_DETAILS_WRITE_ARGS_2(JM), typename boost::fusion::result_of::at_c<ArgsType, 2>::type a2
#define SE3_DETAILS_WRITE_ARGS_4(JM) SE3_DETAILS_WRITE_ARGS_3(JM), typename boost::fusion::result_of::at_c<ArgsType, 3>::type a3
    
47
48
49
50
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_1(Algo)                          \
  template <typename Visitor, typename JointCollection>                       \
  struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > {           \
    typedef typename Visitor::ArgsType ArgsType;                              \
51
    static void run (SE3_DETAILS_WRITE_ARGS_1(JointModelCompositeTpl<JointCollection>))         \
52
    { ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0)); } \
53
  }
54
   
55
56
57
58
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_2(Algo)                 \
  template <typename Visitor, typename JointCollection>                    \
  struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > {        \
    typedef typename Visitor::ArgsType ArgsType;                  \
59
    static void run (SE3_DETAILS_WRITE_ARGS_2(JointModelCompositeTpl<JointCollection>))         \
60
    { ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1)); } \
61
62
  }
    
63
64
65
66
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(Algo)                 \
  template <typename Visitor, typename JointCollection>                    \
  struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > {        \
    typedef typename Visitor::ArgsType ArgsType;                  \
67
    static void run (SE3_DETAILS_WRITE_ARGS_3(JointModelCompositeTpl<JointCollection>))         \
68
    { ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1,a2)); } \
69
70
  }
    
71
72
73
74
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(Algo)                 \
  template <typename Visitor, typename JointCollection>                    \
  struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > {        \
    typedef typename Visitor::ArgsType ArgsType;                  \
75
    static void run (SE3_DETAILS_WRITE_ARGS_4(JointModelCompositeTpl<JointCollection>))         \
76
    { ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1,a2,a3)); } \
77
78
  }
    
79
80
#define SE3_DETAILS_VISITOR_METHOD_ALGO_1(Algo, Visitor)                      \
    typedef LieGroup_t LieGroupMap;                                              \
81
82
    template<typename JointModel>                                              \
    static void algo(SE3_DETAILS_WRITE_ARGS_1(JointModel))                     \
83
84
85
86
    { AlgoDispatch<JointModel>::run(jmodel, a0); }                   \
    template<typename JointModel>                            \
    struct AlgoDispatch : Algo<Visitor, JointModel>                            \
    { using Algo<Visitor, JointModel>::run; };
87
    
88
89
#define SE3_DETAILS_VISITOR_METHOD_ALGO_2(Algo, Visitor)                      \
    typedef LieGroup_t LieGroupMap;                                              \
90
91
    template<typename JointModel>                                              \
    static void algo(SE3_DETAILS_WRITE_ARGS_2(JointModel))                     \
92
93
94
95
    { AlgoDispatch<JointModel>::run(jmodel, a0, a1); }               \
    template<typename JointModel>                            \
    struct AlgoDispatch : Algo<Visitor, JointModel>                            \
    { using Algo<Visitor, JointModel>::run; };
96
    
97
98
#define SE3_DETAILS_VISITOR_METHOD_ALGO_3(Algo, Visitor)                      \
    typedef LieGroup_t LieGroupMap;                                              \
99
100
    template<typename JointModel>                                              \
    static void algo(SE3_DETAILS_WRITE_ARGS_3(JointModel))                     \
101
102
103
104
    { AlgoDispatch<JointModel>::run(jmodel, a0, a1, a2); }           \
    template<typename JointModel>                            \
    struct AlgoDispatch : Algo<Visitor, JointModel>                            \
    { using Algo<Visitor, JointModel>::run; };
105
    
106
107
#define SE3_DETAILS_VISITOR_METHOD_ALGO_4(Algo, Visitor)                      \
    typedef LieGroup_t LieGroupMap;                                              \
108
109
    template<typename JointModel>                                              \
    static void algo(SE3_DETAILS_WRITE_ARGS_4(JointModel))                     \
110
111
112
113
    { AlgoDispatch<JointModel>::run(jmodel, a0, a1, a2, a3); }       \
    template<typename JointModel>                            \
    struct AlgoDispatch : Algo<Visitor, JointModel>                            \
    { using Algo<Visitor, JointModel>::run; };
114
115
  } // namespace details
  
116
  template<typename Visitor, typename JointModel> struct IntegrateStepAlgo;
117
  
118
  template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename ConfigVectorOut>
119
  struct IntegrateStep
120
  : public fusion::JointVisitorBase< IntegrateStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,ConfigVectorOut> >
121
  {
122
123
124
125
    typedef boost::fusion::vector<const ConfigVectorIn &,
                                  const TangentVectorIn &,
                                  ConfigVectorOut &
                                  > ArgsType;
126
    
127
    SE3_DETAILS_VISITOR_METHOD_ALGO_3(IntegrateStepAlgo, IntegrateStep)
128
129
  };
  
130
  template<typename Visitor, typename JointModel>
131
132
133
134
135
136
137
  struct IntegrateStepAlgo
  {
    template<typename ConfigVectorIn, typename TangentVector, typename ConfigVectorOut>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorIn> & q,
                    const Eigen::MatrixBase<TangentVector> & v,
                    const Eigen::MatrixBase<ConfigVectorOut> & result)
138
    {
139
      typedef typename Visitor::LieGroupMap LieGroupMap;
140
      
141
142
143
144
      typename LieGroupMap::template operation<JointModel>::type lgo;
      lgo.integrate(jmodel.jointConfigSelector  (q.derived()),
                    jmodel.jointVelocitySelector(v.derived()),
                    jmodel.jointConfigSelector  (EIGEN_CONST_CAST(ConfigVectorOut,result)));
145
146
147
    }
  };
  
148
  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(IntegrateStepAlgo);
149
  
150
151
152
153
154
155
156
157
158
159
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
  template<typename Visitor, typename JointModel> struct dIntegrateStepAlgo;
  
  template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixType>
  struct dIntegrateStep
  : public fusion::JointVisitorBase< dIntegrateStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> >
  {
    typedef boost::fusion::vector<const ConfigVectorIn &,
                                  const TangentVectorIn &,
                                  JacobianMatrixType &,
                                  const ArgumentPosition &
                                  > ArgsType;
    
    SE3_DETAILS_VISITOR_METHOD_ALGO_4(dIntegrateStepAlgo, dIntegrateStep)
  };
  
  template<typename Visitor, typename JointModel>
  struct dIntegrateStepAlgo
  {
    template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixType>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorIn> & q,
                    const Eigen::MatrixBase<TangentVector> & v,
                    const Eigen::MatrixBase<JacobianMatrixType> & mat,
                    const ArgumentPosition & arg)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;
      
      typename LieGroupMap::template operation<JointModel>::type lgo;
      lgo.dIntegrate(jmodel.jointConfigSelector  (q.derived()),
                     jmodel.jointVelocitySelector(v.derived()),
                     jmodel.jointBlock(EIGEN_CONST_CAST(JacobianMatrixType,mat)),
                     arg);
    }
  };
  
  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateStepAlgo);
  
187
  template<typename Visitor, typename JointModel> struct InterpolateStepAlgo;
188
  
189
  template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar, typename ConfigVectorOut>
190
  struct InterpolateStep
191
  : public fusion::JointVisitorBase< InterpolateStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar,ConfigVectorOut  > >
192
  {
193
194
195
196
197
    typedef boost::fusion::vector<const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &,
                                  const Scalar &,
                                  ConfigVectorOut &
                                  > ArgsType;
198
    
199
    SE3_DETAILS_VISITOR_METHOD_ALGO_4(InterpolateStepAlgo, InterpolateStep)
200
201
  };
  
202
  template<typename Visitor, typename JointModel>
203
204
205
206
207
208
209
210
  struct InterpolateStepAlgo
  {
    template<typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar, typename ConfigVectorOut>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
                    const Scalar & u,
                    const Eigen::MatrixBase<ConfigVectorOut> & result)
211
    {
212
213
214
      typedef typename Visitor::LieGroupMap LieGroupMap;
      
      typename LieGroupMap::template operation<JointModel>::type lgo;
215
216
      lgo.interpolate(jmodel.jointConfigSelector(q0.derived()),
                      jmodel.jointConfigSelector(q1.derived()),
217
                      u,
218
                      jmodel.jointConfigSelector(EIGEN_CONST_CAST(ConfigVectorOut,result)));
219
220
221
    }
  };
  
222
  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(InterpolateStepAlgo);
223
  
224
  template<typename Visitor, typename JointModel> struct DifferenceStepAlgo;
225
  
226
  template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename TangentVectorOut>
227
  struct DifferenceStep
228
  : public fusion::JointVisitorBase< DifferenceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,TangentVectorOut> >
229
  {
230
231
232
233
    typedef boost::fusion::vector<const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &,
                                  TangentVectorOut &
                                  > ArgsType;
234
    
235
    SE3_DETAILS_VISITOR_METHOD_ALGO_3(DifferenceStepAlgo, DifferenceStep)
236
237
  };
  
238
  template<typename Visitor, typename JointModel>
239
240
241
242
243
244
245
  struct DifferenceStepAlgo
  {
    template<typename ConfigVectorIn1, typename ConfigVectorIn2, typename TangentVectorOut>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
                    const Eigen::MatrixBase<TangentVectorOut> & result)
246
    {
247
248
249
      typedef typename Visitor::LieGroupMap LieGroupMap;
      
      typename LieGroupMap::template operation<JointModel>::type lgo;
250
251
252
      lgo.difference(jmodel.jointConfigSelector(q0.derived()),
                     jmodel.jointConfigSelector(q1.derived()),
                     jmodel.jointVelocitySelector(EIGEN_CONST_CAST(TangentVectorOut,result)));
253
254
255
    }
  };
  
256
  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(DifferenceStepAlgo);
257
  
258
  template<typename Visitor, typename JointModel> struct SquaredDistanceStepAlgo;
259
  
260
261
262
  template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename DistanceVectorOut>
  struct SquaredDistanceStep
  : public fusion::JointVisitorBase<SquaredDistanceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,DistanceVectorOut> >
263
  {
jcarpent's avatar
jcarpent committed
264
    typedef boost::fusion::vector<const JointIndex,
265
266
267
268
                                  const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &,
                                  DistanceVectorOut &
                                  > ArgsType;
269
    
270
    SE3_DETAILS_VISITOR_METHOD_ALGO_4(SquaredDistanceStepAlgo, SquaredDistanceStep)
271
272
  };
  
273
  template<typename Visitor, typename JointModel>
274
275
276
277
  struct SquaredDistanceStepAlgo
  {
    template<typename ConfigVectorIn1, typename ConfigVectorIn2, typename DistanceVectorOut>
    static void run(const JointModelBase<JointModel> & jmodel,
jcarpent's avatar
jcarpent committed
278
                    const JointIndex i,
279
280
281
                    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
                    const Eigen::MatrixBase<DistanceVectorOut> & distances)
282
    {
283
284
285
      typedef typename Visitor::LieGroupMap LieGroupMap;
      
      typename LieGroupMap::template operation<JointModel>::type lgo;
286
287
288
      DistanceVectorOut & distances_ = EIGEN_CONST_CAST(DistanceVectorOut,distances);
      distances_[(long)i] += lgo.squaredDistance(jmodel.jointConfigSelector(q0.derived()),
                                                 jmodel.jointConfigSelector(q1.derived()));
289
290
291
    }
  };
  
292
  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(SquaredDistanceStepAlgo);
293
  
294
  template<typename Visitor, typename JointModel> struct RandomConfigurationStepAlgo;
295
  
296
  template<typename LieGroup_t,typename ConfigVectorOut, typename ConfigVectorIn1, typename ConfigVectorIn2>
297
  struct RandomConfigurationStep
298
  : public fusion::JointVisitorBase< RandomConfigurationStep<LieGroup_t,ConfigVectorOut,ConfigVectorIn1,ConfigVectorIn2> >
299
  {
300
301
302
303
    typedef boost::fusion::vector<ConfigVectorOut &,
                                  const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &
                                  > ArgsType;
304
    
305
    SE3_DETAILS_VISITOR_METHOD_ALGO_3(RandomConfigurationStepAlgo, RandomConfigurationStep)
306
307
  };
  
308
  template<typename Visitor, typename JointModel>
309
310
  struct RandomConfigurationStepAlgo
  {
311
    template<typename ConfigVectorOut, typename ConfigVectorIn1, typename ConfigVectorIn2>
312
    static void run(const JointModelBase<JointModel> & jmodel,
313
314
315
                    const Eigen::MatrixBase<ConfigVectorOut> & q,
                    const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
                    const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
316
    {
317
318
319
      typedef typename Visitor::LieGroupMap LieGroupMap;
      
      typename LieGroupMap::template operation<JointModel>::type lgo;
320
321
      lgo.randomConfiguration(jmodel.jointConfigSelector(lowerLimits.derived()),
                              jmodel.jointConfigSelector(upperLimits.derived()),
322
                              jmodel.jointConfigSelector(EIGEN_CONST_CAST(ConfigVectorOut,q)));
323
324
325
    }
  };
  
326
  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(RandomConfigurationStepAlgo);
327
  
328
  template<typename Visitor, typename JointModel> struct NormalizeStepAlgo;
329
  
330
  template<typename LieGroup_t, typename ConfigVectorType>
331
  struct NormalizeStep
332
  : public fusion::JointVisitorBase< NormalizeStep<LieGroup_t,ConfigVectorType> >
333
  {
334
    typedef boost::fusion::vector<ConfigVectorType &> ArgsType;
335
    
336
    SE3_DETAILS_VISITOR_METHOD_ALGO_1(NormalizeStepAlgo, NormalizeStep)
337
338
  };
  
339
  template<typename Visitor, typename JointModel>
340
341
  struct NormalizeStepAlgo
  {
342
343
344
    template<typename ConfigVectorType>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorType> & qout)
345
    {
346
347
348
      typedef typename Visitor::LieGroupMap LieGroupMap;
      
      typename LieGroupMap::template operation<JointModel>::type lgo;
349
      lgo.normalize(jmodel.jointConfigSelector(EIGEN_CONST_CAST(ConfigVectorType,qout)));
350
351
352
    }
  };
  
353
  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NormalizeStepAlgo);
354
  
355
  template<typename Visitor, typename JointModel> struct IsSameConfigurationStepAlgo;
356
  
357
  template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar>
358
  struct IsSameConfigurationStep
359
  : public fusion::JointVisitorBase< IsSameConfigurationStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar> >
360
361
  {
    typedef boost::fusion::vector<bool &,
362
363
364
                                  const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &,
                                  const Scalar &> ArgsType;
365
    
366
    SE3_DETAILS_VISITOR_METHOD_ALGO_4(IsSameConfigurationStepAlgo, IsSameConfigurationStep)
367
368
  };
  
369
  template<typename Visitor, typename JointModel>
370
371
372
373
  struct IsSameConfigurationStepAlgo
  {
    template<typename ConfigVectorIn1, typename ConfigVectorIn2>
    static void run(const JointModelBase<JointModel> & jmodel,
374
                    bool & isSame,
375
376
377
                    const Eigen::MatrixBase<ConfigVectorIn1> & q1,
                    const Eigen::MatrixBase<ConfigVectorIn2> & q2,
                    const typename ConfigVectorIn1::Scalar & prec)
378
    {
379
380
381
      typedef typename Visitor::LieGroupMap LieGroupMap;
      
      typename LieGroupMap::template operation<JointModel>::type lgo;
382
383
      isSame &= lgo.isSameConfiguration(jmodel.jointConfigSelector(q1.derived()),
                                        jmodel.jointConfigSelector(q2.derived()),
384
385
386
387
                                        prec);
    }
  };
  
388
  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(IsSameConfigurationStepAlgo);
389
  
390
  template<typename Visitor, typename JointModel> struct NeutralStepAlgo;
391
  
392
  template<typename LieGroup_t, typename ConfigVectorType>
393
  struct NeutralStep
394
  : public fusion::JointVisitorBase< NeutralStep<LieGroup_t,ConfigVectorType> >
395
  {
396
    typedef boost::fusion::vector<ConfigVectorType &> ArgsType;
397
    
398
    SE3_DETAILS_VISITOR_METHOD_ALGO_1(NeutralStepAlgo, NeutralStep)
399
400
  };
  
401
  template<typename Visitor, typename JointModel>
402
403
  struct NeutralStepAlgo
  {
404
405
406
    template<typename ConfigVectorType>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorType> & neutral_elt)
407
    {
408
409
410
      typedef typename Visitor::LieGroupMap LieGroupMap;
      
      typename LieGroupMap::template operation<JointModel>::type lgo;
411
      jmodel.jointConfigSelector(EIGEN_CONST_CAST(ConfigVectorType,neutral_elt)) = lgo.neutral();
412
413
414
    }
  };
  
415
  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NeutralStepAlgo);
416
  
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
  template<typename Visitor, typename JointModel> struct IntegrateCoeffWiseJacobianStepAlgo;
  
  template<typename LieGroup_t, typename ConfigVectorType, typename JacobianMatrix>
  struct IntegrateCoeffWiseJacobianStep
  : public fusion::JointVisitorBase< IntegrateCoeffWiseJacobianStep<LieGroup_t,ConfigVectorType,JacobianMatrix> >
  {
    typedef boost::fusion::vector<const ConfigVectorType &, JacobianMatrix &> ArgsType;
    
    SE3_DETAILS_VISITOR_METHOD_ALGO_2(IntegrateCoeffWiseJacobianStepAlgo,
                                      IntegrateCoeffWiseJacobianStep)
  };
  
  template<typename Visitor, typename JointModel>
  struct IntegrateCoeffWiseJacobianStepAlgo
  {
    template<typename ConfigVectorType, typename JacobianMatrix>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorType> & q,
                    const Eigen::MatrixBase<JacobianMatrix> & jacobian)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;
      
      typedef typename LieGroupMap::template operation<JointModel>::type LieGroup;
      LieGroup lgo;
      lgo.integrateCoeffWiseJacobian(jmodel.jointConfigSelector(q.derived()),
                                     EIGEN_CONST_CAST(JacobianMatrix,jacobian).template block<LieGroup::NQ,LieGroup::NV>(jmodel.idx_q(),jmodel.idx_v(),jmodel.nq(),jmodel.nv()));
    }
  };
  
  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_2(IntegrateCoeffWiseJacobianStepAlgo);
  
448
449
}

450
#endif // ifndef __pinocchio_lie_group_algo_hxx__