joint-free-flyer.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_joint_free_flyer_hpp__
7 #define __pinocchio_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/constraint.hpp"
14 #include "pinocchio/math/fwd.hpp"
15 #include "pinocchio/math/quaternion.hpp"
16 
17 namespace pinocchio
18 {
19 
20  template<typename Scalar, int Options> struct ConstraintIdentityTpl;
21 
22  template<typename _Scalar, int _Options>
23  struct traits< ConstraintIdentityTpl<_Scalar,_Options> >
24  {
25  typedef _Scalar Scalar;
26  enum { Options = _Options };
27  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
28  enum {
29  LINEAR = 0,
30  ANGULAR = 3
31  };
33  typedef Eigen::Matrix<Scalar,6,1,Options> JointForce;
34  typedef Eigen::Matrix<Scalar,6,6,Options> DenseBase;
35  typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType;
36  typedef typename Matrix6::IdentityReturnType MatrixReturnType;
37  }; // traits ConstraintRevolute
38 
39 
40  template<typename _Scalar, int _Options>
42  : ConstraintBase< ConstraintIdentityTpl<_Scalar,_Options> >
43  {
44  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46 
47  enum { NV = 6 };
48 
49  template<typename Vector6Like>
50  JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const
51  {
52  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
53  return JointMotion(vj);
54  }
55 
56  template<typename S1, int O1>
58  se3Action(const SE3Tpl<S1,O1> & m) const
59  {
60  return m.toActionMatrix();
61 
62  }
63 
64  template<typename S1, int O1>
67  {
68  return m.toActionMatrixInverse();
69  }
70 
71  int nv_impl() const { return NV; }
72 
74  {
75  template<typename Derived>
78  { return phi.toVector(); }
79 
80  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
81  template<typename MatrixDerived>
82  typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
83  operator*(const Eigen::MatrixBase<MatrixDerived> & F)
84  {
85  return F.derived();
86  }
87  };
88 
89  TransposeConst transpose() const { return TransposeConst(); }
90  MatrixReturnType matrix_impl() const { return DenseBase::Identity(); }
91 
92  template<typename MotionDerived>
93  typename MotionDerived::ActionMatrixType
95  { return v.toActionMatrix(); }
96 
97  bool isEqual(const ConstraintIdentityTpl &) const { return true; }
98 
99  }; // struct ConstraintIdentityTpl
100 
101  template<typename Scalar, int Options, typename Vector6Like>
104  const Eigen::MatrixBase<Vector6Like> & v)
105  {
106  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
107 // typedef typename ConstraintIdentityTpl<Scalar,Options>::Motion Motion;
109  return Motion(v.derived());
110  }
111 
112  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
113  template<typename S1, int O1, typename S2, int O2>
114  inline typename InertiaTpl<S1,O1>::Matrix6
116  {
117  return Y.matrix();
118  }
119 
120  /* [ABA] Y*S operator*/
121  template<typename Matrix6Like, typename S2, int O2>
122  inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like)
123  operator*(const Eigen::MatrixBase<Matrix6Like> & Y, const ConstraintIdentityTpl<S2,O2> &)
124  {
125  return Y.derived();
126  }
127 
128  template<typename S1, int O1>
131 
132  template<typename S1, int O1, typename MotionDerived>
133  struct MotionAlgebraAction< ConstraintIdentityTpl<S1,O1>,MotionDerived >
135 
136  template<typename Scalar, int Options> struct JointFreeFlyerTpl;
137 
138  template<typename _Scalar, int _Options>
139  struct traits< JointFreeFlyerTpl<_Scalar,_Options> >
140  {
141  enum {
142  NQ = 7,
143  NV = 6
144  };
145  typedef _Scalar Scalar;
146  enum { Options = _Options };
153 
154  // [ABA]
155  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
156  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
157  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
158 
160 
161  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
162  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
163  };
164 
165  template<typename Scalar, int Options>
168 
169  template<typename Scalar, int Options>
172 
173  template<typename _Scalar, int _Options>
174  struct JointDataFreeFlyerTpl : public JointDataBase< JointDataFreeFlyerTpl<_Scalar,_Options> >
175  {
176  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
180 
181  Constraint_t S;
182  Transformation_t M;
183  Motion_t v;
184  Bias_t c;
185 
186  // [ABA] specific data
187  U_t U;
188  D_t Dinv;
189  UD_t UDinv;
190 
192  : M(Transformation_t::Identity())
193  , v(Motion_t::Zero())
194  , U(U_t::Zero())
195  , Dinv(D_t::Zero())
196  , UDinv(UD_t::Identity())
197  {}
198 
199  static std::string classname() { return std::string("JointDataFreeFlyer"); }
200  std::string shortname() const { return classname(); }
201 
202  }; // struct JointDataFreeFlyerTpl
203 
205  template<typename _Scalar, int _Options>
207  : public JointModelBase< JointModelFreeFlyerTpl<_Scalar,_Options> >
208  {
209  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
211  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
212 
214  using Base::id;
215  using Base::idx_q;
216  using Base::idx_v;
217  using Base::setIndexes;
218 
219  JointDataDerived createData() const { return JointDataDerived(); }
220 
221  const std::vector<bool> hasConfigurationLimit() const
222  {
223  return {true, true, true, false, false, false, false};
224  }
225 
226  const std::vector<bool> hasConfigurationLimitInTangent() const
227  {
228  return {true, true, true, false, false, false};
229  }
230 
231  template<typename ConfigVectorLike>
232  inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
233  {
234  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
235  typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
236  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
237 
238  ConstQuaternionMap quat(q_joint.template tail<4>().data());
239  //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
240  assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4);
241 
242  M.rotation(quat.matrix());
243  M.translation(q_joint.template head<3>());
244  }
245 
246  template<typename Vector3Derived, typename QuaternionDerived>
247  EIGEN_DONT_INLINE
248  void calc(JointDataDerived & data,
249  const typename Eigen::MatrixBase<Vector3Derived> & trans,
250  const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
251  {
252  data.M.translation(trans);
253  data.M.rotation(quat.matrix());
254  }
255 
256  template<typename ConfigVector>
257  EIGEN_DONT_INLINE
258  void calc(JointDataDerived & data,
259  const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
260  {
261  typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
262  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
263 
264  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
265  ConstQuaternionMap quat(q.template tail<4>().data());
266 
267  calc(data,q.template head<3>(),quat);
268  }
269 
270  template<typename ConfigVector>
271  EIGEN_DONT_INLINE
272  void calc(JointDataDerived & data,
273  const typename Eigen::MatrixBase<ConfigVector> & qs) const
274  {
275  typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
276 
277  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
278  const Quaternion quat(q.template tail<4>());
279 
280  calc(data,q.template head<3>(),quat);
281  }
282 
283  template<typename ConfigVector, typename TangentVector>
284  EIGEN_DONT_INLINE
285  void calc(JointDataDerived & data,
286  const typename Eigen::MatrixBase<ConfigVector> & qs,
287  const typename Eigen::MatrixBase<TangentVector> & vs) const
288  {
289  calc(data,qs.derived());
290 
291  data.v = vs.template segment<NV>(idx_v());
292  }
293 
294  template<typename Matrix6Like>
295  void calc_aba(JointDataDerived & data,
296  const Eigen::MatrixBase<Matrix6Like> & I,
297  const bool update_I) const
298  {
299  data.U = I;
300 
301  // compute inverse
302 // data.Dinv.setIdentity();
303 // I.llt().solveInPlace(data.Dinv);
305 
306  if (update_I)
307  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero();
308  }
309 
310  static std::string classname() { return std::string("JointModelFreeFlyer"); }
311  std::string shortname() const { return classname(); }
312 
314  template<typename NewScalar>
316  {
318  ReturnType res;
319  res.setIndexes(id(),idx_q(),idx_v());
320  return res;
321  }
322 
323  }; // struct JointModelFreeFlyerTpl
324 
325 } // namespace pinocchio
326 
327 #include <boost/type_traits.hpp>
328 
329 namespace boost
330 {
331  template<typename Scalar, int Options>
332  struct has_nothrow_constructor< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
333  : public integral_constant<bool,true> {};
334 
335  template<typename Scalar, int Options>
336  struct has_nothrow_copy< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
337  : public integral_constant<bool,true> {};
338 
339  template<typename Scalar, int Options>
340  struct has_nothrow_constructor< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
341  : public integral_constant<bool,true> {};
342 
343  template<typename Scalar, int Options>
344  struct has_nothrow_copy< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
345  : public integral_constant<bool,true> {};
346 }
347 
348 #endif // ifndef __pinocchio_joint_free_flyer_hpp__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
int NQ
Definition: dpendulum.py:8
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model tangent space corre...
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:81
Return type of the ation of a Motion onto an object of type D.
JointDataFreeFlyerTpl< Scalar, Options > JointDataDerived
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
JointDataDerived createData() const
MatrixReturnType matrix_impl() const
const std::vector< bool > hasConfigurationLimit() const
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::PlainObjectBase< ConfigVector > &qs) const
PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived) operator*(const Eigen
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointFreeFlyerTpl< _Scalar, _Options > JointDerived
JointModelFreeFlyerTpl< NewScalar, Options > cast() const
ConstraintIdentityTpl< Scalar, Options > Constraint_t
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
TransposeConst transpose() const
SE3::Scalar Scalar
Definition: conversions.cpp:13
ActionMatrixType toActionMatrix() const
The action matrix of .
Definition: se3-base.hpp:60
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointFreeFlyerTpl< _Scalar, _Options > JointDerived
SE3Tpl< S1, O1 >::ActionMatrixType se3Action(const SE3Tpl< S1, O1 > &m) const
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
bool isEqual(const ConstraintIdentityTpl &) const
MotionDerived::ActionMatrixType motionAction(const MotionBase< MotionDerived > &v) const
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
MotionTpl< double, 0 > Motion
static EIGEN_STRONG_INLINE void run(const Eigen::MatrixBase< M1 > &StYS, const Eigen::MatrixBase< M2 > &Dinv)
SE3Tpl< S1, O1 >::ActionMatrixType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
quat
JointModelBase< JointModelFreeFlyerTpl > Base
Main pinocchio namespace.
Definition: timings.cpp:28
res
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< Vector3Derived > &trans, const typename Eigen::QuaternionBase< QuaternionDerived > &quat) const
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Constraint_t S
void forwardKinematics(Transformation_t &M, const Eigen::MatrixBase< ConfigVectorLike > &q_joint) const
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
ActionMatrixType toActionMatrixInverse() const
The action matrix of .
Definition: se3-base.hpp:70
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
JointMotion __mult__(const Eigen::MatrixBase< Vector6Like > &vj) const
NV
Definition: dcrba.py:444
ActionMatrixType toActionMatrix() const
Definition: motion-base.hpp:41
const std::vector< bool > hasConfigurationLimitInTangent() const
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 .
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
JointModelFreeFlyerTpl< Scalar, Options > JointModelDerived
ForceDense< Derived >::ToVectorConstReturnType operator*(const ForceDense< Derived > &phi)
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30