pinocchio  2.2.1-dirty
joint-free-flyer.hpp
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
45  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintIdentityTpl)
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>
66  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
67  {
68  return m.toActionMatrixInverse();
69  }
70 
71  int nv_impl() const { return NV; }
72 
74  {
75  template<typename Derived>
77  operator*(const ForceDense<Derived> & phi)
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
94  motionAction(const MotionBase<MotionDerived> & v) const
95  { return v.toActionMatrix(); }
96 
97  }; // struct ConstraintIdentityTpl
98 
99  template<typename Scalar, int Options, typename Vector6Like>
102  const Eigen::MatrixBase<Vector6Like> & v)
103  {
104  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
105 // typedef typename ConstraintIdentityTpl<Scalar,Options>::Motion Motion;
107  return Motion(v.derived());
108  }
109 
110  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
111  template<typename S1, int O1, typename S2, int O2>
112  inline typename InertiaTpl<S1,O1>::Matrix6
114  {
115  return Y.matrix();
116  }
117 
118  /* [ABA] Y*S operator*/
119  template<typename Matrix6Like, typename S2, int O2>
120  inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like)
121  operator*(const Eigen::MatrixBase<Matrix6Like> & Y, const ConstraintIdentityTpl<S2,O2> &)
122  {
123  return Y.derived();
124  }
125 
126  template<typename S1, int O1>
128  { typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };
129 
130  template<typename S1, int O1, typename MotionDerived>
131  struct MotionAlgebraAction< ConstraintIdentityTpl<S1,O1>,MotionDerived >
132  { typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };
133 
134  template<typename Scalar, int Options> struct JointFreeFlyerTpl;
135 
136  template<typename _Scalar, int _Options>
137  struct traits< JointFreeFlyerTpl<_Scalar,_Options> >
138  {
139  enum {
140  NQ = 7,
141  NV = 6
142  };
143  typedef _Scalar Scalar;
144  enum { Options = _Options };
151 
152  // [ABA]
153  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
154  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
155  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
156 
157  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
158 
159  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
160  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
161  };
162 
163  template<typename Scalar, int Options>
164  struct traits< JointDataFreeFlyerTpl<Scalar,Options> >
166 
167  template<typename Scalar, int Options>
168  struct traits< JointModelFreeFlyerTpl<Scalar,Options> >
170 
171  template<typename _Scalar, int _Options>
172  struct JointDataFreeFlyerTpl : public JointDataBase< JointDataFreeFlyerTpl<_Scalar,_Options> >
173  {
174  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
176  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
177  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
178 
179  Constraint_t S;
180  Transformation_t M;
181  Motion_t v;
182  Bias_t c;
183 
184  // [ABA] specific data
185  U_t U;
186  D_t Dinv;
187  UD_t UDinv;
188 
189  JointDataFreeFlyerTpl() : M(1), U(), Dinv(), UDinv(UD_t::Identity()) {}
190 
191  static std::string classname() { return std::string("JointDataFreeFlyer"); }
192  std::string shortname() const { return classname(); }
193 
194  }; // struct JointDataFreeFlyerTpl
195 
196  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl);
197  template<typename _Scalar, int _Options>
199  : public JointModelBase< JointModelFreeFlyerTpl<_Scalar,_Options> >
200  {
201  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
203  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
204 
206  using Base::id;
207  using Base::idx_q;
208  using Base::idx_v;
209  using Base::setIndexes;
210 
211  JointDataDerived createData() const { return JointDataDerived(); }
212 
213  template<typename ConfigVectorLike>
214  inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
215  {
216  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
217  typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
218  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
219 
220  ConstQuaternionMap quat(q_joint.template tail<4>().data());
221  //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
222  assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
223 
224  M.rotation(quat.matrix());
225  M.translation(q_joint.template head<3>());
226  }
227 
228  template<typename Vector3Derived, typename QuaternionDerived>
229  EIGEN_DONT_INLINE
230  void calc(JointDataDerived & data,
231  const typename Eigen::MatrixBase<Vector3Derived> & trans,
232  const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
233  {
234  data.M.translation(trans);
235  data.M.rotation(quat.matrix());
236  }
237 
238  template<typename ConfigVector>
239  EIGEN_DONT_INLINE
240  void calc(JointDataDerived & data,
241  const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
242  {
243  typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
244  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
245 
246  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
247  ConstQuaternionMap quat(q.template tail<4>().data());
248 
249  calc(data,q.template head<3>(),quat);
250  }
251 
252  template<typename ConfigVector>
253  EIGEN_DONT_INLINE
254  void calc(JointDataDerived & data,
255  const typename Eigen::MatrixBase<ConfigVector> & qs) const
256  {
257  typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
258 
259  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
260  const Quaternion quat(q.template tail<4>());
261 
262  calc(data,q.template head<3>(),quat);
263  }
264 
265  template<typename ConfigVector, typename TangentVector>
266  EIGEN_DONT_INLINE
267  void calc(JointDataDerived & data,
268  const typename Eigen::MatrixBase<ConfigVector> & qs,
269  const typename Eigen::MatrixBase<TangentVector> & vs) const
270  {
271  calc(data,qs.derived());
272 
273  data.v = vs.template segment<NV>(idx_v());
274  }
275 
276  template<typename Matrix6Like>
277  void calc_aba(JointDataDerived & data,
278  const Eigen::MatrixBase<Matrix6Like> & I,
279  const bool update_I) const
280  {
281  data.U = I;
282 
283  // compute inverse
284 // data.Dinv.setIdentity();
285 // I.llt().solveInPlace(data.Dinv);
286  internal::PerformStYSInversion<Scalar>::run(I,data.Dinv);
287 
288  if (update_I)
289  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero();
290  }
291 
292  static std::string classname() { return std::string("JointModelFreeFlyer"); }
293  std::string shortname() const { return classname(); }
294 
296  template<typename NewScalar>
298  {
300  ReturnType res;
301  res.setIndexes(id(),idx_q(),idx_v());
302  return res;
303  }
304 
305  }; // struct JointModelFreeFlyerTpl
306 
307 } // namespace pinocchio
308 
309 #include <boost/type_traits.hpp>
310 
311 namespace boost
312 {
313  template<typename Scalar, int Options>
314  struct has_nothrow_constructor< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
315  : public integral_constant<bool,true> {};
316 
317  template<typename Scalar, int Options>
318  struct has_nothrow_copy< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
319  : public integral_constant<bool,true> {};
320 
321  template<typename Scalar, int Options>
322  struct has_nothrow_constructor< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
323  : public integral_constant<bool,true> {};
324 
325  template<typename Scalar, int Options>
326  struct has_nothrow_copy< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
327  : public integral_constant<bool,true> {};
328 }
329 
330 #endif // ifndef __pinocchio_joint_free_flyer_hpp__
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...
Return type of the ation of a Motion onto an object of type D.
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
JointModelFreeFlyerTpl< NewScalar, Options > cast() const
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:86
ActionMatrixType toActionMatrixInverse() const
The action matrix of .
Definition: se3-base.hpp:66
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
ActionMatrixType toActionMatrix() const
The action matrix of .
Definition: se3-base.hpp:56
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
void calc_aba(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcAbaVisitor to...
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)