pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-spherical-ZYX.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_spherical_ZYX_hpp__
7 #define __pinocchio_joint_spherical_ZYX_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/joint/joint-spherical.hpp"
12 #include "pinocchio/multibody/constraint.hpp"
13 #include "pinocchio/math/sincos.hpp"
14 #include "pinocchio/math/matrix.hpp"
15 #include "pinocchio/spatial/inertia.hpp"
16 #include "pinocchio/spatial/skew.hpp"
17 
18 namespace pinocchio
19 {
20  template<typename Scalar, int Options> struct ConstraintSphericalZYXTpl;
21 
22  template <typename _Scalar, int _Options>
23  struct traits< ConstraintSphericalZYXTpl<_Scalar,_Options> >
24  {
25  typedef _Scalar Scalar;
26  enum { Options = _Options };
27 
28  enum {
29  LINEAR = 0,
30  ANGULAR = 3
31  };
32 
34  typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
35  typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
36 
37  typedef DenseBase MatrixReturnType;
38  typedef const DenseBase ConstMatrixReturnType;
39  }; // struct traits struct ConstraintRotationalSubspace
40 
41  template<typename _Scalar, int _Options>
43  : public ConstraintBase< ConstraintSphericalZYXTpl<_Scalar,_Options> >
44  {
45  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46 
47  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalZYXTpl)
48 
49  enum { NV = 3 };
50  typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
51 
53 
54  template<typename Matrix3Like>
55  ConstraintSphericalZYXTpl(const Eigen::MatrixBase<Matrix3Like> & subspace)
56  : m_S(subspace)
57  { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3); }
58 
59  template<typename Vector3Like>
60  JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const
61  {
62  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
63  return JointMotion(m_S * v);
64  }
65 
66  Matrix3 & operator() () { return m_S; }
67  const Matrix3 & operator() () const { return m_S; }
68 
69  int nv_impl() const { return NV; }
70 
72  {
73  const ConstraintSphericalZYXTpl & ref;
74  ConstraintTranspose(const ConstraintSphericalZYXTpl & ref) : ref(ref) {}
75 
76  template<typename Derived>
77  const typename MatrixMatrixProduct<
78  Eigen::Transpose<const Matrix3>,
79  Eigen::Block<const typename ForceDense<Derived>::Vector6,3,1>
80  >::type
81  operator* (const ForceDense<Derived> & phi) const
82  {
83  return ref.m_S.transpose() * phi.angular();
84  }
85 
86  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
87  template<typename D>
88  const typename MatrixMatrixProduct<
89  typename Eigen::Transpose<const Matrix3>,
90  typename Eigen::MatrixBase<const D>::template NRowsBlockXpr<3>::Type
91  >::type
92  operator* (const Eigen::MatrixBase<D> & F) const
93  {
94  EIGEN_STATIC_ASSERT(D::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
95  return ref.m_S.transpose () * F.template middleRows<3>(ANGULAR);
96  }
97  }; // struct ConstraintTranspose
98 
99  ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
100 
101  DenseBase matrix_impl() const
102  {
103  DenseBase S;
104  S.template middleRows<3>(LINEAR).setZero();
105  S.template middleRows<3>(ANGULAR) = m_S;
106  return S;
107  }
108 
109  // const typename Eigen::ProductReturnType<
110  // const ConstraintDense,
111  // const Matrix3
112  // >::Type
113  template<typename S1, int O1>
114  Eigen::Matrix<Scalar,6,3,Options>
115  se3Action(const SE3Tpl<S1,O1> & m) const
116  {
117  // Eigen::Matrix <Scalar,6,3,Options> X_subspace;
118  // X_subspace.template block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ();
119  // X_subspace.template block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
120  //
121  // return (X_subspace * m_S).eval();
122 
123  Eigen::Matrix<Scalar,6,3,Options> result;
124 
125  // ANGULAR
126  result.template middleRows<3>(ANGULAR).noalias() = m.rotation () * m_S;
127 
128  // LINEAR
129  cross(m.translation(),
130  result.template middleRows<3>(Motion::ANGULAR),
131  result.template middleRows<3>(LINEAR));
132 
133  return result;
134  }
135 
136  template<typename S1, int O1>
137  Eigen::Matrix<Scalar,6,3,Options>
138  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
139  {
140  Eigen::Matrix<Scalar,6,3,Options> result;
141 
142  // LINEAR
143  cross(m.translation(),
144  m_S,
145  result.template middleRows<3>(ANGULAR));
146  result.template middleRows<3>(LINEAR).noalias() = -m.rotation().transpose() * result.template middleRows<3>(ANGULAR);
147 
148  // ANGULAR
149  result.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * m_S;
150 
151  return result;
152  }
153 
154  template<typename MotionDerived>
155  DenseBase motionAction(const MotionDense<MotionDerived> & m) const
156  {
157  const typename MotionDerived::ConstLinearType v = m.linear();
158  const typename MotionDerived::ConstAngularType w = m.angular();
159 
160  DenseBase res;
161  cross(v,m_S,res.template middleRows<3>(LINEAR));
162  cross(w,m_S,res.template middleRows<3>(ANGULAR));
163 
164  return res;
165  }
166 
167  Matrix3 & angularSubspace() { return m_S; }
168  const Matrix3 & angularSubspace() const { return m_S; }
169 
170  bool isEqual(const ConstraintSphericalZYXTpl & other) const
171  {
172  return m_S == other.m_S;
173  }
174 
175  protected:
176  Matrix3 m_S;
177 
178  }; // struct ConstraintSphericalZYXTpl
179 
180  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
181  template <typename S1, int O1, typename S2, int O2>
182  Eigen::Matrix<S1,6,3,O1>
183  operator*(const InertiaTpl<S1,O1> & Y,
185  {
186  typedef typename InertiaTpl<S1,O1>::Symmetric3 Symmetric3;
187  typedef ConstraintSphericalZYXTpl<S2,O2> Constraint;
188  Eigen::Matrix<S1,6,3,O1> M;
189  alphaSkew (-Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::LINEAR));
190  M.template middleRows<3>(Constraint::ANGULAR) = (Y.inertia () -
191  typename Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ())).matrix();
192 
193  return (M * S.angularSubspace()).eval();
194  }
195 
196  /* [ABA] Y*S operator (Inertia Y,Constraint S) */
197  // inline Eigen::Matrix<double,6,3>
198  template<typename Matrix6Like, typename S2, int O2>
199  const typename MatrixMatrixProduct<
200  typename Eigen::internal::remove_const<typename SizeDepType<3>::ColsReturn<Matrix6Like>::ConstType>::type,
201  typename ConstraintSphericalZYXTpl<S2,O2>::Matrix3
202  >::type
203  operator*(const Eigen::MatrixBase<Matrix6Like> & Y,
205  {
206  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like,6,6);
207  return Y.derived().template middleCols<3>(Inertia::ANGULAR) * S.angularSubspace();
208  }
209 
210  template<typename S1, int O1>
212  {
213  // typedef const typename Eigen::ProductReturnType<
214  // Eigen::Matrix <double,6,3,0>,
215  // Eigen::Matrix <double,3,3,0>
216  // >::Type Type;
217  typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
218  };
219 
220  template<typename S1, int O1, typename MotionDerived>
221  struct MotionAlgebraAction< ConstraintSphericalZYXTpl<S1,O1>, MotionDerived >
222  {
223  typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
224  };
225 
226  template<typename Scalar, int Options> struct JointSphericalZYXTpl;
227 
228  template<typename _Scalar, int _Options>
229  struct traits< JointSphericalZYXTpl<_Scalar,_Options> >
230  {
231  enum {
232  NQ = 3,
233  NV = 3
234  };
235  typedef _Scalar Scalar;
236  enum { Options = _Options };
243 
244  // [ABA]
245  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
246  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
247  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
248 
249  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
250 
251  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
252  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
253  };
254 
255  template<typename Scalar, int Options>
256  struct traits< JointDataSphericalZYXTpl<Scalar,Options> >
258 
259  template<typename Scalar, int Options>
260  struct traits< JointModelSphericalZYXTpl<Scalar,Options> >
262 
263  template<typename _Scalar, int _Options>
264  struct JointDataSphericalZYXTpl : public JointDataBase< JointDataSphericalZYXTpl<_Scalar,_Options> >
265  {
266  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
268  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
269  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
270 
271  Constraint_t S;
272  Transformation_t M;
273  Motion_t v;
274  Bias_t c;
275 
276  // [ABA] specific data
277  U_t U;
278  D_t Dinv;
279  UD_t UDinv;
280  D_t StU;
281 
283  : S(Constraint_t::Matrix3::Zero())
284  , M(Transformation_t::Identity())
285  , v(Motion_t::Vector3::Zero())
286  , c(Bias_t::Vector3::Zero())
287  , U(U_t::Zero())
288  , Dinv(D_t::Zero())
289  , UDinv(UD_t::Zero())
290  {}
291 
292  static std::string classname() { return std::string("JointDataSphericalZYX"); }
293  std::string shortname() const { return classname(); }
294 
295  }; // strcut JointDataSphericalZYXTpl
296 
297  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalZYXTpl);
298  template<typename _Scalar, int _Options>
300  : public JointModelBase< JointModelSphericalZYXTpl<_Scalar,_Options> >
301  {
302  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
304  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
305 
307  using Base::id;
308  using Base::idx_q;
309  using Base::idx_v;
310  using Base::setIndexes;
311 
312  JointDataDerived createData() const { return JointDataDerived(); }
313 
314  template<typename ConfigVector>
315  void calc(JointDataDerived & data,
316  const typename Eigen::MatrixBase<ConfigVector> & qs) const
317  {
318  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q());
319 
320  typedef typename ConfigVector::Scalar S2;
321 
322  S2 c0,s0; SINCOS(q(0), &s0, &c0);
323  S2 c1,s1; SINCOS(q(1), &s1, &c1);
324  S2 c2,s2; SINCOS(q(2), &s2, &c2);
325 
326  data.M.rotation () << c0 * c1,
327  c0 * s1 * s2 - s0 * c2,
328  c0 * s1 * c2 + s0 * s2,
329  s0 * c1,
330  s0 * s1 * s2 + c0 * c2,
331  s0 * s1 * c2 - c0 * s2,
332  -s1,
333  c1 * s2,
334  c1 * c2;
335 
336  data.S.angularSubspace()
337  << -s1, Scalar(0), Scalar(1),
338  c1 * s2, c2, Scalar(0),
339  c1 * c2, -s2, Scalar(0);
340  }
341 
342  template<typename ConfigVector, typename TangentVector>
343  void calc(JointDataDerived & data,
344  const typename Eigen::MatrixBase<ConfigVector> & qs,
345  const typename Eigen::MatrixBase<TangentVector> & vs) const
346  {
347  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q());
348 
349  typedef typename ConfigVector::Scalar S2;
350 
351  S2 c0,s0; SINCOS(q(0), &s0, &c0);
352  S2 c1,s1; SINCOS(q(1), &s1, &c1);
353  S2 c2,s2; SINCOS(q(2), &s2, &c2);
354 
355  data.M.rotation () << c0 * c1,
356  c0 * s1 * s2 - s0 * c2,
357  c0 * s1 * c2 + s0 * s2,
358  s0 * c1,
359  s0 * s1 * s2 + c0 * c2,
360  s0 * s1 * c2 - c0 * s2,
361  -s1,
362  c1 * s2,
363  c1 * c2;
364 
365  data.S.angularSubspace()
366  << -s1, Scalar(0), Scalar(1),
367  c1 * s2, c2, Scalar(0),
368  c1 * c2, -s2, Scalar(0);
369 
370  typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type
371  & q_dot = vs.template segment<NV>(idx_v());
372 
373  data.v().noalias() = data.S.angularSubspace() * q_dot;
374 
375  data.c()(0) = -c1 * q_dot(0) * q_dot(1);
376  data.c()(1) = -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 * q_dot(1) * q_dot(2);
377  data.c()(2) = -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 * q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2);
378  }
379 
380  template<typename Matrix6Like>
381  void calc_aba(JointDataDerived & data,
382  const Eigen::MatrixBase<Matrix6Like> & I,
383  const bool update_I) const
384  {
385  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace();
386  data.StU.noalias() = data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
387 
388  // compute inverse
389 // data.Dinv.setIdentity();
390 // data.StU.llt().solveInPlace(data.Dinv);
391  internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
392 
393  data.UDinv.noalias() = data.U * data.Dinv;
394 
395  if (update_I)
396  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
397  }
398 
399  static std::string classname() { return std::string("JointModelSphericalZYX"); }
400  std::string shortname() const { return classname(); }
401 
403  template<typename NewScalar>
405  {
407  ReturnType res;
408  res.setIndexes(id(),idx_q(),idx_v());
409  return res;
410  }
411 
412  }; // struct JointModelSphericalZYXTpl
413 
414 } // namespace pinocchio
415 
416 #include <boost/type_traits.hpp>
417 
418 namespace boost
419 {
420  template<typename Scalar, int Options>
421  struct has_nothrow_constructor< ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> >
422  : public integral_constant<bool,true> {};
423 
424  template<typename Scalar, int Options>
425  struct has_nothrow_copy< ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> >
426  : public integral_constant<bool,true> {};
427 
428  template<typename Scalar, int Options>
429  struct has_nothrow_constructor< ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> >
430  : public integral_constant<bool,true> {};
431 
432  template<typename Scalar, int Options>
433  struct has_nothrow_copy< ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> >
434  : public integral_constant<bool,true> {};
435 }
436 
437 #endif // ifndef __pinocchio_joint_spherical_ZYX_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...
Definition: casadi.hpp:13
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.
Definition: motion.hpp:44
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Definition: skew.hpp:211
JointModelSphericalZYXTpl< NewScalar, Options > cast() const
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
Main pinocchio namespace.
Definition: treeview.dox:24
void alphaSkew(const Scalar alpha, const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector multiplied by a given scalar. i.e. the antisymmetric matrix representation of the cross product operator ( )
Definition: skew.hpp:124
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
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)
 .