joint-configuration.hpp 38.3 KB
Newer Older
1
//
2
// Copyright (c) 2016-2019 CNRS INRIA
3
4
//

5
6
#ifndef __pinocchio_joint_configuration_hpp__
#define __pinocchio_joint_configuration_hpp__
7

8
#include "pinocchio/multibody/fwd.hpp"
9
10
#include "pinocchio/multibody/model.hpp"

11
#include "pinocchio/multibody/liegroup/liegroup.hpp"
12

13
namespace pinocchio
14
15
{

16
17
18
  /// \name API with return value as argument
  /// \{

19
  /**
20
   *
21
   * @brief      Integrate a configuration vector for the specified model for a tangent vector during one unit time
22
   *
23
24
25
   * @details This function corresponds to the exponential map of the joint configuration Lie Group.
   *          Its output can be interpreted as the "sum" from the Lie algebra to the joint configuration space \f$ q \oplus v \f$.
   *
26
   * @param[in]  model   Model of the kinematic tree over which the integration is performed.
27
   * @param[in]  q       Initial configuration (size model.nq)
28
   * @param[in]  v       Joint velocity (size model.nv)
29
   *
30
   * @param[out] qout    The integrated configuration (size model.nq)
31
   *
32
33
34
35
36
37
38
39
40
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType>
  void
  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
            const Eigen::MatrixBase<ConfigVectorType> & q,
            const Eigen::MatrixBase<TangentVectorType> & v,
            const Eigen::MatrixBase<ReturnType> & qout);

  /**
41
   *
42
   * @brief      Integrate a configuration vector for the specified model for a tangent vector during one unit time
43
   *
44
45
46
   * @details This function corresponds to the exponential map of the joint configuration Lie Group.
   *          Its output can be interpreted as the "sum" from the Lie algebra to the joint configuration space \f$ q \oplus v \f$.
   *
47
   * @param[in]  model  Model of the kinematic tree over which the integration is performed.
48
   * @param[in]  q       Initial configuration (size model.nq)
49
   * @param[in]  v       Joint velocity (size model.nv)
50
   *
51
   * @param[out] qout    The integrated configuration (size model.nq)
52
   *
53
54
55
56
57
58
59
60
61
62
63
64
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType>
  void
  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
            const Eigen::MatrixBase<ConfigVectorType> & q,
            const Eigen::MatrixBase<TangentVectorType> & v,
            const Eigen::MatrixBase<ReturnType> & qout)
  {
    integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,ReturnType>(model, q.derived(), v.derived(), qout.derived());
  }

  /**
65
   *
66
   * @brief      Interpolate two configurations for a given model
67
   *
68
   * @param[in]  model   Model of the kinematic tree over which the interpolation is performed.
69
70
71
   * @param[in]  q0      Initial configuration vector (size model.nq)
   * @param[in]  q1      Final configuration vector (size model.nq)
   * @param[in]  u       u in [0;1] position along the interpolation.
72
   *
73
   * @param[out] qout    The interpolated configuration (q0 if u = 0, q1 if u = 1)
74
   *
75
76
77
78
79
80
81
82
83
84
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
  void
  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
              const Eigen::MatrixBase<ConfigVectorIn1> & q0,
              const Eigen::MatrixBase<ConfigVectorIn2> & q1,
              const Scalar & u,
              const Eigen::MatrixBase<ReturnType> & qout);

  /**
85
   *
86
   * @brief      Interpolate two configurations for a given model
87
   *
88
   * @param[in]  model   Model of the kinematic tree over which the interpolation is performed.
89
90
91
   * @param[in]  q0      Initial configuration vector (size model.nq)
   * @param[in]  q1      Final configuration vector (size model.nq)
   * @param[in]  u       u in [0;1] position along the interpolation.
92
   *
93
   * @param[out] qout    The interpolated configuration (q0 if u = 0, q1 if u = 1)
94
   *
95
96
97
98
99
100
101
102
103
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
  void
  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
              const Eigen::MatrixBase<ConfigVectorIn1> & q0,
              const Eigen::MatrixBase<ConfigVectorIn2> & q1,
              const Scalar & u,
              const Eigen::MatrixBase<ReturnType> & qout)
  {
104
    interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
105
106
107
  }

  /**
108
   *
109
110
   * @brief      Compute the tangent vector that must be integrated during one unit time to go from q0 to q1
   *
111
112
113
   * @details This function corresponds to the log map of the joint configuration Lie Group.
   *          Its output can be interpreted as a difference from the joint configuration space to the Lie algebra \f$ q_1 \ominus q_0 \f$.
   *
114
   * @param[in]  model   Model of the system over which the difference operation is performed.
115
116
   * @param[in]  q0      Initial configuration (size model.nq)
   * @param[in]  q1      Wished configuration (size model.nq)
117
   *
118
   * @param[out] dvout   The corresponding velocity (size model.nv)
119
   *
120
121
122
123
124
125
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
  void
  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
             const Eigen::MatrixBase<ConfigVectorIn1> & q0,
             const Eigen::MatrixBase<ConfigVectorIn2> & q1,
126
             const Eigen::MatrixBase<ReturnType> & dvout);
127
128

  /**
129
   *
130
131
   * @brief      Compute the tangent vector that must be integrated during one unit time to go from q0 to q1
   *
132
133
134
   * @details This function corresponds to the log map of the joint configuration Lie Group.
   *          Its output can be interpreted as a difference from the joint configuration space to the Lie algebra \f$ q_1 \ominus q_0 \f$.
   *
135
   * @param[in]  model   Model of the system over which the difference operation is performed.
136
137
   * @param[in]  q0      Initial configuration (size model.nq)
   * @param[in]  q1      Wished configuration (size model.nq)
138
139
140
   *
   * @param[out] dvout   The corresponding velocity (size model.nv).
   *
141
142
143
144
145
146
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
  void
  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
             const Eigen::MatrixBase<ConfigVectorIn1> & q0,
             const Eigen::MatrixBase<ConfigVectorIn2> & q1,
147
             const Eigen::MatrixBase<ReturnType> & dvout)
148
  {
149
    difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,dvout));
150
151
152
  }

  /**
153
   *
154
155
   * @brief      Squared distance between two configuration vectors
   *
156
157
158
159
160
   * @param[in]  model      Model of the system over which the squared distance operation is performed.
   * @param[in]  q0             Configuration 0 (size model.nq)
   * @param[in]  q1             Configuration 1 (size model.nq)
   * @param[out] out          The corresponding squared distances for each joint (size model.njoints-1 = number of joints).
   *
161
162
163
164
165
166
167
168
169
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
  void
  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
                  const Eigen::MatrixBase<ReturnType> & out);

  /**
170
   *
171
172
   * @brief      Squared distance between two configuration vectors
   *
173
174
175
176
177
   * @param[in]  model      Model of the system over which the squared distance operation is performed.
   * @param[in]  q0             Configuration 0 (size model.nq)
   * @param[in]  q1             Configuration 1 (size model.nq)
   * @param[out] out          The corresponding squared distances for each joint (size model.njoints-1 = number of joints).
   *
178
179
180
181
182
183
184
185
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
  void
  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
                  const Eigen::MatrixBase<ReturnType> & out)
  {
186
    squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,out));
187
188
189
  }

  /**
190
   *
191
192
193
194
   * @brief      Generate a configuration vector uniformly sampled among provided limits.
   *
   * @remarks    Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded.
   *
195
196
197
198
199
200
   * @warning     If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample.
   *
   * @param[in]  model                Model of the system over which the random configuration operation is performed.
   * @param[in]  lowerLimits  Joints lower limits (size model.nq).
   * @param[in]  upperLimits  Joints upper limits (size model.nq).
   * @param[out] qout                  The resulting configuration vector (size model.nq).
201
202
203
204
205
206
207
208
209
210
   *
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
  void
  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                      const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
                      const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
                      const Eigen::MatrixBase<ReturnType> & qout);

 /**
211
212
213
214
215
216
217
218
219
220
221
222
223
  *
  * @brief      Generate a configuration vector uniformly sampled among provided limits.
  *
  * @remarks    Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded.
  *
  * @warning     If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample
  *
  * @param[in]  model               Model of the system over which the random configuration operation is performed.
  * @param[in]  lowerLimits  Joints lower limits (size model.nq).
  * @param[in]  upperLimits  Joints upper limits (size model.nq).
  * @param[out] qout                  The resulting configuration vector (size model.nq).
  *
  */
224
225
226
227
228
229
230
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
  void
  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                      const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
                      const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
                      const Eigen::MatrixBase<ReturnType> & qout)
  {
231
    randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, lowerLimits.derived(), upperLimits.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
232
233
234
  }

  /**
235
   *
236
237
   * @brief         Return the neutral configuration element related to the model configuration space.
   *
238
239
240
241
   * @param[in]     model      Model of the kinematic tree over which the neutral element is computed
   *
   * @param[out]    qout        The neutral configuration element (size model.nq).
   *
242
243
244
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType>
  void
245
246
  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
          const Eigen::MatrixBase<ReturnType> & qout);
247
248

  /**
249
   *
250
251
   * @brief         Return the neutral configuration element related to the model configuration space.
   *
252
253
254
255
   * @param[in]     model      Model of the kinematic tree over which the neutral element is computed.
   *
   * @param[out]    qout        The neutral configuration element (size model.nq).
   *
256
257
258
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType>
  void
259
260
  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
          const Eigen::MatrixBase<ReturnType> & qout)
261
  {
262
    neutral<LieGroupMap,Scalar,Options,JointCollectionTpl,ReturnType>(model,PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
263
264
  }

265
  /**
266
267
   *
   * @brief   Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity.
268
   *
269
270
271
272
273
274
275
   * @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such,
   *          it is expressed in the tangent space only, not the configuration space.
   *          Calling \f$ f(q, v) \f$ the integrate function, these jacobians satisfy the following relationships in the
   *          tangent space:
   *           - Jacobian relative to q: \f$ f(q \oplus \delta q, v) \ominus f(q, v) = J_q \delta q + o(\delta q)\f$.
   *           - Jacobian relative to v: \f$ f(q, v + \delta v) \ominus f(q, v) = J_v \delta v + o(\delta v)\f$.
   *
276
277
   * @param[in]  model   Model that must be integrated
   * @param[in]  q       Initial configuration (size model.nq)
278
   * @param[in]  v       Joint velocity (size model.nv)
279
   * @param[out] J       Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv).
280
   * @param[in]  arg     Argument (either q or v) with respect to which the differentiation is performed.
281
   *
282
   */
283
284
285
286
287
288
289
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
  void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  const Eigen::MatrixBase<ConfigVectorType> & q,
                  const Eigen::MatrixBase<TangentVectorType> & v,
                  const Eigen::MatrixBase<JacobianMatrixType> & J,
                  const ArgumentPosition arg);

290
  /**
291
292
   *
   * @brief   Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity.
293
   *
294
295
296
297
298
299
300
   * @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such,
   *          it is expressed in the tangent space only, not the configuration space.
   *          Calling \f$ f(q, v) \f$ the integrate function, these jacobians satisfy the following relationships in the
   *          tangent space:
   *           - Jacobian relative to q: \f$ f(q \oplus \delta q, v) \ominus f(q, v) = J_q(q, v) \delta q + o(\delta q)\f$.
   *           - Jacobian relative to v: \f$ f(q, v + \delta v) \ominus f(q, v) = J_v(q, v) \delta v + o(\delta v)\f$.
   *
301
302
   * @param[in]  model   Model that must be integrated
   * @param[in]  q       Initial configuration (size model.nq)
303
   * @param[in]  v       Joint velocity (size model.nv)
304
   * @param[out] J       Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv).
305
   * @param[in]  arg     Argument (either q or v) with respect to which the differentiation is performed.
306
307
   *
   */
308
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
309
310
311
312
  void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  const Eigen::MatrixBase<ConfigVectorType> & q,
                  const Eigen::MatrixBase<TangentVectorType> & v,
                  const Eigen::MatrixBase<JacobianMatrixType> & J,
313
314
                  const ArgumentPosition arg)
  {
315
    dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
316
317
  }

318
  /**
319
   *
320
321
322
323
324
325
326
   * @brief      Overall squared distance between two configuration vectors
   *
   * @param[in]  model      Model we want to compute the distance
   * @param[in]  q0         Configuration 0 (size model.nq)
   * @param[in]  q1         Configuration 1 (size model.nq)
   *
   * @return     The squared distance between the two configurations
327
   *
328
329
330
331
332
333
334
335
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  Scalar squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                            const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                            const Eigen::MatrixBase<ConfigVectorIn2> & q1);

  /**
   *
336
337
338
339
340
341
342
   * @brief      Overall squared distance between two configuration vectors, namely \f$ || q_{1} \ominus q_{0} ||_2^{2} \f$.
   *
   * @param[in]  model  Model of the kinematic tree
   * @param[in]  q0         Configuration from (size model.nq)
   * @param[in]  q1         Configuration to (size model.nq)
   *
   * @return     The squared distance between the two configurations q0 and q1.
343
344
345
346
347
348
349
350
351
352
353
   *
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  inline Scalar
  squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                     const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                     const Eigen::MatrixBase<ConfigVectorIn2> & q1)
  {
    return squaredDistanceSum<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
  }

354
  /**
355
356
   *
   * @brief      Distance between two configuration vectors, namely \f$ || q_{1} \ominus q_{0} ||_2 \f$.
357
358
359
360
361
   *
   * @param[in]  model      Model we want to compute the distance
   * @param[in]  q0         Configuration 0 (size model.nq)
   * @param[in]  q1         Configuration 1 (size model.nq)
   *
362
363
   * @return     The distance between the two configurations q0 and q1.
   *
364
365
366
367
368
369
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  Scalar distance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                  const Eigen::MatrixBase<ConfigVectorIn2> & q1);

Gabriele Buondonno's avatar
Gabriele Buondonno committed
370
  /**
371
   *
Gabriele Buondonno's avatar
Gabriele Buondonno committed
372
373
374
375
376
377
   * @brief      Distance between two configuration vectors
   *
   * @param[in]  model      Model we want to compute the distance
   * @param[in]  q0         Configuration 0 (size model.nq)
   * @param[in]  q1         Configuration 1 (size model.nq)
   *
378
379
   * @return     The distance between the two configurations q0 and q1.
   *
Gabriele Buondonno's avatar
Gabriele Buondonno committed
380
381
382
383
384
385
386
387
388
389
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  inline Scalar
  distance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
           const Eigen::MatrixBase<ConfigVectorIn1> & q0,
           const Eigen::MatrixBase<ConfigVectorIn2> & q1)
  {
    return distance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
  }

390
391
  /**
   *
392
393
394
395
   * @brief         Normalize a configuration vector.
   *
   * @param[in]     model      Model of the kinematic tree.
   * @param[in,out] q               Configuration to normalize (size model.nq).
396
397
398
399
400
401
402
403
   *
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const Eigen::MatrixBase<ConfigVectorType> & qout);

  /**
   *
404
405
406
407
   * @brief         Normalize a configuration vector.
   *
   * @param[in]     model      Model of the kinematic tree.
   * @param[in,out] q               Configuration to normalize (size model.nq).
408
409
410
411
412
413
   *
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const Eigen::MatrixBase<ConfigVectorType> & qout)
  {
414
    normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout));
415
416
417
418
  }

  /**
   *
419
420
421
422
423
424
425
426
427
   * @brief         Return true if the given configurations are equivalents, within the given precision.
   * @remarks       Two configurations can be equivalent but not equally coefficient wise (e.g two quaternions with opposite coefficients give rise to the same orientation, i.e. they are equivalent.).
   *
   * @param[in]     model     Model of the kinematic tree.
   * @param[in]     q1        The first configuraiton to compare.
   * @param[in]     q2        The second configuration to compare.
   * @param[in]     prec      precision of the comparison.
   *
   * @return     Whether the configurations are equivalent or not, within the given precision.
428
429
430
431
432
433
434
435
436
437
438
   *
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  inline bool
  isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                      const Eigen::MatrixBase<ConfigVectorIn1> & q1,
                      const Eigen::MatrixBase<ConfigVectorIn2> & q2,
                      const Scalar & prec);

  /**
   *
439
440
441
442
443
444
445
   * @brief         Return true if the given configurations are equivalents, within the given precision.
   * @remarks       Two configurations can be equivalent but not equally coefficient wise (e.g two quaternions with opposite coefficients give rise to the same orientation, i.e. they are equivalent.).
   *
   * @param[in]     model     Model of the kinematic tree.
   * @param[in]     q1           The first configuraiton to compare
   * @param[in]     q2           The second configuration to compare
   * @param[in]     prec      precision of the comparison.
446
   *
447
   * @return     Whether the configurations are equivalent or not
448
   *
449
450
451
452
453
454
455
456
457
458
459
460
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  inline bool
  isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                      const Eigen::MatrixBase<ConfigVectorIn1> & q1,
                      const Eigen::MatrixBase<ConfigVectorIn2> & q2,
                      const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
  {
    return isSameConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q1.derived(), q2.derived(), prec);
  }

  /**
461
   *
462
463
   * @brief         Return the Jacobian of the integrate function for the components of the config vector.
   *
464
   * @param[in]     model          Model of the kinematic tree.
465
466
467
468
469
470
471
472
473
474
475
476
477
   * @param[out]    jacobian   The Jacobian of the integrate operation.
   *
   * @details       This function is often required for the numerical solvers that are working on the
   *                tangent of the configuration space, instead of the configuration space itself.
   *
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
  inline void
  integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                             const Eigen::MatrixBase<ConfigVector> & q,
                             const Eigen::MatrixBase<JacobianMatrix> & jacobian);

  /**
478
   *
479
480
   * @brief         Return the Jacobian of the integrate function for the components of the config vector.
   *
481
   * @param[in]     model          Model of the kinematic tree.
482
483
484
485
486
487
488
489
490
491
492
493
   * @param[out]    jacobian   The Jacobian of the integrate operation.
   *
   * @details       This function is often required for the numerical solvers that are working on the
   *                tangent of the configuration space, instead of the configuration space itself.
   *
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
  inline void
  integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                             const Eigen::MatrixBase<ConfigVector> & q,
                             const Eigen::MatrixBase<JacobianMatrix> & jacobian)
  {
494
    integrateCoeffWiseJacobian<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector,JacobianMatrix>(model,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian));
495
496
497
498
499
500
501
502
  }

  /// \}

  /// \name API that allocates memory
  /// \{

  /**
503
   *
504
   * @brief      Integrate a configuration vector for the specified model for a tangent vector during one unit time
505
   *
506
507
508
   * @param[in]  model   Model of the kinematic tree over which the integration operation is performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  v            Joint velocity (size model.nv)
509
510
   *
   * @return     The integrated configuration (size model.nq)
511
   *
512
513
514
515
516
517
518
519
520
   */
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
            const Eigen::MatrixBase<ConfigVectorType> & q,
            const Eigen::MatrixBase<TangentVectorType> & v);

  /**
   *
521
522
523
524
525
   * @brief      Integrate a configuration vector for the specified model for a tangent vector during one unit time.
   *
   * @param[in]  model   Model of the kinematic tree over which the integration operation is performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  v            Joint velocity (size model.nv)
526
527
   *
   * @return     The integrated configuration (size model.nq)
528
   *
529
530
531
532
533
534
535
536
537
538
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
            const Eigen::MatrixBase<ConfigVectorType> & q,
            const Eigen::MatrixBase<TangentVectorType> & v)
  {
    return integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType>(model, q.derived(), v.derived());
  }

539
540
  /**
   *
541
542
543
544
545
546
   * @brief      Interpolate two configurations for a given model.
   *
   * @param[in]  model   Model of the kinematic tree over which the interpolation operation is performed.
   * @param[in]  q0          Initial configuration vector (size model.nq)
   * @param[in]  q1          Final configuration vector (size model.nq)
   * @param[in]  u            u in [0;1] position along the interpolation.
547
   *
548
   * @return     The interpolated configuration (q0 if u = 0, q1 if u = 1)
549
   *
550
   */
551
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
gabrielebndn's avatar
gabrielebndn committed
552
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
553
  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
554
555
556
              const Eigen::MatrixBase<ConfigVectorIn1> & q0,
              const Eigen::MatrixBase<ConfigVectorIn2> & q1,
              const Scalar & u);
557

558
559
  /**
   *
560
561
562
563
564
565
   * @brief      Interpolate two configurations for a given model.
   *
   * @param[in]  model   Model of the kinematic tree over which the interpolation operation is performed.
   * @param[in]  q0          Initial configuration vector (size model.nq)
   * @param[in]  q1          Final configuration vector (size model.nq)
   * @param[in]  u            u in [0;1] position along the interpolation.
566
567
   *
   * @return     The interpolated configuration (q0 if u = 0, q1 if u = 1)
568
   *
569
570
571
572
573
574
575
576
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
              const Eigen::MatrixBase<ConfigVectorIn1> & q0,
              const Eigen::MatrixBase<ConfigVectorIn2> & q1,
              const Scalar & u)
  {
577
    return interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived(), u);
578
579
  }

580
  /**
581
   *
582
   * @brief      Compute the tangent vector that must be integrated during one unit time to go from q0 to q1
583
   *
584
585
586
   * @param[in]  model   Model of the kinematic tree over which the difference operation is performed.
   * @param[in]  q0          Initial configuration (size model.nq)
   * @param[in]  q1          Finial configuration (size model.nq)
587
   *
588
   * @return     The corresponding velocity (size model.nv)
589
   *
590
   */
591
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
gabrielebndn's avatar
gabrielebndn committed
592
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
593
  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
594
595
             const Eigen::MatrixBase<ConfigVectorIn1> & q0,
             const Eigen::MatrixBase<ConfigVectorIn2> & q1);
596

597
598
  /**
   *
599
600
601
602
603
   * @brief      Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.
   *
   * @param[in]  model   Model of the kinematic tree over which the difference operation is performed.
   * @param[in]  q0          Initial configuration (size model.nq)
   * @param[in]  q1          Final configuration (size model.nq)
604
605
   *
   * @return     The corresponding velocity (size model.nv)
606
   *
607
608
609
610
611
612
613
614
615
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
             const Eigen::MatrixBase<ConfigVectorIn1> & q0,
             const Eigen::MatrixBase<ConfigVectorIn2> & q1)
  {
    return difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
  }
616
617
618

  /**
   *
619
620
621
622
623
624
625
   * @brief      Squared distance between two configurations.
   *
   * @param[in]  model      Model of the kinematic tree over which the squared distance operation is performed.
   * @param[in]  q0             Configuration 0 (size model.nq)
   * @param[in]  q1             Configuration 1 (size model.nq)
   *
   * @return     The corresponding squared distances for each joint (size model.njoints-1, corresponding to the number of joints)
626
   *
627
   */
628
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
gabrielebndn's avatar
gabrielebndn committed
629
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
630
  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
631
632
                  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
633

634
  /**
635
   *
636
   * @brief      Squared distance between two configuration vectors
637
   *
638
639
640
641
642
   * @param[in]  model      Model of the kinematic tree over which the squared distance operation is performed.
   * @param[in]  q0             Configuration 0 (size model.nq)
   * @param[in]  q1             Configuration 1 (size model.nq)
   *
   * @return     The corresponding squared distances for each joint (size model.njoints-1, corresponding to the number of joints)
643
   *
644
   */
645
646
647
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
648
                  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
649
650
651
652
                  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
  {
    return squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
  }
653
654

  /**
655
656
   *
   * @brief      Generate a configuration vector uniformly sampled among given limits.
657
   *
658
   * @remarks    Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded.
659
   *
660
661
   * @warning     If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample
   *
662
663
664
665
666
   * @param[in]  model                Model of the kinematic tree over which the uniform sampling operation is performed.
   * @param[in]  lowerLimits   Joints lower limits (size model.nq).
   * @param[in]  upperLimits   Joints upper limits (size model.nq).
   *
   * @return     The resulting configuration vector (size model.nq).
667
668
   *
   */
669
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
gabrielebndn's avatar
gabrielebndn committed
670
  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
671
  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
672
673
                      const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
                      const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
674

675
 /**
676
677
678
679
680
681
682
683
684
685
686
687
688
689
  *
  * @brief      Generate a configuration vector uniformly sampled among provided limits.
  *
  * @remarks    Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded.
  *
  * @warning     If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample
  *
  * @param[in]  model               Model of the kinematic tree over which the uniform sampling operation is performed.
  * @param[in]  lowerLimits  Joints lower limits (size model.nq).
  * @param[in]  upperLimits  Joints upper limits (size model.nq).
  *
  * @return     The resulting configuration vector (size model.nq)
  
  */
690
691
692
693
694
695
696
697
698
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                      const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
                      const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
  {
    return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, lowerLimits.derived(), upperLimits.derived());
  }

699
  /**
700
   *
701
   * @brief      Generate a configuration vector uniformly sampled among the joint limits of the specified Model.
702
   *
703
704
705
   * @remarks    Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded.
   *
   * @warning    If limits are infinite (no one specified when adding a body or no modification directly in my_model.{lowerPositionLimit,upperPositionLimit},
706
   *             exceptions may be thrown in the joint implementation of uniformlySample
707
   *
708
709
710
   * @param[in]  model   Model of the kinematic tree over which the uniform sampling operation is performed.
   *
   * @return     The resulting configuration vector (size model.nq)
711
   *
712
   */
713
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
gabrielebndn's avatar
gabrielebndn committed
714
  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
715
  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
716

717
  /**
718
   *
719
   * @brief      Generate a configuration vector uniformly sampled among the joint limits of the specified Model.
720
   *
721
   * @remarks    Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded.
722
   *
723
724
   * @warning    If limits are infinite (no one specified when adding a body or no modification directly in my_model.{lowerPositionLimit,upperPositionLimit},
   *             exceptions may be thrown in the joint implementation of uniformlySample
725
   *
726
727
728
   * @param[in]  model   Model of the kinematic tree over which the uniform sampling operation is performed.
   *
   * @return     The resulting configuration vector (size model.nq).
729
   *
730
   */
731
732
733
734
735
736
737
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
  {
    return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
  }

738
  /**
739
   *
740
741
   * @brief         Return the neutral configuration element related to the model configuration space.
   *
742
743
744
   * @param[in]     model      Model of the kinematic tree over which the neutral element is computed.
   *
   * @return        The neutral configuration element (size model.nq).
745
746
   *
   */
747
748
749
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
750

751
752
753
  /**
   * @brief         Return the neutral configuration element related to the model configuration space.
   *
754
   * @param[in]     model      Model of the kinematic tree over which the neutral element is computed.
755
   *
756
   * @return        The neutral configuration element (size model.nq).
757
758
759
760
761
762
763
764
   */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
  {
    return neutral<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
  }

765
  /// \}
766

767
} // namespace pinocchio
768
769

/* --- Details -------------------------------------------------------------------- */
770
#include "pinocchio/algorithm/joint-configuration.hxx"
771

772
#endif // ifndef __pinocchio_joint_configuration_hpp__
773