GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/liegroup/liegroup-variant-visitors.hxx Lines: 89 140 63.6 %
Date: 2024-01-23 21:41:47 Branches: 55 153 35.9 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2018 CNRS
3
//
4
5
#ifndef __pinocchio_lie_group_variant_visitor_hxx__
6
#define __pinocchio_lie_group_variant_visitor_hxx__
7
8
#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
9
#include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp"
10
#include "pinocchio/multibody/visitor.hpp"
11
12
#include <string>
13
14
#define LIE_GROUP_VISITOR(VISITOR) \
15
VISITOR(ArgsType & args) : args(args) {} \
16
ArgsType & args
17
18
namespace pinocchio
19
{
20
21
  namespace visitor
22
  {
23
    namespace bf = boost::fusion;
24
25
    template<typename Visitor>
26
    struct LieGroupVisitorBase : public boost::static_visitor<>
27
    {
28
      template<typename LieGroupDerived>
29
352
      void operator() (const LieGroupBase<LieGroupDerived> & lg) const
30
      {
31
352
        bf::invoke(&Visitor::template algo<LieGroupDerived>,
32
352
                   bf::append(boost::ref(lg),
33
352
                              static_cast<const Visitor*>(this)->args));
34
352
      }
35
36
      template<typename LieGroupCollection, typename ArgsTmp>
37
352
      static void run(const LieGroupGenericTpl<LieGroupCollection> & lg,
38
                      ArgsTmp args)
39
      {
40
352
        return boost::apply_visitor(Visitor(args),lg);
41
      }
42
    };
43
44
    template<typename Scalar, int Options>
45
    struct LieGroupEqual : public boost::static_visitor<bool>
46
    {
47
      template <typename T, typename U>
48
      bool operator()( const T &, const U & ) const
49
      {
50
        // Should we handle
51
        // - dynamic size vector space versus static size vector space
52
        // - dynamic versus static cartesian product
53
        return false;
54
      }
55
56
      template <typename T>
57
18
      bool operator()( const T & lhs, const T & rhs ) const
58
      {
59
18
        return lhs == rhs;
60
      }
61
62
      /// \name Vector space comparison
63
      /// \{
64
1
      bool operator() (const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & lhs,
65
                       const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & rhs ) const
66
      {
67
1
        return lhs == rhs;
68
      }
69
70
      template <int Dim>
71
4
      bool operator() (const VectorSpaceOperationTpl<Dim, Scalar, Options> & lhs,
72
                       const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & rhs ) const
73
      {
74
4
        return lhs.nq() == rhs.nq();
75
      }
76
77
      template <int Dim>
78
      bool operator() (const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & lhs,
79
                       const VectorSpaceOperationTpl<Dim, Scalar, Options> & rhs ) const
80
      {
81
        return lhs.nq() == rhs.nq();
82
      }
83
      /// \}
84
85
      template <typename LieGroup1, typename LieGroup2,
86
                template<typename,int> class LieGroupCollectionTpl>
87
      bool operator() (const CartesianProductOperation<LieGroup1, LieGroup2> & lhs,
88
                       const CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl> & rhs ) const
89
      {
90
        return rhs.isEqual(lhs);
91
      }
92
93
      template <typename LieGroup1, typename LieGroup2,
94
                template<typename,int> class LieGroupCollectionTpl>
95
      bool operator() (const CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl> & lhs,
96
                       const CartesianProductOperation<LieGroup1, LieGroup2> & rhs) const
97
      {
98
        return lhs.isEqual(rhs);
99
      }
100
    };
101
  }
102
103
#define PINOCCHIO_LG_CHECK_VECTOR_SIZE(type,var,exp_size)                      \
104
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(type);                                     \
105
    assert(var.size() == exp_size)
106
#define PINOCCHIO_LG_CHECK_MATRIX_SIZE(var,nr,nc)                              \
107
    assert(var.rows() == nr); assert(var.cols() == nc)
108
109
#define PINOCCHIO_LG_VISITOR(Name,type,_method)                                \
110
  /** @brief Lie Group visitor of the _method */                               \
111
  struct LieGroup ## Name ## Visitor: public boost::static_visitor<type>       \
112
  {                                                                            \
113
    template<typename LieGroupDerived>                                         \
114
    type operator()(const LieGroupBase<LieGroupDerived> & lg) const            \
115
    { return lg._method(); }                                                   \
116
                                                                               \
117
    template<typename LieGroupCollection>                                      \
118
    static type run(const LieGroupGenericTpl<LieGroupCollection> & lg)         \
119
    { return boost::apply_visitor( LieGroup ## Name ## Visitor(), lg ); }      \
120
  };                                                                           \
121
                                                                               \
122
  template<typename LieGroupCollection>                                        \
123
  inline type _method(const LieGroupGenericTpl<LieGroupCollection> & lg)       \
124
  { return LieGroup ## Name ## Visitor::run(lg); }
125
126
2004
  PINOCCHIO_LG_VISITOR(Nq,int,nq)
127
492
  PINOCCHIO_LG_VISITOR(Nv,int,nv)
128
84
  PINOCCHIO_LG_VISITOR(Name,std::string,name)
129
#undef PINOCCHIO_LG_VISITOR
130
131
  /**
132
   * @brief Visitor of the Lie Group neutral element
133
   */
134
  template<typename Vector>
135
  struct LieGroupNeutralVisitor: public boost::static_visitor<Vector>
136
  {
137
    template<typename LieGroupDerived>
138
56
    Vector operator()(const LieGroupBase<LieGroupDerived> & lg) const
139
56
    { return lg.neutral(); }
140
141
    template<typename LieGroupCollection>
142
28
    static Vector run(const LieGroupGenericTpl<LieGroupCollection> & lg)
143
28
    { return boost::apply_visitor( LieGroupNeutralVisitor(), lg ); }
144
  };
145
146
  template<typename LieGroupCollection>
147
  inline Eigen::Matrix<typename LieGroupCollection::Scalar,Eigen::Dynamic,1,LieGroupCollection::Options>
148
28
  neutral(const LieGroupGenericTpl<LieGroupCollection> & lg)
149
  {
150
    typedef Eigen::Matrix<typename LieGroupCollection::Scalar,Eigen::Dynamic,1,LieGroupCollection::Options> ReturnType;
151
28
    return LieGroupNeutralVisitor<ReturnType>::run(lg);
152
  }
153
154
#define PINOCCHIO_LG_VISITOR(Name,_method)                                     \
155
  /** @brief Visitor of the Lie Group _method method */                        \
156
  template <class M_t>                                                         \
157
  struct LieGroup ## Name ## Visitor                                           \
158
  : visitor::LieGroupVisitorBase< LieGroup ## Name ## Visitor<M_t> >           \
159
  {                                                                            \
160
    typedef boost::fusion::vector<const Eigen::MatrixBase<M_t> &> ArgsType;    \
161
    LIE_GROUP_VISITOR(LieGroup ## Name ## Visitor);                            \
162
    template<typename LieGroupDerived>                                         \
163
    static void algo(const LieGroupBase<LieGroupDerived> & lg,                 \
164
                     const Eigen::MatrixBase<M_t>& m1)                         \
165
    { lg._method(m1); }                                                        \
166
  }
167
168
104
  PINOCCHIO_LG_VISITOR(Random, random);
169
36
  PINOCCHIO_LG_VISITOR(Normalize, normalize);
170
171
  template<typename LieGroupCollection, class Config_t>
172
36
  inline void random(const LieGroupGenericTpl<LieGroupCollection> & lg,
173
                     const Eigen::MatrixBase<Config_t> & qout)
174
  {
175
36
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, qout, nq(lg));
176
177
    typedef LieGroupRandomVisitor<Config_t> Operation;
178
36
    Operation::run(lg,typename Operation::ArgsType(qout));
179
36
  }
180
181
  template<typename LieGroupCollection, class Config_t>
182
28
  inline void normalize(const LieGroupGenericTpl<LieGroupCollection> & lg,
183
                        const Eigen::MatrixBase<Config_t> & qout)
184
  {
185
28
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, qout, nq(lg));
186
187
    typedef LieGroupNormalizeVisitor<Config_t> Operation;
188
28
    Operation::run(lg,typename Operation::ArgsType(qout));
189
28
  }
190
191
#undef PINOCCHIO_LG_VISITOR
192
193
  /** @brief Visitor of the Lie Group isNormalized method */
194
  template <class Matrix_t>
195
  struct LieGroupIsNormalizedVisitor
196
  : visitor::LieGroupVisitorBase< LieGroupIsNormalizedVisitor<Matrix_t> >
197
  {
198
    typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix_t> &,
199
                                  const typename Matrix_t::Scalar&,
200
                                  bool&> ArgsType;
201
8
    LIE_GROUP_VISITOR(LieGroupIsNormalizedVisitor);
202
    template<typename LieGroupDerived>
203
16
    static void algo(const LieGroupBase<LieGroupDerived> & lg,
204
                     const Eigen::MatrixBase<Matrix_t>& qin,
205
                     const typename Matrix_t::Scalar& prec,
206
                     bool& res)
207
    {
208
16
      res = lg.isNormalized(qin, prec);
209
16
    }
210
  };
211
212
  template<typename LieGroupCollection, class Config_t>
213
8
  inline bool isNormalized(const LieGroupGenericTpl<LieGroupCollection> & lg,
214
                           const Eigen::MatrixBase<Config_t> & qin,
215
                           const typename Config_t::Scalar& prec)
216
  {
217

8
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, qin, nq(lg));
218
219
    bool res;
220
    typedef LieGroupIsNormalizedVisitor<Config_t> Operation;
221

8
    Operation::run(lg,typename Operation::ArgsType(qin,prec,res));
222
8
    return res;
223
  }
224
225
  /** @brief Visitor of the Lie Group isSameConfiguration method */
226
  template <class Matrix1_t, class Matrix2_t>
227
  struct LieGroupIsSameConfigurationVisitor
228
  : visitor::LieGroupVisitorBase< LieGroupIsSameConfigurationVisitor<Matrix1_t, Matrix2_t> >
229
  {
230
    typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix1_t> &,
231
                                  const Eigen::MatrixBase<Matrix2_t> &,
232
                                  const typename Matrix1_t::Scalar &,
233
                                  bool&> ArgsType;
234
20
    LIE_GROUP_VISITOR(LieGroupIsSameConfigurationVisitor);
235
    template<typename LieGroupDerived>
236
20
    static void algo(const LieGroupBase<LieGroupDerived> & lg,
237
                     const Eigen::MatrixBase<Matrix1_t>& q0,
238
                     const Eigen::MatrixBase<Matrix2_t>& q1,
239
                     const typename Matrix1_t::Scalar & prec,
240
                     bool& res)
241
    {
242
20
      res = lg.isSameConfiguration(q0, q1, prec);
243
    }
244
  };
245
246
  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
247
20
  inline bool isSameConfiguration(const LieGroupGenericTpl<LieGroupCollection> & lg,
248
                          const Eigen::MatrixBase<ConfigL_t> & q0,
249
                          const Eigen::MatrixBase<ConfigR_t> & q1,
250
                          const typename ConfigL_t::Scalar& prec)
251
  {
252

20
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
253

20
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
254
255
    bool res;
256
    typedef LieGroupIsSameConfigurationVisitor<ConfigL_t,ConfigR_t> Operation;
257

20
    Operation::run(lg,typename Operation::ArgsType(q0,q1,prec,res));
258
20
    return res;
259
  }
260
261
#define PINOCCHIO_LG_VISITOR(Name,_method)                                             \
262
  /** @brief Visitor of the Lie Group _method method */                                \
263
  template <class Matrix1_t, class Matrix2_t>                                          \
264
  struct LieGroup ## Name ## Visitor                                                   \
265
  : visitor::LieGroupVisitorBase< LieGroup ## Name ## Visitor<Matrix1_t, Matrix2_t> >  \
266
  {                                                                                    \
267
    typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix1_t> &,                \
268
                                  const Eigen::MatrixBase<Matrix2_t> &,                \
269
                                  typename Matrix1_t::Scalar &> ArgsType;              \
270
    LIE_GROUP_VISITOR(LieGroup ## Name ## Visitor);                                    \
271
    template<typename LieGroupDerived>                                                 \
272
    static void algo(const LieGroupBase<LieGroupDerived> & lg,                         \
273
                     const Eigen::MatrixBase<Matrix1_t>& m1,                           \
274
                     const Eigen::MatrixBase<Matrix2_t>& m2,                           \
275
                     typename Matrix1_t::Scalar& res)                                  \
276
    {                                                                                  \
277
      res = lg._method(m1, m2);                                                        \
278
    }                                                                                  \
279
  }
280
281
  //PINOCCHIO_LG_VISITOR(Distance, distance);
282
112
  PINOCCHIO_LG_VISITOR(SquaredDistance, squaredDistance);
283
284
  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
285
  inline typename ConfigL_t::Scalar
286
48
  squaredDistance(const LieGroupGenericTpl<LieGroupCollection> & lg,
287
                  const Eigen::MatrixBase<ConfigL_t> & q0,
288
                  const Eigen::MatrixBase<ConfigR_t> & q1)
289
  {
290

48
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
291

48
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
292
293
    typedef LieGroupSquaredDistanceVisitor<ConfigL_t,ConfigR_t> Operation;
294
    typename ConfigL_t::Scalar d2;
295

48
    Operation::run(lg,typename Operation::ArgsType(q0,q1,d2));
296
48
    return d2;
297
  }
298
299
#undef PINOCCHIO_LG_VISITOR
300
301
#define PINOCCHIO_LG_VISITOR(Name,_method)                                                        \
302
  /** @brief Visitor of the Lie Group _method method */                                           \
303
  template <class Matrix1_t, class Matrix2_t, class Matrix3_t>                                    \
304
  struct LieGroup ## Name ## Visitor                                                              \
305
  : visitor::LieGroupVisitorBase< LieGroup ## Name ## Visitor<Matrix1_t, Matrix2_t, Matrix3_t> >  \
306
  {                                                                                               \
307
    typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix1_t> &,                           \
308
                                  const Eigen::MatrixBase<Matrix2_t> &,                           \
309
                                  const Eigen::MatrixBase<Matrix3_t> &> ArgsType;                 \
310
    LIE_GROUP_VISITOR(LieGroup ## Name ## Visitor);                                               \
311
    template<typename LieGroupDerived>                                                            \
312
    static void algo(const LieGroupBase<LieGroupDerived> & lg,                                    \
313
                     const Eigen::MatrixBase<Matrix1_t>& m1,                                      \
314
                     const Eigen::MatrixBase<Matrix2_t>& m2,                                      \
315
                     const Eigen::MatrixBase<Matrix3_t>& m3)                                      \
316
    {                                                                                             \
317
      lg._method(m1, m2, m3);                                                                     \
318
    }                                                                                             \
319
  }
320
321
72
  PINOCCHIO_LG_VISITOR(Integrate, integrate);
322
36
  PINOCCHIO_LG_VISITOR(Difference, difference);
323
20
  PINOCCHIO_LG_VISITOR(RandomConfiguration, randomConfiguration);
324
325
  template<typename LieGroupCollection, class ConfigIn_t, class Tangent_t, class ConfigOut_t>
326
28
  inline void integrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
327
                        const Eigen::MatrixBase<ConfigIn_t> & q,
328
                        const Eigen::MatrixBase<Tangent_t>  & v,
329
                        const Eigen::MatrixBase<ConfigOut_t>& qout)
330
  {
331
28
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigIn_t, q, nq(lg));
332
28
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
333
28
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigOut_t, qout, nq(lg));
334
335
    typedef LieGroupIntegrateVisitor<ConfigIn_t,Tangent_t,ConfigOut_t> Operation;
336
28
    Operation::run(lg,typename Operation::ArgsType(q,v,qout));
337
28
  }
338
339
  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class Tangent_t>
340
28
  inline void difference(const LieGroupGenericTpl<LieGroupCollection> & lg,
341
                         const Eigen::MatrixBase<ConfigL_t> & q0,
342
                         const Eigen::MatrixBase<ConfigR_t> & q1,
343
                         const Eigen::MatrixBase<Tangent_t> & v)
344
  {
345
28
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
346
28
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
347
28
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
348
349
    typedef LieGroupDifferenceVisitor<ConfigL_t,ConfigR_t,Tangent_t> Operation;
350
28
    Operation::run(lg,typename Operation::ArgsType(q0,q1,v));
351
28
  }
352
353
  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t>
354
20
  inline void randomConfiguration(const LieGroupGenericTpl<LieGroupCollection> & lg,
355
                                  const Eigen::MatrixBase<ConfigL_t> & q0,
356
                                  const Eigen::MatrixBase<ConfigR_t> & q1,
357
                                  const Eigen::MatrixBase<ConfigOut_t> & qout)
358
  {
359
20
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
360
20
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
361
20
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigOut_t, qout, nq(lg));
362
363
    typedef LieGroupRandomConfigurationVisitor<ConfigL_t,ConfigR_t,ConfigOut_t> Operation;
364
20
    Operation::run(lg,typename Operation::ArgsType(q0,q1,qout));
365
20
  }
366
367
#undef PINOCCHIO_LG_VISITOR
368
369
  /** @brief Visitor of the Lie Group interpolate method */
370
  template <class Matrix1_t, class Matrix2_t, class Matrix3_t>
371
  struct LieGroupInterpolateVisitor
372
  : visitor::LieGroupVisitorBase< LieGroupInterpolateVisitor<Matrix1_t, Matrix2_t, Matrix3_t> >
373
  {
374
    typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix1_t> &,
375
                                  const Eigen::MatrixBase<Matrix2_t> &,
376
                                  const typename Matrix1_t::Scalar &,
377
                                  const Eigen::MatrixBase<Matrix3_t> &> ArgsType;
378
    LIE_GROUP_VISITOR(LieGroupInterpolateVisitor);
379
    template<typename LieGroupDerived>
380
    static void algo(const LieGroupBase<LieGroupDerived> & lg,
381
                     const Eigen::MatrixBase<Matrix1_t>& q0,
382
                     const Eigen::MatrixBase<Matrix2_t>& q1,
383
                     const typename Matrix1_t::Scalar & u,
384
                     const Eigen::MatrixBase<Matrix3_t>& qout)
385
    {
386
      lg.interpolate(q0, q1, u, qout);
387
    }
388
  };
389
390
  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t>
391
  inline void interpolate(const LieGroupGenericTpl<LieGroupCollection> & lg,
392
                          const Eigen::MatrixBase<ConfigL_t> & q0,
393
                          const Eigen::MatrixBase<ConfigR_t> & q1,
394
                          const typename ConfigL_t::Scalar& u,
395
                          const Eigen::MatrixBase<ConfigOut_t> & qout)
396
  {
397
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
398
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
399
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigOut_t, qout, nq(lg));
400
401
    typedef LieGroupInterpolateVisitor<ConfigL_t,ConfigR_t,ConfigOut_t> Operation;
402
    Operation::run(lg,typename Operation::ArgsType(q0,q1,u,qout));
403
  }
404
405
#define PINOCCHIO_LG_VISITOR(Name,_method,HasArgPos)                                              \
406
  /** @brief Visitor of the Lie Group _method method */                                           \
407
  template <class Matrix1_t, class Matrix2_t, class Matrix3_t>                                    \
408
  struct LieGroup ## Name ## Visitor                                                              \
409
  : visitor::LieGroupVisitorBase< LieGroup ## Name ## Visitor<Matrix1_t, Matrix2_t, Matrix3_t> >  \
410
  {                                                                                               \
411
    typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix1_t> &,                           \
412
                                  const Eigen::MatrixBase<Matrix2_t> &,                           \
413
                                  const Eigen::MatrixBase<Matrix3_t> &,                           \
414
                                  const ArgumentPosition BOOST_PP_COMMA_IF(HasArgPos)             \
415
                                  BOOST_PP_IIF(HasArgPos, const AssignmentOperatorType,)> ArgsType;                         \
416
    LIE_GROUP_VISITOR(LieGroup ## Name ## Visitor);                                               \
417
    template<typename LieGroupDerived>                                                            \
418
    static void algo(const LieGroupBase<LieGroupDerived> & lg,                                    \
419
                     const Eigen::MatrixBase<Matrix1_t>& m1,                                      \
420
                     const Eigen::MatrixBase<Matrix2_t>& m2,                                      \
421
                     const Eigen::MatrixBase<Matrix3_t>& m3,                                      \
422
                     const ArgumentPosition arg BOOST_PP_COMMA_IF(HasArgPos)                      \
423
                     BOOST_PP_IF(HasArgPos, const AssignmentOperatorType op = SETTO,))            \
424
    {                                                                                             \
425
      lg._method(m1, m2, m3, arg BOOST_PP_COMMA_IF(HasArgPos) BOOST_PP_IF(HasArgPos, op,));       \
426
    }                                                                                             \
427
  }
428
429
80
  PINOCCHIO_LG_VISITOR(DIntegrate, dIntegrate, 1);
430
40
  PINOCCHIO_LG_VISITOR(DDifference, dDifference, 0);
431
432
  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t>
433
40
  void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
434
                  const Eigen::MatrixBase<Config_t >  & q,
435
                  const Eigen::MatrixBase<Tangent_t>  & v,
436
                  const Eigen::MatrixBase<JacobianOut_t> & J,
437
                  const ArgumentPosition arg,
438
                  const AssignmentOperatorType op)
439
  {
440
40
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
441
40
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
442

40
    PINOCCHIO_LG_CHECK_MATRIX_SIZE(J, nv(lg), nv(lg));
443
444
    typedef LieGroupDIntegrateVisitor<Config_t,Tangent_t,JacobianOut_t> Operation;
445
40
    Operation::run(lg,typename Operation::ArgsType(q,v,J,arg,op));
446
40
  }
447
448
  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
449
40
  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
450
                   const Eigen::MatrixBase<ConfigL_t> & q0,
451
                   const Eigen::MatrixBase<ConfigR_t> & q1,
452
                   const Eigen::MatrixBase<JacobianOut_t> & J,
453
                   const ArgumentPosition arg)
454
  {
455
40
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
456
40
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
457

40
    PINOCCHIO_LG_CHECK_MATRIX_SIZE(J, nv(lg), nv(lg));
458
459
    typedef LieGroupDDifferenceVisitor<ConfigL_t,ConfigR_t,JacobianOut_t> Operation;
460
40
    Operation::run(lg,typename Operation::ArgsType(q0,q1,J,arg));
461
40
  }
462
463
#undef PINOCCHIO_LG_VISITOR
464
465
  template <class M1_t, class M2_t, class M3_t, class M4_t, bool dIntegrateOnTheLeft>
466
  struct LieGroupDIntegrateProductVisitor
467
  : visitor::LieGroupVisitorBase< LieGroupDIntegrateProductVisitor<M1_t, M2_t, M3_t, M4_t, dIntegrateOnTheLeft> >
468
  {
469
    typedef boost::fusion::vector<const M1_t &,
470
                                  const M2_t &,
471
                                  const M3_t &,
472
                                  M4_t &,
473
                                  const ArgumentPosition,
474
                                  const AssignmentOperatorType> ArgsType;
475
476
    LIE_GROUP_VISITOR(LieGroupDIntegrateProductVisitor);
477
478
    template<typename LieGroupDerived>
479
    static void algo(const LieGroupBase<LieGroupDerived> & lg,
480
                     const M1_t& q,
481
                     const M2_t& v,
482
                     const M3_t& J_in,
483
                     M4_t& J_out,
484
                     const ArgumentPosition arg,
485
                     const AssignmentOperatorType op)
486
    {
487
      PINOCCHIO_CHECK_INPUT_ARGUMENT(arg==ARG0||arg==ARG1, "arg should be either ARG0 or ARG1");
488
      switch (arg) {
489
        case ARG0:
490
          if(dIntegrateOnTheLeft) lg.dIntegrate_dq(q, v, SELF, J_in, J_out, op);
491
          else                    lg.dIntegrate_dq(q, v, J_in, SELF, J_out, op);
492
          return;
493
        case ARG1:
494
          if(dIntegrateOnTheLeft) lg.dIntegrate_dv(q, v, SELF, J_in, J_out, op);
495
          else                    lg.dIntegrate_dv(q, v, J_in, SELF, J_out, op);
496
          return;
497
        default:
498
          return;
499
      }
500
    }
501
  };
502
503
  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
504
  void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
505
                  const Eigen::MatrixBase<Config_t >  & q,
506
                  const Eigen::MatrixBase<Tangent_t>  & v,
507
                  const Eigen::MatrixBase<JacobianIn_t> & J_in,
508
                  int self,
509
                  const Eigen::MatrixBase<JacobianOut_t> & J_out,
510
                  const ArgumentPosition arg,
511
                  const AssignmentOperatorType op)
512
  {
513
    PINOCCHIO_UNUSED_VARIABLE(self);
514
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
515
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
516
517
    typedef LieGroupDIntegrateProductVisitor<Config_t, Tangent_t, JacobianIn_t, JacobianOut_t, false> Operation;
518
    Operation::run(lg,typename Operation::ArgsType(
519
          q.derived(),v.derived(),J_in.derived(),
520
          PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), arg, op));
521
  }
522
523
  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
524
  void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
525
                  const Eigen::MatrixBase<Config_t >  & q,
526
                  const Eigen::MatrixBase<Tangent_t>  & v,
527
                  int self,
528
                  const Eigen::MatrixBase<JacobianIn_t> & J_in,
529
                  const Eigen::MatrixBase<JacobianOut_t> & J_out,
530
                  const ArgumentPosition arg,
531
                  const AssignmentOperatorType op)
532
  {
533
    PINOCCHIO_UNUSED_VARIABLE(self);
534
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
535
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
536
537
    typedef LieGroupDIntegrateProductVisitor<Config_t, Tangent_t, JacobianIn_t, JacobianOut_t, true> Operation;
538
    Operation::run(lg,typename Operation::ArgsType(
539
          q.derived(),v.derived(),J_in.derived(),
540
          PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), arg, op));
541
  }
542
543
  template <class M1_t, class M2_t, class M3_t, class M4_t, bool dDifferenceOnTheLeft, ArgumentPosition arg>
544
  struct LieGroupDDifferenceProductVisitor
545
  : visitor::LieGroupVisitorBase< LieGroupDDifferenceProductVisitor<M1_t, M2_t, M3_t, M4_t, dDifferenceOnTheLeft, arg> >
546
  {
547
    typedef boost::fusion::vector<const M1_t &,
548
                                  const M2_t &,
549
                                  const M3_t &,
550
                                  M4_t &,
551
                                  const AssignmentOperatorType> ArgsType;
552
553
    LIE_GROUP_VISITOR(LieGroupDDifferenceProductVisitor);
554
555
    template<typename LieGroupDerived>
556
    static void algo(const LieGroupBase<LieGroupDerived> & lg,
557
                     const M1_t& q0,
558
                     const M2_t& q1,
559
                     const M3_t& J_in,
560
                     M4_t& J_out,
561
                     const AssignmentOperatorType op)
562
    {
563
      if(dDifferenceOnTheLeft) lg.template dDifference<arg>(q0, q1, SELF, J_in, J_out, op);
564
      else                     lg.template dDifference<arg>(q0, q1, J_in, SELF, J_out, op);
565
    }
566
  };
567
568
  template<ArgumentPosition arg, typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
569
  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
570
                   const Eigen::MatrixBase<ConfigL_t> & q0,
571
                   const Eigen::MatrixBase<ConfigR_t> & q1,
572
                   const Eigen::MatrixBase<JacobianIn_t> & J_in,
573
                   int self,
574
                   const Eigen::MatrixBase<JacobianOut_t> & J_out,
575
                   const AssignmentOperatorType op)
576
  {
577
    PINOCCHIO_UNUSED_VARIABLE(self);
578
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
579
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
580
581
    typedef LieGroupDDifferenceProductVisitor<ConfigL_t, ConfigR_t, JacobianIn_t, JacobianOut_t, false, arg> Operation;
582
    Operation::run(lg,typename Operation::ArgsType(
583
          q0.derived(),q1.derived(),J_in.derived(),
584
          PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), op));
585
  }
586
587
  template<ArgumentPosition arg, typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
588
  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
589
                   const Eigen::MatrixBase<ConfigL_t> & q0,
590
                   const Eigen::MatrixBase<ConfigR_t> & q1,
591
                   int self,
592
                   const Eigen::MatrixBase<JacobianIn_t> & J_in,
593
                   const Eigen::MatrixBase<JacobianOut_t> & J_out,
594
                   const AssignmentOperatorType op)
595
  {
596
    PINOCCHIO_UNUSED_VARIABLE(self);
597
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
598
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
599
600
    typedef LieGroupDDifferenceProductVisitor<ConfigL_t, ConfigR_t, JacobianIn_t, JacobianOut_t, true, arg> Operation;
601
    Operation::run(lg,typename Operation::ArgsType(
602
          q0.derived(),q1.derived(),J_in.derived(),
603
          PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), op));
604
  }
605
606
  template <class M1_t, class M2_t, class M3_t, class M4_t>
607
  struct LieGroupDIntegrateTransportVisitor
608
  : visitor::LieGroupVisitorBase< LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, M4_t> >
609
  {
610
    typedef boost::fusion::vector<const Eigen::MatrixBase<M1_t> &,
611
                                  const Eigen::MatrixBase<M2_t> &,
612
                                  const Eigen::MatrixBase<M3_t> &,
613
                                  const Eigen::MatrixBase<M4_t> &,
614
                                  const ArgumentPosition> ArgsType;
615
616
    LIE_GROUP_VISITOR(LieGroupDIntegrateTransportVisitor);
617
618
    template<typename LieGroupDerived>
619
    static void algo(const LieGroupBase<LieGroupDerived> & lg,
620
                     const Eigen::MatrixBase<M1_t>& q,
621
                     const Eigen::MatrixBase<M2_t>& v,
622
                     const Eigen::MatrixBase<M3_t>& J_in,
623
                     const Eigen::MatrixBase<M4_t>& J_out,
624
                     const ArgumentPosition arg)
625
    {
626
      lg.dIntegrateTransport(q, v, J_in, J_out, arg);
627
    }
628
  };
629
630
  template <class M1_t, class M2_t, class M3_t>
631
  struct LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, void>
632
  : visitor::LieGroupVisitorBase< LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, void> >
633
  {
634
    typedef boost::fusion::vector<const Eigen::MatrixBase<M1_t> &,
635
                                  const Eigen::MatrixBase<M2_t> &,
636
                                  const Eigen::MatrixBase<M3_t> &,
637
                                  const ArgumentPosition> ArgsType;
638
639
    LIE_GROUP_VISITOR(LieGroupDIntegrateTransportVisitor);
640
    template<typename LieGroupDerived>
641
    static void algo(const LieGroupBase<LieGroupDerived> & lg,
642
                     const Eigen::MatrixBase<M1_t>& q,
643
                     const Eigen::MatrixBase<M2_t>& v,
644
                     const Eigen::MatrixBase<M3_t>& J,
645
                     const ArgumentPosition arg)
646
    {
647
      lg.dIntegrateTransport(q, v, J, arg);
648
    }
649
  };
650
651
  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
652
  void dIntegrateTransport(const LieGroupGenericTpl<LieGroupCollection> & lg,
653
                           const Eigen::MatrixBase<Config_t > & q,
654
                           const Eigen::MatrixBase<Tangent_t> & v,
655
                           const Eigen::MatrixBase<JacobianIn_t> & J_in,
656
                           const Eigen::MatrixBase<JacobianOut_t> & J_out,
657
                           const ArgumentPosition arg)
658
  {
659
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
660
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
661
    assert(J_in .rows() == nv(lg));
662
    assert(J_out.rows() == nv(lg));
663
664
    typedef LieGroupDIntegrateTransportVisitor<Config_t, Tangent_t, JacobianIn_t, JacobianOut_t> Operation;
665
    Operation::run(lg,typename Operation::ArgsType(q,v,J_in,J_out,arg));
666
  }
667
668
  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t>
669
  void dIntegrateTransport(const LieGroupGenericTpl<LieGroupCollection> & lg,
670
                           const Eigen::MatrixBase<Config_t > & q,
671
                           const Eigen::MatrixBase<Tangent_t> & v,
672
                           const Eigen::MatrixBase<JacobianOut_t> & J,
673
                           const ArgumentPosition arg)
674
  {
675
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
676
    PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
677
    assert(J.rows() == nv(lg));
678
679
    typedef LieGroupDIntegrateTransportVisitor<Config_t, Tangent_t, JacobianOut_t, void> Operation;
680
    Operation::run(lg,typename Operation::ArgsType(q,v,J,arg));
681
  }
682
683
}
684
685
#undef PINOCCHIO_LG_CHECK_VECTOR_SIZE
686
687
#endif // ifndef __pinocchio_lie_group_variant_visitor_hxx__
688