GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/liegroup/liegroup-base.hpp Lines: 11 12 91.7 %
Date: 2024-04-26 13:14:21 Branches: 0 0 - %

Line Branch Exec Source
1
//
2
// Copyright (c) 2016-2020 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
6
#define __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
7
8
#include "pinocchio/multibody/liegroup/fwd.hpp"
9
10
#include <limits>
11
12
namespace pinocchio
13
{
14
#ifdef PINOCCHIO_WITH_CXX11_SUPPORT
15
  constexpr int SELF = 0;
16
#else
17
  enum { SELF = 0 };
18
#endif
19
20
#define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,TYPENAME)               \
21
  typedef          LieGroupBase<Derived> Base;                        \
22
  typedef TYPENAME Base::Index Index;                                          \
23
  typedef TYPENAME traits<Derived>::Scalar Scalar;                             \
24
  enum {                                                                       \
25
    Options = traits<Derived>::Options,                                        \
26
    NQ = Base::NQ,                                                             \
27
    NV = Base::NV                                                              \
28
  };                                                                           \
29
  typedef TYPENAME Base::ConfigVector_t ConfigVector_t;                        \
30
  typedef TYPENAME Base::TangentVector_t TangentVector_t;                      \
31
  typedef TYPENAME Base::JacobianMatrix_t JacobianMatrix_t
32
33
#define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE(Derived)                                \
34
PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,PINOCCHIO_MACRO_EMPTY_ARG)
35
36
#define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived)                            \
37
PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
38
39
  template<typename Derived>
40
  struct LieGroupBase
41
  {
42
    typedef Derived LieGroupDerived;
43
    typedef int Index;
44
    typedef typename traits<LieGroupDerived>::Scalar Scalar;
45
    enum
46
    {
47
      Options = traits<LieGroupDerived>::Options,
48
      NQ = traits<LieGroupDerived>::NQ,
49
      NV = traits<LieGroupDerived>::NV
50
    };
51
52
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
53
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
54
    typedef Eigen::Matrix<Scalar,NV,NV,Options> JacobianMatrix_t;
55
56
    /// \name API with return value as argument
57
    /// \{
58
59
    /**
60
     * @brief      Integrate a joint's configuration with a tangent vector during one unit time duration
61
     *
62
     * @param[in]  q     the initial configuration.
63
     * @param[in]  v     the tangent velocity.
64
     *
65
     * @param[out] qout  the configuration integrated.
66
     */
67
    template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
68
    void integrate(const Eigen::MatrixBase<ConfigIn_t> & q,
69
                   const Eigen::MatrixBase<Tangent_t>  & v,
70
                   const Eigen::MatrixBase<ConfigOut_t> & qout) const;
71
72
    /**
73
     * @brief      Computes the Jacobian of the integrate operator around zero.
74
     *
75
     * @details    This function computes the Jacobian of the configuration vector variation (component-vise) with respect to a small variation
76
     *             in the tangent.
77
     *
78
     * @param[in]  q    configuration vector.
79
     *
80
     * @param[out] J    the Jacobian of the log of the Integrate operation w.r.t. the configuration vector q.
81
     *
82
     * @remarks    This function might be useful for instance when using google-ceres to compute the Jacobian of the integrate operation.
83
     */
84
    template<class Config_t, class Jacobian_t>
85
    void integrateCoeffWiseJacobian(const Eigen::MatrixBase<Config_t >  & q,
86
                                    const Eigen::MatrixBase<Jacobian_t> & J) const;
87
88
    /**
89
     * @brief      Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tangent space at identity.
90
     *
91
     * @details    This Jacobian corresponds to the Jacobian of \f$ (\mathbf{q} \oplus \delta \mathbf{q}) \oplus \mathbf{v} \f$ with
92
     *             \f$ \delta \mathbf{q} \rightarrow 0 \f$ if arg == ARG0 or \f$ \delta \mathbf{v} \rightarrow 0 \f$ if arg == ARG1.
93
     *
94
     * @param[in]  q    configuration vector.
95
     * @param[in]  v    tangent vector.
96
     * @param[in]  op   assignment operator (SETTO, ADDTO or RMTO).
97
     * @tparam     arg  ARG0 (resp. ARG1) to get the Jacobian with respect to q (resp. v).
98
     *
99
     * @param[out] J    the Jacobian of the Integrate operation w.r.t. the argument arg.
100
     */
101
    template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
102
320
    void dIntegrate(const Eigen::MatrixBase<Config_t >  & q,
103
                    const Eigen::MatrixBase<Tangent_t>  & v,
104
                    const Eigen::MatrixBase<JacobianOut_t> & J,
105
                    AssignmentOperatorType op = SETTO) const
106
    {
107
      PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
108
320
      return dIntegrate(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg,op);
109
    }
110
111
    /**
112
     * @brief      Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tangent space at identity.
113
     *
114
     * @details    This Jacobian corresponds to the Jacobian of \f$ (\mathbf{q} \oplus \delta \mathbf{q}) \oplus \mathbf{v} \f$ with
115
     *             \f$ \delta \mathbf{q} \rightarrow 0 \f$ if arg == ARG0 or \f$ \delta \mathbf{v} \rightarrow 0 \f$ if arg == ARG1.
116
     *
117
     * @param[in]  q    configuration vector.
118
     * @param[in]  v    tangent vector.
119
     * @param[in] arg  ARG0 (resp. ARG1) to get the Jacobian with respect to q (resp. v).
120
     * @param[in]  op   assignment operator (SETTO, ADDTO and RMTO).
121
     *
122
     * @param[out] J    the Jacobian of the Integrate operation w.r.t. the argument arg.
123
     */
124
    template<class Config_t, class Tangent_t, class JacobianOut_t>
125
    void dIntegrate(const Eigen::MatrixBase<Config_t >  & q,
126
                    const Eigen::MatrixBase<Tangent_t>  & v,
127
                    const Eigen::MatrixBase<JacobianOut_t> & J,
128
                    const ArgumentPosition arg,
129
                    const AssignmentOperatorType op = SETTO) const;
130
131
    /**
132
     * @brief      Computes the Jacobian of a small variation of the configuration vector into tangent space at identity.
133
     *
134
     * @details    This Jacobian corresponds to the Jacobian of \f$ (\mathbf{q} \oplus \delta \mathbf{q}) \oplus \mathbf{v} \f$ with
135
     *             \f$ \delta \mathbf{q} \rightarrow 0 \f$.
136
     *
137
     * @param[in]  q    configuration vector.
138
     * @param[in]  v    tangent vector.
139
     * @param[in]  op   assignment operator (SETTO, ADDTO or RMTO).
140
     *
141
     * @param[out] J    the Jacobian of the Integrate operation w.r.t. the configuration vector q.
142
     */
143
    template <class Config_t, class Tangent_t, class JacobianOut_t>
144
    void dIntegrate_dq(const Eigen::MatrixBase<Config_t >  & q,
145
                       const Eigen::MatrixBase<Tangent_t>  & v,
146
                       const Eigen::MatrixBase<JacobianOut_t> & J,
147
                       const AssignmentOperatorType op = SETTO) const;
148
149
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
150
    void dIntegrate_dq(const Eigen::MatrixBase<Config_t >  & q,
151
                       const Eigen::MatrixBase<Tangent_t>  & v,
152
                       const Eigen::MatrixBase<JacobianIn_t> & Jin,
153
                       int self,
154
                       const Eigen::MatrixBase<JacobianOut_t> & Jout,
155
                       const AssignmentOperatorType op = SETTO) const;
156
157
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
158
    void dIntegrate_dq(const Eigen::MatrixBase<Config_t >  & q,
159
                       const Eigen::MatrixBase<Tangent_t>  & v,
160
                       int self,
161
                       const Eigen::MatrixBase<JacobianIn_t> & Jin,
162
                       const Eigen::MatrixBase<JacobianOut_t> & Jout,
163
                       const AssignmentOperatorType op = SETTO) const;
164
165
    /**
166
     * @brief      Computes the Jacobian of a small variation of the tangent vector into tangent space at identity.
167
     *
168
     * @details    This Jacobian corresponds to the Jacobian of \f$ \mathbf{q} \oplus (\mathbf{v}  + \delta \mathbf{v}) \f$ with
169
     *             \f$ \delta \mathbf{v} \rightarrow 0 \f$.
170
     *
171
     * @param[in]  q    configuration vector.
172
     * @param[in]  v    tangent vector.
173
     * @param[in]  op   assignment operator (SETTO, ADDTO or RMTO).
174
     *
175
     * @param[out] J    the Jacobian of the Integrate operation w.r.t. the tangent vector v.
176
     */
177
    template <class Config_t, class Tangent_t, class JacobianOut_t>
178
    void dIntegrate_dv(const Eigen::MatrixBase<Config_t >  & q,
179
                       const Eigen::MatrixBase<Tangent_t>  & v,
180
                       const Eigen::MatrixBase<JacobianOut_t> & J,
181
                       const AssignmentOperatorType op = SETTO) const;
182
183
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
184
    void dIntegrate_dv(const Eigen::MatrixBase<Config_t >  & q,
185
                       const Eigen::MatrixBase<Tangent_t>  & v,
186
                       int self,
187
                       const Eigen::MatrixBase<JacobianIn_t> & Jin,
188
                       const Eigen::MatrixBase<JacobianOut_t> & Jout,
189
                       const AssignmentOperatorType op = SETTO) const;
190
191
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
192
    void dIntegrate_dv(const Eigen::MatrixBase<Config_t >  & q,
193
                       const Eigen::MatrixBase<Tangent_t>  & v,
194
                       const Eigen::MatrixBase<JacobianIn_t> & Jin,
195
                       int self,
196
                       const Eigen::MatrixBase<JacobianOut_t> & Jout,
197
                       const AssignmentOperatorType op = SETTO) const;
198
199
    /**
200
     *
201
     * @brief   Transport a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the configuration or the velocity arguments.
202
     *
203
     * @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$,
204
     *          to the tangent space at \f$ q \f$.
205
   *          In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between
206
   *          \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector.
207
   *          A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$.
208
     *          In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity.
209
     *          For Lie groups, its corresponds to the canonical vector field transportation.
210
     *
211
     * @param[in]  q        configuration vector.
212
     * @param[in]  v        tangent vector
213
     * @param[in]  Jin    the input matrix
214
     * @param[in]  arg    argument with respect to which the differentiation is performed (ARG0 corresponding to q, and ARG1 to v)
215
     *
216
     * @param[out] Jout    Transported matrix
217
     *
218
     */
219
    template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
220
    void dIntegrateTransport(const Eigen::MatrixBase<Config_t >  & q,
221
                             const Eigen::MatrixBase<Tangent_t>  & v,
222
                             const Eigen::MatrixBase<JacobianIn_t> & Jin,
223
                             const Eigen::MatrixBase<JacobianOut_t> & Jout,
224
                             const ArgumentPosition arg) const;
225
226
    /**
227
     *
228
     * @brief   Transport a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the configuration argument.
229
     *
230
     * @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$,
231
     *          to the tangent space at \f$ q \f$.
232
     *          In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between
233
     *          \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector.
234
     *          A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$.
235
     *          In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity.
236
     *          For Lie groups, its corresponds to the canonical vector field transportation.
237
     *
238
     * @param[in]  q    configuration vector.
239
     * @param[in]  v    tangent vector
240
     * @param[in]  Jin    the input matrix
241
     *
242
     * @param[out] Jout    Transported matrix
243
     */
244
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
245
    void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t >  & q,
246
                                const Eigen::MatrixBase<Tangent_t>  & v,
247
                                const Eigen::MatrixBase<JacobianIn_t> & Jin,
248
                                const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
249
    /**
250
     *
251
     * @brief   Transport a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the velocity argument.
252
     *
253
     * @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$,
254
     *          to the tangent space at \f$ q \f$.
255
   *          In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between
256
   *          \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector.
257
   *          A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$.
258
     *          In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity.
259
     *          For Lie groups, its corresponds to the canonical vector field transportation.
260
     *
261
     * @param[in]  q    configuration vector.
262
     * @param[in]  v    tangent vector
263
     * @param[in]  Jin    the input matrix
264
     *
265
     * @param[out] Jout    Transported matrix
266
     */
267
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
268
    void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t >  & q,
269
                                const Eigen::MatrixBase<Tangent_t>  & v,
270
                                const Eigen::MatrixBase<JacobianIn_t> & Jin,
271
                                const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
272
273
274
    /**
275
     *
276
     * @brief   Transport in place a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the configuration or the velocity arguments.
277
     *
278
     * @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$,
279
     *          to the tangent space at \f$ q \f$.
280
   *          In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between
281
   *          \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector.
282
   *          A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$.
283
     *          In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity.
284
     *          For Lie groups, its corresponds to the canonical vector field transportation.
285
     *
286
     * @param[in]     q       configuration vector.
287
     * @param[in]     v       tangent vector
288
     * @param[in,out] J       the inplace matrix
289
     * @param[in]     arg  argument with respect to which the differentiation is performed (ARG0 corresponding to q, and ARG1 to v)
290
     */
291
    template<class Config_t, class Tangent_t, class Jacobian_t>
292
    void dIntegrateTransport(const Eigen::MatrixBase<Config_t >  & q,
293
                             const Eigen::MatrixBase<Tangent_t>  & v,
294
                             const Eigen::MatrixBase<Jacobian_t> & J,
295
                             const ArgumentPosition arg) const;
296
297
    /**
298
     *
299
     * @brief   Transport in place a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the configuration argument.
300
     *
301
     * @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$,
302
     *          to the tangent space at \f$ q \f$.
303
   *          In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between
304
   *          \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector.
305
   *          A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$.
306
     *          In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity.
307
     *          For Lie groups, its corresponds to the canonical vector field transportation.
308
     *
309
     * @param[in]  q    configuration vector.
310
     * @param[in]  v    tangent vector
311
     * @param[in,out]  Jin    the inplace matrix
312
     *
313
    */
314
    template <class Config_t, class Tangent_t, class Jacobian_t>
315
    void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t >  & q,
316
                                const Eigen::MatrixBase<Tangent_t>  & v,
317
                                const Eigen::MatrixBase<Jacobian_t> & J) const;
318
    /**
319
     *
320
     * @brief   Transport in place a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the velocity argument.
321
     *
322
     * @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$,
323
     *          to the tangent space at \f$ q \f$.
324
   *          In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between
325
   *          \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector.
326
   *          A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$.
327
     *          In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity.
328
     *          For Lie groups, its corresponds to the canonical vector field transportation.
329
     *
330
     * @param[in]  q    configuration vector.
331
     * @param[in]  v    tangent vector
332
     * @param[in,out]  J    the inplace matrix
333
     *
334
    */
335
    template <class Config_t, class Tangent_t, class Jacobian_t>
336
    void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t >  & q,
337
                                const Eigen::MatrixBase<Tangent_t>  & v,
338
                                const Eigen::MatrixBase<Jacobian_t> & J) const;
339
340
    /**
341
     * @brief      Interpolation between two joint's configurations
342
     *
343
     * @param[in]  q0    the initial configuration to interpolate.
344
     * @param[in]  q1    the final configuration to interpolate.
345
     * @param[in]  u     in [0;1] the absicca along the interpolation.
346
     *
347
     * @param[out] qout  the interpolated configuration (q0 if u = 0, q1 if u = 1)
348
     */
349
    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
350
    void interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
351
                     const Eigen::MatrixBase<ConfigR_t> & q1,
352
                     const Scalar& u,
353
                     const Eigen::MatrixBase<ConfigOut_t> & qout) const;
354
355
    /**
356
     * @brief      Normalize the joint configuration given as input.
357
     *             For instance, the quaternion must be unitary.
358
     *
359
     * @note       If the input vector is too small (i.e., qout.norm()==0), then it is left unchanged.
360
     *             It is therefore possible that after this method is called `isNormalized(qout)` is still false.
361
     *
362
     * @param[in,out]     qout  the normalized joint configuration.
363
     */
364
    template <class Config_t>
365
    void normalize(const Eigen::MatrixBase<Config_t> & qout) const;
366
367
    /**
368
     * @brief      Check whether the input joint configuration is normalized.
369
     *             For instance, the quaternion must be unitary.
370
     *
371
     * @param[in]     qin  The joint configuration to check.
372
     *
373
     * @return true if the input vector is normalized, false otherwise.
374
     */
375
    template <class Config_t>
376
    bool isNormalized(const Eigen::MatrixBase<Config_t> & qin,
377
                      const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
378
379
    /**
380
     * @brief      Generate a random joint configuration, normalizing quaternions when necessary.
381
     *
382
     * \warning    Do not take into account the joint limits. To shoot a configuration uniformingly
383
     *             depending on joint limits, see randomConfiguration.
384
     *
385
     * @param[out] qout  the random joint configuration.
386
     */
387
    template <class Config_t>
388
    void random(const Eigen::MatrixBase<Config_t> & qout) const;
389
390
    /**
391
     * @brief      Generate a configuration vector uniformly sampled among
392
     *             provided limits.
393
     *
394
     * @param[in]  lower_pos_limit  the lower joint limit vector.
395
     * @param[in]  upper_pos_limit  the upper joint limit vector.
396
     *
397
     * @param[out] qout             the random joint configuration in the two bounds.
398
     */
399
    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
400
    void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
401
                             const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
402
                             const Eigen::MatrixBase<ConfigOut_t> & qout) const;
403
404
    /**
405
     * @brief      Computes the tangent vector that must be integrated during one unit time to go from q0 to q1.
406
     *
407
     * @param[in]  q0    the initial configuration vector.
408
     * @param[in]  q1    the terminal configuration vector.
409
     *
410
     * @param[out] v     the corresponding velocity.
411
     *
412
     * @note             Both inputs must be well-formed configuration vectors. The output of this function is
413
     *                   unspecified if inputs contain NaNs or extremal values for the underlying scalar type.
414
     *
415
     * \cheatsheet \f$ q_1 \ominus q_0 = - \left( q_0 \ominus q_1 \right) \f$
416
     */
417
    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
418
    void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
419
                    const Eigen::MatrixBase<ConfigR_t> & q1,
420
                    const Eigen::MatrixBase<Tangent_t> & v) const;
421
422
    /**
423
     *
424
     * @brief      Computes the Jacobian of the difference operation with respect to q0 or q1.
425
     *
426
     * @tparam     arg   ARG0 (resp. ARG1) to get the Jacobian with respect to q0 (resp. q1).
427
     *
428
     * @param[in]  q0    the initial configuration vector.
429
     * @param[in]  q1    the terminal configuration vector.
430
     *
431
     * @param[out] J     the Jacobian of the difference operation.
432
     *
433
     * \warning because \f$ q_1 \ominus q_0 = - \left( q_0 \ominus q_1 \right) \f$,
434
     * you may be tempted to write
435
     * \f$ \frac{\partial\ominus}{\partial q_1} = - \frac{\partial\ominus}{\partial q_0} \f$.
436
     * This is **false** in the general case because
437
     * \f$ \frac{\partial\ominus}{\partial q_i} \f$ expects an input velocity relative to frame i.
438
     * See SpecialEuclideanOperationTpl<3,_Scalar,_Options>::dDifference_impl.
439
     *
440
     * \cheatsheet \f$ \frac{\partial\ominus}{\partial q_1} \frac{\partial\oplus}{\partial v} = I \f$
441
     */
442
    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
443
    void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
444
                     const Eigen::MatrixBase<ConfigR_t> & q1,
445
                     const Eigen::MatrixBase<JacobianOut_t> & J) const;
446
447
    /**
448
     *
449
     * @brief      Computes the Jacobian of the difference operation with respect to q0 or q1.
450
     *
451
     * @param[in]  q0    the initial configuration vector.
452
     * @param[in]  q1    the terminal configuration vector.
453
     * @param[in]  arg   ARG0 (resp. ARG1) to get the Jacobian with respect to q0 (resp. q1).
454
     *
455
     * @param[out] J     the Jacobian of the difference operation.
456
     *
457
     */
458
    template<class ConfigL_t, class ConfigR_t, class JacobianOut_t>
459
    void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
460
                     const Eigen::MatrixBase<ConfigR_t> & q1,
461
                     const Eigen::MatrixBase<JacobianOut_t> & J,
462
                     const ArgumentPosition arg) const;
463
464
    template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
465
    void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
466
                     const Eigen::MatrixBase<ConfigR_t> & q1,
467
                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
468
                     int self,
469
                     const Eigen::MatrixBase<JacobianOut_t> & Jout,
470
                     const AssignmentOperatorType op = SETTO) const;
471
472
    template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
473
    void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
474
                     const Eigen::MatrixBase<ConfigR_t> & q1,
475
                     int self,
476
                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
477
                     const Eigen::MatrixBase<JacobianOut_t> & Jout,
478
                     const AssignmentOperatorType op = SETTO) const;
479
480
    /**
481
     * @brief      Squared distance between two joint configurations.
482
     *
483
     * @param[in]  q0    the initial configuration vector.
484
     * @param[in]  q1    the terminal configuration vector.
485
     *
486
     * @param[out] d     the corresponding distance betwenn q0 and q1.
487
     */
488
    template <class ConfigL_t, class ConfigR_t>
489
    Scalar squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
490
                           const Eigen::MatrixBase<ConfigR_t> & q1) const;
491
492
    /**
493
     * @brief      Distance between two configurations of the joint
494
     *
495
     * @param[in]  q0    the initial configuration vector.
496
     * @param[in]  q1    the terminal configuration vector.
497
     *
498
     * @return     The corresponding distance.
499
     */
500
    template <class ConfigL_t, class ConfigR_t>
501
    Scalar distance(const Eigen::MatrixBase<ConfigL_t> & q0,
502
                    const Eigen::MatrixBase<ConfigR_t> & q1) const;
503
504
    /**
505
     * @brief      Check if two configurations are equivalent within the given precision.
506
     *
507
     * @param[in]  q0    Configuration 0
508
     * @param[in]  q1    Configuration 1
509
     *
510
     * \cheatsheet \f$ q_1 \equiv  q_0 \oplus \left( q_1 \ominus q_0 \right) \f$ (\f$\equiv\f$ means equivalent, not equal).
511
     */
512
    template <class ConfigL_t, class ConfigR_t>
513
    bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
514
                             const Eigen::MatrixBase<ConfigR_t> & q1,
515
                             const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
516
517
38
    bool operator== (const LieGroupBase& other) const
518
    {
519
38
      return derived().isEqual_impl(other.derived());
520
    }
521
522
    bool operator!= (const LieGroupBase& other) const
523
    {
524
      return derived().isDifferent_impl(other.derived());
525
    }
526
    /// \}
527
528
    /// \name API that allocates memory
529
    /// \{
530
531
    template <class Config_t, class Tangent_t>
532
    ConfigVector_t integrate(const Eigen::MatrixBase<Config_t>  & q,
533
                             const Eigen::MatrixBase<Tangent_t> & v) const ;
534
535
    template <class ConfigL_t, class ConfigR_t>
536
    ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
537
                               const Eigen::MatrixBase<ConfigR_t> & q1,
538
                               const Scalar& u) const;
539
540
    ConfigVector_t random() const;
541
542
    template <class ConfigL_t, class ConfigR_t>
543
    ConfigVector_t randomConfiguration
544
    (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
545
     const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;
546
547
    template <class ConfigL_t, class ConfigR_t>
548
    TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
549
                               const Eigen::MatrixBase<ConfigR_t> & q1) const;
550
    /// \}
551
552
553
    /// \name Default implementations
554
    /// \{
555
556
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
557
    void dIntegrate_product_impl(const Config_t & q,
558
                                 const Tangent_t & v,
559
                                 const JacobianIn_t & Jin,
560
                                 JacobianOut_t & Jout,
561
                                 bool dIntegrateOnTheLeft,
562
                                 const ArgumentPosition arg,
563
                                 const AssignmentOperatorType op) const;
564
565
    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
566
    void dDifference_product_impl(const ConfigL_t & q0,
567
                                  const ConfigR_t & q1,
568
                                  const JacobianIn_t & Jin,
569
                                  JacobianOut_t & Jout,
570
                                  bool dDifferenceOnTheLeft,
571
                                  const AssignmentOperatorType op) const;
572
573
    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
574
    void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
575
                          const Eigen::MatrixBase<ConfigR_t> & q1,
576
                          const Scalar& u,
577
                          const Eigen::MatrixBase<ConfigOut_t> & qout) const;
578
579
    template <class Config_t>
580
    void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
581
582
    template <class Config_t>
583
    bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin,
584
                           const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
585
586
    template <class ConfigL_t, class ConfigR_t>
587
    Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
588
                                const Eigen::MatrixBase<ConfigR_t> & q1) const;
589
590
    template <class ConfigL_t, class ConfigR_t>
591
    bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
592
                                  const Eigen::MatrixBase<ConfigR_t> & q1,
593
                                  const Scalar & prec) const;
594
595
    /// \brief Default equality check.
596
    /// By default, two LieGroupBase of same type are considered equal.
597
10
    bool isEqual_impl (const LieGroupBase& /*other*/) const { return true; }
598
11
    bool isDifferent_impl (const LieGroupBase& other) const
599
    {
600
11
      return !derived().isEqual_impl(other.derived());
601
    }
602
603
    /// Get dimension of Lie Group vector representation
604
    ///
605
    /// For instance, for SO(3), the dimension of the vector representation is
606
    /// 4 (quaternion) while the dimension of the tangent space is 3.
607
    Index nq () const;
608
    /// Get dimension of Lie Group tangent space
609
    Index nv () const;
610
    /// Get neutral element as a vector
611
    ConfigVector_t neutral () const;
612
613
    /// Get name of instance
614
    std::string name () const;
615
616
    Derived& derived ()
617
    {
618
      return static_cast <Derived&> (*this);
619
    }
620
621
1098959
    const Derived& derived () const
622
    {
623
1098959
      return static_cast <const Derived&> (*this);
624
    }
625
    /// \}
626
627
  protected:
628
    /// Default constructor.
629
    ///
630
    /// Prevent the construction of derived class.
631
790652
    LieGroupBase() {}
632
633
    /// Copy constructor
634
    ///
635
    /// Prevent the copy of derived class.
636
1710
    LieGroupBase( const LieGroupBase & /*clone*/) {}
637
    LieGroupBase& operator=(const LieGroupBase & /*x*/) { return *this; }
638
639
    // C++11
640
    // LieGroupBase(const LieGroupBase &) = delete;
641
    // LieGroupBase& operator=(const LieGroupBase & /*x*/) = delete;
642
  }; // struct LieGroupBase
643
644
} // namespace pinocchio
645
646
#include "pinocchio/multibody/liegroup/liegroup-base.hxx"
647
648
#endif // ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__