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

5
6
#ifndef __pinocchio_joint_configuration_hxx__
#define __pinocchio_joint_configuration_hxx__
7
8

#include "pinocchio/multibody/visitor.hpp"
9
#include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
10
11

/* --- Details -------------------------------------------------------------------- */
12
namespace pinocchio
13
14
{

15
16
  // --------------- API with return value as argument ---------------------- //

17
18
19
20
21
22
23
  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)
  {
Justin Carpentier's avatar
Justin Carpentier committed
24
25
26
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(qout.size() == model.nq, "The output argument is not of the right size");
27

28
29
30
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
    ReturnType & res = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout);
31

32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
    typedef IntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,ReturnType> Algo;
    typename Algo::ArgsType args(q.derived(),v.derived(),res);
    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
    {
      Algo::run(model.joints[i], args);
    }
  }

  template<typename LieGroup_t, 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)
  {
Justin Carpentier's avatar
Justin Carpentier committed
48
49
50
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q0.size() == model.nq, "The first configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q1.size() == model.nq, "The second configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(qout.size() == model.nq, "The output argument is not of the right size");
51

52
53
54
55
56
57
58
59
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
    ReturnType & res = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout);

    typedef InterpolateStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar,ReturnType> Algo;
    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
    {
      Algo::run(model.joints[i],
60
                typename Algo::ArgsType(q0.derived(), q1.derived(), u, res.derived()));
61
62
63
64
65
66
67
68
    }
  }

  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,
69
             const Eigen::MatrixBase<ReturnType> & dvout)
70
  {
Justin Carpentier's avatar
Justin Carpentier committed
71
72
73
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q0.size() == model.nq, "The first configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q1.size() == model.nq, "The second configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(dvout.size() == model.nv, "The output argument is not of the right size");
74

75
76
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
77
    ReturnType & res = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, dvout);
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93

    typedef DifferenceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,ReturnType> Algo;
    typename Algo::ArgsType args(q0.derived(),q1.derived(),res);
    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
    {
      Algo::run(model.joints[i], args);
    }
  }

  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)
  {
Justin Carpentier's avatar
Justin Carpentier committed
94
95
96
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q0.size() == model.nq, "The first configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q1.size() == model.nq, "The second configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(out.size() == (model.njoints-1), "The output argument is not of the right size");
97

98
99
100
101
102
103
104
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
    ReturnType & distances = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, out);

    typedef SquaredDistanceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,ReturnType> Algo;
    for(JointIndex i=0; i<(JointIndex) model.njoints-1; ++i)
    {
105
      typename Algo::ArgsType args(i,q0.derived(),q1.derived(), distances.derived());
106
107
108
109
110
111
112
113
114
115
116
      Algo::run(model.joints[i+1], args);
    }
  }

  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)
  {
Justin Carpentier's avatar
Justin Carpentier committed
117
118
119
    PINOCCHIO_CHECK_INPUT_ARGUMENT(lowerLimits.size() == model.nq, "The lower limits vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(upperLimits.size() == model.nq, "The upper limits vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(qout.size() == model.nq, "The output argument is not of the right size");
120

121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
    ReturnType & q = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout);

    typedef RandomConfigurationStep<LieGroup_t,ReturnType,ConfigVectorIn1,ConfigVectorIn2> Algo;
    typename Algo::ArgsType args(PINOCCHIO_EIGEN_CONST_CAST(ReturnType,q), lowerLimits.derived(), upperLimits.derived());
    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
    {
      Algo::run(model.joints[i], args);
    }
  }

  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType>
  void
  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, const Eigen::MatrixBase<ReturnType> & qout)
  {
Justin Carpentier's avatar
Justin Carpentier committed
137
    PINOCCHIO_CHECK_INPUT_ARGUMENT(qout.size() == model.nq, "The output argument is not of the right size");
138

139
140
141
142
143
144
145
146
147
148
149
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
    ReturnType & neutral_elt = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout);
    
    typename NeutralStep<LieGroup_t,ReturnType>::ArgsType args(neutral_elt.derived());
    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i )
    {
      NeutralStep<LieGroup_t,ReturnType>::run(model.joints[i],args);
    }
  }

150
151
152
153
154
155
  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)
156
  {
Justin Carpentier's avatar
Justin Carpentier committed
157
158
159
160
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(J.rows() == model.nv, "The output argument is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(J.cols() == model.nv, "The output argument is not of the right size");
161

162
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
163
164
    typedef typename Model::JointIndex JointIndex;
    
165
166
    typedef dIntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo;
    typename Algo::ArgsType args(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
167
    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
168
    {
169
      Algo::run(model.joints[i], args);
170
171
    }
  }
Gabriele Buondonno's avatar
Gabriele Buondonno committed
172
173
174

  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
  Scalar
175
176
177
  squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                     const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                     const Eigen::MatrixBase<ConfigVectorIn2> & q1)
Gabriele Buondonno's avatar
Gabriele Buondonno committed
178
  {
Justin Carpentier's avatar
Justin Carpentier committed
179
180
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q0.size() == model.nq, "The first configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q1.size() == model.nq, "The second configuration vector is not of the right size");
181

Gabriele Buondonno's avatar
Gabriele Buondonno committed
182
183
184
185
186
187
188
189
190
191
192
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
    typename ConfigVectorIn1::Scalar squaredDistance = 0.0;

    typedef SquaredDistanceSumStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar> Algo;
    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
    {
      typename Algo::ArgsType args(q0.derived(),q1.derived(), squaredDistance);
      Algo::run(model.joints[i], args);
    }
    
193
194
195
196
197
198
199
200
201
202
    return squaredDistance;
  }

  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)
  {
    const Scalar & squaredDistance = squaredDistanceSum<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
Gabriele Buondonno's avatar
Gabriele Buondonno committed
203
204
205
    return math::sqrt(squaredDistance);
  }

206
207
208
  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)
209
  {
Justin Carpentier's avatar
Justin Carpentier committed
210
    PINOCCHIO_CHECK_INPUT_ARGUMENT(qout.size() == model.nq, "The output argument is not of the right size");
211

212
213
214
215
216
217
218
219
220
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
    
    typedef NormalizeStep<LieGroup_t,ConfigVectorType> Algo;
    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
    {
      Algo::run(model.joints[i],
                typename Algo::ArgsType(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout)));
    }
221
  }
222
223
224
225
226
227
228

  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)
229
  {
Justin Carpentier's avatar
Justin Carpentier committed
230
231
232
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q1.size() == model.nq, "The first configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q2.size() == model.nq, "The second configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(prec >= 0, "The precision is negative");
233

234
235
236
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;
    
237
238
239
240
    bool result = true;
    typedef IsSameConfigurationStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar> Algo;
    typename Algo::ArgsType args(result,q1.derived(),q2.derived(),prec);
    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
241
242
    {
      Algo::run(model.joints[i], args);
243
244
      if(!result)
        return false;
245
    }
246
247
    
    return true;
248
  }
249

250
251
252
253
254
  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)
255
  {
Justin Carpentier's avatar
Justin Carpentier committed
256
257
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of the right size");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(jacobian.rows() == model.nq && jacobian.cols() == model.nv
258
           && "The jacobian does not have the right dimension");
259

260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
    typedef IntegrateCoeffWiseJacobianStep<LieGroup_t,ConfigVector,JacobianMatrix> Algo;
    typename Algo::ArgsType args(q.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian));
    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
    {
      Algo::run(model.joints[i],args);
    }
  }

  // ----------------- API that allocates memory ---------------------------- //

  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)
  {
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) ReturnType;
    ReturnType res(model.nq);
278
    integrate<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,ReturnType>(model, q.derived(), v.derived(), res);
279
    return res;
280
281
  }

282
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
gabrielebndn's avatar
gabrielebndn committed
283
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
284
  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
285
286
287
              const Eigen::MatrixBase<ConfigVectorIn1> & q0,
              const Eigen::MatrixBase<ConfigVectorIn2> & q1,
              const Scalar & u)
288
  {
gabrielebndn's avatar
gabrielebndn committed
289
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType;
290
    ReturnType res(model.nq);
291
    interpolate<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, q0.derived(), q1.derived(), u, res);
292
    return res;
293
294
  }

295
  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
gabrielebndn's avatar
gabrielebndn committed
296
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
297
  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
298
299
             const Eigen::MatrixBase<ConfigVectorIn1> & q0,
             const Eigen::MatrixBase<ConfigVectorIn2> & q1)
300
  {
gabrielebndn's avatar
gabrielebndn committed
301
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType;
302
    ReturnType res(model.nv);
303
    difference<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),res);
304
    return res;
305
306
  }

307
  template<typename LieGroup_t,typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
gabrielebndn's avatar
gabrielebndn committed
308
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
309
  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
310
311
                  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
312
  {
Gabriele Buondonno's avatar
Gabriele Buondonno committed
313
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType;
314
    ReturnType distances(ReturnType::Zero(model.njoints-1));
315
    squaredDistance<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),distances);    
316
317
    return distances;
  }
318

319
  template<typename LieGroup_t,typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
gabrielebndn's avatar
gabrielebndn committed
320
  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
321
  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
322
323
                      const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
                      const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
324
  {
325
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType)) ReturnType; 
326
    ReturnType q(model.nq);
327
    randomConfiguration<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, lowerLimits.derived(), upperLimits.derived(), q);
328
329
330
    return q;
  }

331
  template<typename LieGroup_t,typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
gabrielebndn's avatar
gabrielebndn committed
332
  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
333
  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
334
  {
335
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
336
    typedef typename Model::ConfigVectorType ConfigVectorType;
337
    return randomConfiguration<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorType,ConfigVectorType>(model, model.lowerPositionLimit, model.upperPositionLimit);
338
  }
339

340
341
342
  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)
343
  {
344
345
346
347
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ReturnType;    
    ReturnType q(model.nq);
    neutral<LieGroup_t,Scalar,Options,JointCollectionTpl,ReturnType>(model,q);
    return q;
348
  }
349

350
} // namespace pinocchio
351

352
#endif // ifndef __pinocchio_joint_configuration_hxx__
353