pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
joint-free-flyer.hpp
1//
2// Copyright (c) 2015-2020 CNRS INRIA
3// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4//
5
6#ifndef __pinocchio_multibody_joint_free_flyer_hpp__
7#define __pinocchio_multibody_joint_free_flyer_hpp__
8
9#include "pinocchio/macros.hpp"
10#include "pinocchio/spatial/inertia.hpp"
11#include "pinocchio/spatial/explog.hpp"
12#include "pinocchio/multibody/joint/joint-base.hpp"
13#include "pinocchio/multibody/joint-motion-subspace.hpp"
14#include "pinocchio/math/fwd.hpp"
15#include "pinocchio/math/quaternion.hpp"
16
17namespace pinocchio
18{
19
20 template<typename Scalar, int Options>
21 struct JointMotionSubspaceIdentityTpl;
22
23 template<typename _Scalar, int _Options>
25 {
26 typedef _Scalar Scalar;
27 enum
28 {
29 Options = _Options
30 };
31 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
32 enum
33 {
34 LINEAR = 0,
35 ANGULAR = 3
36 };
38 typedef Eigen::Matrix<Scalar, 6, 1, Options> JointForce;
39 typedef Eigen::Matrix<Scalar, 6, 6, Options> DenseBase;
40 typedef Eigen::Matrix<Scalar, 6, 6, Options> ReducedSquaredMatrix;
41
42 typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType;
43 typedef typename Matrix6::IdentityReturnType MatrixReturnType;
44 typedef typename Matrix6::IdentityReturnType StDiagonalMatrixSOperationReturnType;
45 }; // traits ConstraintRevolute
46
47 template<typename _Scalar, int _Options>
49 : JointMotionSubspaceBase<JointMotionSubspaceIdentityTpl<_Scalar, _Options>>
50 {
52 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceIdentityTpl)
53
54 enum
55 {
56 NV = 6
57 };
58
59 template<typename Vector6Like>
60 JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const
61 {
63 return JointMotion(vj);
64 }
65
66 template<typename S1, int O1>
67 typename SE3Tpl<S1, O1>::ActionMatrixType se3Action(const SE3Tpl<S1, O1> & m) const
68 {
69 return m.toActionMatrix();
70 }
71
72 template<typename S1, int O1>
73 typename SE3Tpl<S1, O1>::ActionMatrixType se3ActionInverse(const SE3Tpl<S1, O1> & m) const
74 {
75 return m.toActionMatrixInverse();
76 }
77
78 int nv_impl() const
79 {
80 return NV;
81 }
82
83 struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceIdentityTpl>
84 {
85 template<typename Derived>
87 operator*(const ForceDense<Derived> & phi)
88 {
89 return phi.toVector();
90 }
91
92 /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
93 template<typename MatrixDerived>
94 typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
95 operator*(const Eigen::MatrixBase<MatrixDerived> & F)
96 {
97 return F.derived();
98 }
99 };
100
101 TransposeConst transpose() const
102 {
103 return TransposeConst();
104 }
105 MatrixReturnType matrix_impl() const
106 {
107 return DenseBase::Identity();
108 }
109
110 template<typename MotionDerived>
111 typename MotionDerived::ActionMatrixType motionAction(const MotionBase<MotionDerived> & v) const
112 {
113 return v.toActionMatrix();
114 }
115
116 bool isEqual(const JointMotionSubspaceIdentityTpl &) const
117 {
118 return true;
119 }
120
121 }; // struct JointMotionSubspaceIdentityTpl
122
123 template<typename Scalar, int Options, typename Vector6Like>
124 MotionRef<const Vector6Like> operator*(
125 const JointMotionSubspaceIdentityTpl<Scalar, Options> &,
126 const Eigen::MatrixBase<Vector6Like> & v)
127 {
128 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6);
129 // typedef typename JointMotionSubspaceIdentityTpl<Scalar,Options>::Motion Motion;
130 typedef MotionRef<const Vector6Like> Motion;
131 return Motion(v.derived());
132 }
133
134 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
135 template<typename S1, int O1, typename S2, int O2>
136 inline typename InertiaTpl<S1, O1>::Matrix6
137 operator*(const InertiaTpl<S1, O1> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &)
138 {
139 return Y.matrix();
140 }
141
142 /* [ABA] Y*S operator*/
143 template<typename Matrix6Like, typename S2, int O2>
144 inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(
145 const Eigen::MatrixBase<Matrix6Like> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &)
146 {
147 return Y.derived();
148 }
149
150 template<typename S1, int O1>
152 {
153 typedef typename SE3Tpl<S1, O1>::ActionMatrixType ReturnType;
154 };
155
156 template<typename S1, int O1, typename MotionDerived>
161
162 template<typename Scalar, int Options>
164
165 template<typename _Scalar, int _Options>
167 {
168 enum
169 {
170 NQ = 7,
171 NV = 6,
172 NVExtended = 6
173 };
174 typedef _Scalar Scalar;
175 enum
176 {
177 Options = _Options
178 };
185
186 // [ABA]
187 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
188 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
189 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
190
191 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
192 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
193
194 typedef boost::mpl::false_ is_mimicable_t;
195
196 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
197 };
198
199 template<typename _Scalar, int _Options>
205
206 template<typename _Scalar, int _Options>
212
213 template<typename _Scalar, int _Options>
214 struct JointDataFreeFlyerTpl : public JointDataBase<JointDataFreeFlyerTpl<_Scalar, _Options>>
215 {
218 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
219 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
220
221 ConfigVector_t joint_q;
222 TangentVector_t joint_v;
223
224 Constraint_t S;
225 Transformation_t M;
226 Motion_t v;
227 Bias_t c;
228
229 // [ABA] specific data
230 U_t U;
231 D_t Dinv;
232 UD_t UDinv;
233 D_t StU;
234
236 : joint_q(ConfigVector_t::Zero())
237 , joint_v(TangentVector_t::Zero())
238 , M(Transformation_t::Identity())
239 , v(Motion_t::Zero())
240 , U(U_t::Zero())
241 , Dinv(D_t::Zero())
242 , UDinv(UD_t::Identity())
243 , StU(D_t::Zero())
244 {
245 joint_q[6] = Scalar(1);
246 }
247
248 static std::string classname()
249 {
250 return std::string("JointDataFreeFlyer");
251 }
252 std::string shortname() const
253 {
254 return classname();
255 }
256
257 }; // struct JointDataFreeFlyerTpl
258
283 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl);
284 template<typename _Scalar, int _Options>
285 struct JointModelFreeFlyerTpl : public JointModelBase<JointModelFreeFlyerTpl<_Scalar, _Options>>
286 {
289 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
290
292 using Base::id;
293 using Base::idx_q;
294 using Base::idx_v;
295 using Base::idx_vExtended;
296 using Base::setIndexes;
297
298 JointDataDerived createData() const
299 {
300 return JointDataDerived();
301 }
302
303 const std::vector<bool> hasConfigurationLimit() const
304 {
305 return {true, true, true, false, false, false, false};
306 }
307
308 const std::vector<bool> hasConfigurationLimitInTangent() const
309 {
310 return {true, true, true, false, false, false};
311 }
312
313 template<typename ConfigVectorLike>
314 inline void forwardKinematics(
315 Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
316 {
318 typedef typename Eigen::Quaternion<
319 typename ConfigVectorLike::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options>
320 Quaternion;
321 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
322
323 ConstQuaternionMap quat(q_joint.template tail<4>().data());
324 // assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename
325 // V::Scalar>::epsilon())); TODO: check validity of the rhs precision
326 assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm() - 1)) <= 1e-4);
327
328 M.rotation(quat.matrix());
329 M.translation(q_joint.template head<3>());
330 }
331
332 template<typename Vector3Derived, typename QuaternionDerived>
333 PINOCCHIO_DONT_INLINE void calc(
334 JointDataDerived & data,
335 const typename Eigen::MatrixBase<Vector3Derived> & trans,
336 const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
337 {
338 data.M.translation(trans);
339 data.M.rotation(quat.matrix());
340 }
341
342 template<typename ConfigVector>
343 PINOCCHIO_DONT_INLINE void
344 calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
345 {
346 typedef typename Eigen::Quaternion<Scalar, Options> Quaternion;
347 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
348
349 data.joint_q = qs.template segment<NQ>(idx_q());
350 ConstQuaternionMap quat(data.joint_q.template tail<4>().data());
351
352 calc(data, data.joint_q.template head<3>(), quat);
353 }
354
355 template<typename TangentVector>
356 PINOCCHIO_DONT_INLINE void
357 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
358 const
359 {
360 data.joint_v = vs.template segment<NV>(idx_v());
361 data.v = data.joint_v;
362 }
363
364 template<typename ConfigVector, typename TangentVector>
365 PINOCCHIO_DONT_INLINE void calc(
366 JointDataDerived & data,
367 const typename Eigen::MatrixBase<ConfigVector> & qs,
368 const typename Eigen::MatrixBase<TangentVector> & vs) const
369 {
370 calc(data, qs.derived());
371
372 data.joint_v = vs.template segment<NV>(idx_v());
373 data.v = data.joint_v;
374 }
375
376 template<typename VectorLike, typename Matrix6Like>
377 void calc_aba(
378 JointDataDerived & data,
379 const Eigen::MatrixBase<VectorLike> & armature,
380 const Eigen::MatrixBase<Matrix6Like> & I,
381 const bool update_I) const
382 {
383 data.U = I;
384 data.StU = I;
385 data.StU.diagonal() += armature;
386
387 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
388 data.UDinv.noalias() = I * data.Dinv;
389
390 if (update_I)
391 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
392 }
393
394 static std::string classname()
395 {
396 return std::string("JointModelFreeFlyer");
397 }
398 std::string shortname() const
399 {
400 return classname();
401 }
402
404 template<typename NewScalar>
406 {
408 ReturnType res;
409 res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
410 return res;
411 }
412
413 }; // struct JointModelFreeFlyerTpl
414
415} // namespace pinocchio
416
417#include <boost/type_traits.hpp>
418
419namespace boost
420{
421 template<typename Scalar, int Options>
422 struct has_nothrow_constructor<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>>
423 : public integral_constant<bool, true>
424 {
425 };
426
427 template<typename Scalar, int Options>
428 struct has_nothrow_copy<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>>
429 : public integral_constant<bool, true>
430 {
431 };
432
433 template<typename Scalar, int Options>
434 struct has_nothrow_constructor<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>>
435 : public integral_constant<bool, true>
436 {
437 };
438
439 template<typename Scalar, int Options>
440 struct has_nothrow_copy<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>>
441 : public integral_constant<bool, true>
442 {
443 };
444} // namespace boost
445
446#endif // ifndef __pinocchio_multibody_joint_free_flyer_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
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.
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.
Blank type.
Definition fwd.hpp:77
JointModelFreeFlyerTpl< NewScalar, Options > cast() const
Return type of the ation of a Motion onto an object of type D.
Definition motion.hpp:46
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72