pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
joint-generic.hpp
1//
2// Copyright (c) 2016-2021 CNRS INRIA
3//
4
5#ifndef __pinocchio_multibody_joint_generic_hpp__
6#define __pinocchio_multibody_joint_generic_hpp__
7
8#include "pinocchio/multibody/joint/joint-collection.hpp"
9#include "pinocchio/multibody/joint/joint-composite.hpp"
10#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
11#include "pinocchio/container/aligned-vector.hpp"
12
13#include <boost/mpl/contains.hpp>
14
15namespace pinocchio
16{
17
18 template<
19 typename Scalar,
20 int Options = context::Options,
21 template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
22 struct JointTpl;
24
25 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
26 struct traits<JointTpl<_Scalar, _Options, JointCollectionTpl>>
27 {
28 enum
29 {
30 Options = _Options,
31 NQ = Eigen::Dynamic, // Dynamic because unknown at compile time
32 NV = Eigen::Dynamic,
33 NVExtended = Eigen::Dynamic
34 };
35
36 typedef _Scalar Scalar;
40
45
46 // [ABA]
47 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> U_t;
48 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> D_t;
49 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> UD_t;
50
56 typedef Motion_t MotionTypeRef;
58 typedef Bias_t BiasTypeRef;
59 typedef U_t UTypeConstRef;
60 typedef U_t UTypeRef;
61 typedef D_t DTypeConstRef;
62 typedef D_t DTypeRef;
63 typedef UD_t UDTypeConstRef;
64 typedef UD_t UDTypeRef;
65
66 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ConfigVector_t;
67 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> TangentVector_t;
68
69 typedef ConfigVector_t ConfigVectorTypeConstRef;
70 typedef ConfigVector_t ConfigVectorTypeRef;
71 typedef TangentVector_t TangentVectorTypeConstRef;
72 typedef TangentVector_t TangentVectorTypeRef;
73
74 typedef boost::mpl::false_ is_mimicable_t;
75 };
76
77 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
78 struct traits<JointDataTpl<_Scalar, _Options, JointCollectionTpl>>
79 {
81 typedef typename traits<JointDerived>::Scalar Scalar;
82 };
83
84 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
85 struct traits<JointModelTpl<_Scalar, _Options, JointCollectionTpl>>
86 {
88 typedef typename traits<JointDerived>::Scalar Scalar;
89 };
90
91 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
93 : public JointDataBase<JointDataTpl<_Scalar, _Options, JointCollectionTpl>>
94 , JointCollectionTpl<_Scalar, _Options>::JointDataVariant
95 {
97
100
101 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
102
104 typedef typename JointCollection::JointDataVariant JointDataVariant;
105
106 using Base::operator==;
107 using Base::operator!=;
108
109 JointDataVariant & toVariant()
110 {
111 return *static_cast<JointDataVariant *>(this);
112 }
113 const JointDataVariant & toVariant() const
114 {
115 return *static_cast<const JointDataVariant *>(this);
116 }
117
118 ConfigVector_t joint_q() const
119 {
120 return pinocchio::joint_q(*this);
121 }
122 TangentVector_t joint_v() const
123 {
124 return pinocchio::joint_v(*this);
125 }
126 Constraint_t S() const
127 {
128 return joint_motin_subspace_xd(*this);
129 }
130 Transformation_t M() const
131 {
132 return joint_transform(*this);
133 }
134 Motion_t v() const
135 {
136 return motion(*this);
137 }
138 Bias_t c() const
139 {
140 return bias(*this);
141 }
142
143 // [ABA CCRBA]
144 U_t U() const
145 {
146 return u_inertia(*this);
147 }
148 D_t Dinv() const
149 {
150 return dinv_inertia(*this);
151 }
152 UD_t UDinv() const
153 {
154 return udinv_inertia(*this);
155 }
156 D_t StU() const
157 {
158 return stu_inertia(*this);
159 }
160
162 : JointDataVariant()
163 {
164 }
165
166 JointDataTpl(const JointDataVariant & jdata_variant)
167 : JointDataVariant(jdata_variant)
168 {
169 }
170
171 template<typename JointDataDerived>
173 : JointCollection::JointDataVariant((JointDataVariant)jdata.derived())
174 {
175 BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types, JointDataDerived>));
176 }
177
178 // Define all the standard accessors
179 ConfigVector_t joint_q_accessor() const
180 {
181 return joint_q();
182 }
183 TangentVector_t joint_v_accessor() const
184 {
185 return joint_v();
186 }
187 Constraint_t S_accessor() const
188 {
189 return S();
190 }
191 Transformation_t M_accessor() const
192 {
193 return M();
194 }
195 Motion_t v_accessor() const
196 {
197 return v();
198 }
199 Bias_t c_accessor() const
200 {
201 return c();
202 }
203 U_t U_accessor() const
204 {
205 return U();
206 }
207 D_t Dinv_accessor() const
208 {
209 return Dinv();
210 }
211 UD_t UDinv_accessor() const
212 {
213 return UDinv();
214 }
215 D_t StU_accessor() const
216 {
217 return StU();
218 }
219
220 static std::string classname()
221 {
222 return "JointData";
223 }
224 std::string shortname() const
225 {
226 return ::pinocchio::shortname(*this);
227 }
228
229 template<typename JointDataDerived>
230 bool isEqual(const JointDataBase<JointDataDerived> & other) const
231 {
232 return ::pinocchio::isEqual(*this, other.derived());
233 }
234
235 bool isEqual(const JointDataTpl & other) const
236 {
237 return Base::isEqual(other) && toVariant() == other.toVariant();
238 }
239
240 bool operator==(const JointDataTpl & other) const
241 {
242 return isEqual(other);
243 }
244
245 bool operator!=(const JointDataTpl & other) const
246 {
247 return !(*this == other);
248 }
249 };
250
251 template<
252 typename NewScalar,
253 typename Scalar,
254 int Options,
255 template<typename S, int O> class JointCollectionTpl>
256 struct CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>
257 {
259 };
260
261 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
263 : JointModelBase<JointModelTpl<_Scalar, _Options, JointCollectionTpl>>
264 , JointCollectionTpl<_Scalar, _Options>::JointModelVariant
265 {
267
269
270 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
271 PINOCCHIO_JOINT_USE_INDEXES(JointModelTpl);
272
274 typedef typename JointCollection::JointDataVariant JointDataVariant;
275 typedef typename JointCollection::JointModelVariant JointModelVariant;
276
277 using Base::id;
278 using Base::setIndexes;
279 using Base::operator==;
280 using Base::operator!=;
281
283 : JointModelVariant()
284 {
285 }
286
287 JointModelTpl(const JointModelVariant & jmodel_variant)
288 : JointCollection::JointModelVariant(jmodel_variant)
289 {
290 }
291
292 const std::vector<bool> hasConfigurationLimit() const
293 {
294 return ::pinocchio::hasConfigurationLimit(*this);
295 }
296
297 const std::vector<bool> hasConfigurationLimitInTangent() const
298 {
299 return ::pinocchio::hasConfigurationLimitInTangent(*this);
300 }
301
302 template<typename JointModelDerived>
304 : JointModelVariant((JointModelVariant)jmodel.derived())
305 {
307 (boost::mpl::contains<typename JointModelVariant::types, JointModelDerived>));
308 }
309
310 JointModelVariant & toVariant()
311 {
312 return *static_cast<JointModelVariant *>(this);
313 }
314
315 const JointModelVariant & toVariant() const
316 {
317 return *static_cast<const JointModelVariant *>(this);
318 }
319
320 JointDataDerived createData() const
321 {
322 return ::pinocchio::createData<Scalar, Options, JointCollectionTpl>(*this);
323 }
324
325 template<typename JointModelDerived>
326 bool isEqual(const JointModelBase<JointModelDerived> & other) const
327 {
328 return ::pinocchio::isEqual(*this, other.derived());
329 }
330
331 template<typename JointModelDerived>
333 {
334 return ::pinocchio::hasSameIndexes(*this, other.derived());
335 }
336
337 bool isEqual(const JointModelTpl & other) const
338 {
339 return Base::isEqual(other) && toVariant() == other.toVariant();
340 }
341
342 bool operator==(const JointModelTpl & other) const
343 {
344 return isEqual(other);
345 }
346
347 bool operator!=(const JointModelTpl & other) const
348 {
349 return !(*this == other);
350 }
351
352 template<typename ConfigVector>
353 void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVector> & q) const
354 {
355 calc_zero_order(*this, data, q.derived());
356 }
357
358 template<typename TangentVector>
359 void calc(
360 JointDataDerived & data, const Blank blank, const Eigen::MatrixBase<TangentVector> & v) const
361 {
362 calc_first_order(*this, data, blank, v.derived());
363 }
364
365 template<typename ConfigVector, typename TangentVector>
366 void calc(
367 JointDataDerived & data,
368 const Eigen::MatrixBase<ConfigVector> & q,
369 const Eigen::MatrixBase<TangentVector> & v) const
370 {
371 calc_first_order(*this, data, q.derived(), v.derived());
372 }
373
374 template<typename VectorLike, typename Matrix6Like>
375 void calc_aba(
376 JointDataDerived & data,
377 const Eigen::MatrixBase<VectorLike> & armature,
378 const Eigen::MatrixBase<Matrix6Like> & I,
379 const bool update_I) const
380 {
382 *this, data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I);
383 }
384
385 /* Acces to dedicated segment in robot config space. */
386 // Const access
387 template<typename D>
388 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
389 JointMappedConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
390 {
391 typedef const Eigen::MatrixBase<D> & InputType;
392 typedef typename SizeDepType<NV>::template SegmentReturn<D>::ConstType ReturnType;
394 typename Visitor::ArgsType arg(a);
395 return Visitor::run(*this, arg);
396 }
397
398 // Non-const access
399 template<typename D>
400 typename SizeDepType<NV>::template SegmentReturn<D>::Type
401 JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a) const
402 {
403 typedef Eigen::MatrixBase<D> & InputType;
404 typedef typename SizeDepType<NV>::template SegmentReturn<D>::Type ReturnType;
406 typename Visitor::ArgsType arg(a);
407 return Visitor::run(*this, arg);
408 }
409
410 std::string shortname() const
411 {
412 return ::pinocchio::shortname(*this);
413 }
414 static std::string classname()
415 {
416 return "JointModel";
417 }
418
419 int nq_impl() const
420 {
421 return ::pinocchio::nq(*this);
422 }
423 int nv_impl() const
424 {
425 return ::pinocchio::nv(*this);
426 }
427 int nvExtended_impl() const
428 {
429 return ::pinocchio::nvExtended(*this);
430 }
431
432 int idx_q_impl() const
433 {
434 return ::pinocchio::idx_q(*this);
435 }
436 int idx_v_impl() const
437 {
438 return ::pinocchio::idx_v(*this);
439 }
440 int idx_vExtended_impl() const
441 {
442 return ::pinocchio::idx_vExtended(*this);
443 }
444
445 JointIndex id_impl() const
446 {
447 return ::pinocchio::id(*this);
448 }
449
450 void setIndexes(JointIndex id, int nq, int nv)
451 {
452 ::pinocchio::setIndexes(*this, id, nq, nv, nv);
453 }
454
455 void setIndexes(JointIndex id, int nq, int nv, int nvExtended)
456 {
458 }
459
461 template<typename NewScalar>
466 };
467
468 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
469 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
470
471 template<
472 typename Scalar,
473 int Options,
474 template<typename S, int O> class JointCollectionTpl,
475 typename JointDataDerived>
476 bool operator==(
477 const JointDataBase<JointDataDerived> & joint_data,
478 const JointDataTpl<Scalar, Options, JointCollectionTpl> & joint_data_generic)
479 {
480 return joint_data_generic == joint_data.derived();
481 }
482
483 template<
484 typename Scalar,
485 int Options,
486 template<typename S, int O> class JointCollectionTpl,
487 typename JointDataDerived>
488 bool operator!=(
489 const JointDataBase<JointDataDerived> & joint_data,
490 const JointDataTpl<Scalar, Options, JointCollectionTpl> & joint_data_generic)
491 {
492 return joint_data_generic != joint_data.derived();
493 }
494
495 template<
496 typename Scalar,
497 int Options,
498 template<typename S, int O> class JointCollectionTpl,
499 typename JointModelDerived>
500 bool operator==(
501 const JointModelBase<JointModelDerived> & joint_model,
502 const JointModelTpl<Scalar, Options, JointCollectionTpl> & joint_model_generic)
503 {
504 return joint_model_generic == joint_model.derived();
505 }
506
507 template<
508 typename Scalar,
509 int Options,
510 template<typename S, int O> class JointCollectionTpl,
511 typename JointModelDerived>
512 bool operator!=(
513 const JointModelBase<JointModelDerived> & joint_model,
514 const JointModelTpl<Scalar, Options, JointCollectionTpl> & joint_model_generic)
515 {
516 return joint_model_generic != joint_model.derived();
517 }
518
519} // namespace pinocchio
520
521#endif // ifndef __pinocchio_multibody_joint_generic_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model.
SE3Tpl< Scalar, Options > joint_transform(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointTransformVisitor to get the joint internal transform (transform bet...
MotionTpl< Scalar, Options > bias(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointBiasVisitor to get the joint bias as a dense motion.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
JointMotionSubspaceTpl< Eigen::Dynamic, Scalar, Options > joint_motin_subspace_xd(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConstraintVisitor to get the joint constraint as a dense constr...
JointDataTpl< Scalar, Options, JointCollectionTpl >::TangentVector_t joint_v(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint velocity vector.
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > dinv_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix d...
MotionTpl< Scalar, Options > motion(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointMotionVisitor to get the joint internal motion as a dense motion.
void calc_zero_order(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< ConfigVectorType > &q)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcZeroOrderVisitor to compute...
const std::vector< bool > hasConfigurationLimitInTangent(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointConfigurationLimitInTangentVisitor to get the configurations limit...
void calc_aba(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< VectorLike > &armature, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcAbaVisitor to.
bool hasSameIndexes(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel_generic, const JointModelBase< JointModelDerived > &jmodel)
Check whether JointModelTpl<Scalar,...> has the indexes than another JointModelDerived.
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v, int vExtended)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
const std::vector< bool > hasConfigurationLimit(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointConfigurationLimitVisitor to get the configurations limits.
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > udinv_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix de...
JointDataTpl< Scalar, Options, JointCollectionTpl >::ConfigVector_t joint_q(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint configuration vector.
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > u_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointUInertiaVisitor to get the U matrix of the inertia matrix decomposi...
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > stu_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointStUInertiaVisitor to get St*I*S matrix of the inertia matrix decomp...
void calc_first_order(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcFirstOrderVisitor to comput...
int nvExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvExtendVisitor to get the dimension of the joint extended tangent...
Blank type.
Definition fwd.hpp:77
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition fwd.hpp:99
JointModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72