pinocchio  2.7.1
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 
33  typedef MotionSphericalTpl<Scalar,Options> JointMotion;
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>
42  struct ConstraintSphericalZYXTpl
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 
52  ConstraintSphericalZYXTpl() {}
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 
71  struct ConstraintTranspose
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,
184  const ConstraintSphericalZYXTpl<S2,O2> & S)
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,
204  const ConstraintSphericalZYXTpl<S2,O2> & S)
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>
211  struct SE3GroupAction< ConstraintSphericalZYXTpl<S1,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 };
237  typedef JointDataSphericalZYXTpl<Scalar,Options> JointDataDerived;
238  typedef JointModelSphericalZYXTpl<Scalar,Options> JointModelDerived;
239  typedef ConstraintSphericalZYXTpl<Scalar,Options> Constraint_t;
240  typedef SE3Tpl<Scalar,Options> Transformation_t;
241  typedef MotionSphericalTpl<Scalar,Options> Motion_t;
242  typedef MotionSphericalTpl<Scalar,Options> Bias_t;
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> >
257  { typedef JointSphericalZYXTpl<Scalar,Options> JointDerived; };
258 
259  template<typename Scalar, int Options>
260  struct traits< JointModelSphericalZYXTpl<Scalar,Options> >
261  { typedef JointSphericalZYXTpl<Scalar,Options> JointDerived; };
262 
263  template<typename _Scalar, int _Options>
264  struct JointDataSphericalZYXTpl : public JointDataBase< JointDataSphericalZYXTpl<_Scalar,_Options> >
265  {
266  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
267  typedef JointSphericalZYXTpl<_Scalar,_Options> JointDerived;
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 
282  JointDataSphericalZYXTpl ()
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>
299  struct JointModelSphericalZYXTpl
300  : public JointModelBase< JointModelSphericalZYXTpl<_Scalar,_Options> >
301  {
302  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
303  typedef JointSphericalZYXTpl<_Scalar,_Options> JointDerived;
304  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
305 
306  typedef JointModelBase<JointModelSphericalZYXTpl> Base;
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  const std::vector<bool> hasConfigurationLimit() const
315  {
316  return {true, true, true};
317  }
318 
319  const std::vector<bool> hasConfigurationLimitInTangent() const
320  {
321  return {true, true, true};
322  }
323 
324  template<typename ConfigVector>
325  void calc(JointDataDerived & data,
326  const typename Eigen::MatrixBase<ConfigVector> & qs) const
327  {
328  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q());
329 
330  typedef typename ConfigVector::Scalar S2;
331 
332  S2 c0,s0; SINCOS(q(0), &s0, &c0);
333  S2 c1,s1; SINCOS(q(1), &s1, &c1);
334  S2 c2,s2; SINCOS(q(2), &s2, &c2);
335 
336  data.M.rotation () << c0 * c1,
337  c0 * s1 * s2 - s0 * c2,
338  c0 * s1 * c2 + s0 * s2,
339  s0 * c1,
340  s0 * s1 * s2 + c0 * c2,
341  s0 * s1 * c2 - c0 * s2,
342  -s1,
343  c1 * s2,
344  c1 * c2;
345 
346  data.S.angularSubspace()
347  << -s1, Scalar(0), Scalar(1),
348  c1 * s2, c2, Scalar(0),
349  c1 * c2, -s2, Scalar(0);
350  }
351 
352  template<typename ConfigVector, typename TangentVector>
353  void calc(JointDataDerived & data,
354  const typename Eigen::MatrixBase<ConfigVector> & qs,
355  const typename Eigen::MatrixBase<TangentVector> & vs) const
356  {
357  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q());
358 
359  typedef typename ConfigVector::Scalar S2;
360 
361  S2 c0,s0; SINCOS(q(0), &s0, &c0);
362  S2 c1,s1; SINCOS(q(1), &s1, &c1);
363  S2 c2,s2; SINCOS(q(2), &s2, &c2);
364 
365  data.M.rotation () << c0 * c1,
366  c0 * s1 * s2 - s0 * c2,
367  c0 * s1 * c2 + s0 * s2,
368  s0 * c1,
369  s0 * s1 * s2 + c0 * c2,
370  s0 * s1 * c2 - c0 * s2,
371  -s1,
372  c1 * s2,
373  c1 * c2;
374 
375  data.S.angularSubspace()
376  << -s1, Scalar(0), Scalar(1),
377  c1 * s2, c2, Scalar(0),
378  c1 * c2, -s2, Scalar(0);
379 
380  typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type
381  & q_dot = vs.template segment<NV>(idx_v());
382 
383  data.v().noalias() = data.S.angularSubspace() * q_dot;
384 
385  data.c()(0) = -c1 * q_dot(0) * q_dot(1);
386  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);
387  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);
388  }
389 
390  template<typename Matrix6Like>
391  void calc_aba(JointDataDerived & data,
392  const Eigen::MatrixBase<Matrix6Like> & I,
393  const bool update_I) const
394  {
395  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace();
396  data.StU.noalias() = data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
397 
398  // compute inverse
399 // data.Dinv.setIdentity();
400 // data.StU.llt().solveInPlace(data.Dinv);
401  internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
402 
403  data.UDinv.noalias() = data.U * data.Dinv;
404 
405  if (update_I)
406  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
407  }
408 
409  static std::string classname() { return std::string("JointModelSphericalZYX"); }
410  std::string shortname() const { return classname(); }
411 
413  template<typename NewScalar>
414  JointModelSphericalZYXTpl<NewScalar,Options> cast() const
415  {
416  typedef JointModelSphericalZYXTpl<NewScalar,Options> ReturnType;
417  ReturnType res;
418  res.setIndexes(id(),idx_q(),idx_v());
419  return res;
420  }
421 
422  }; // struct JointModelSphericalZYXTpl
423 
424 } // namespace pinocchio
425 
426 #include <boost/type_traits.hpp>
427 
428 namespace boost
429 {
430  template<typename Scalar, int Options>
431  struct has_nothrow_constructor< ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> >
432  : public integral_constant<bool,true> {};
433 
434  template<typename Scalar, int Options>
435  struct has_nothrow_copy< ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> >
436  : public integral_constant<bool,true> {};
437 
438  template<typename Scalar, int Options>
439  struct has_nothrow_constructor< ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> >
440  : public integral_constant<bool,true> {};
441 
442  template<typename Scalar, int Options>
443  struct has_nothrow_copy< ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> >
444  : public integral_constant<bool,true> {};
445 }
446 
447 #endif // ifndef __pinocchio_joint_spherical_ZYX_hpp__
pinocchio::MatrixMatrixProduct
Definition: matrix.hpp:64
pinocchio::ConstraintSphericalZYXTpl::ConstraintTranspose
Definition: joint-spherical-ZYX.hpp:71
pinocchio::operator*
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
&#160;
Definition: constraint-base.hpp:112
pinocchio::idx_q
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
pinocchio::idx_v
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...
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::JointDataBase
Definition: joint-data-base.hpp:82
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
pinocchio::JointSphericalZYXTpl
Definition: joint-spherical-ZYX.hpp:226
pinocchio::SE3GroupAction
Definition: se3.hpp:39
pinocchio::ConstraintSphericalZYXTpl
Definition: joint-spherical-ZYX.hpp:20
pinocchio::MotionSphericalTpl
Definition: joint-spherical.hpp:19
pinocchio::JointModelSphericalZYXTpl::cast
JointModelSphericalZYXTpl< NewScalar, Options > cast() const
Definition: joint-spherical-ZYX.hpp:414
pinocchio::JointDataSphericalZYXTpl
Definition: fwd.hpp:51
pinocchio::ForceDense
Definition: force-dense.hpp:24
pinocchio::JointModelSphericalZYXTpl
Definition: fwd.hpp:48
pinocchio::cross
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
pinocchio::ConstraintBase
Definition: constraint-base.hpp:48
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::alphaSkew
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....
Definition: skew.hpp:124
pinocchio::ForceDense::angular
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11