pinocchio  2.2.1-dirty
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  : S_minimal(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(S_minimal * v);
64  }
65 
66  Matrix3 & operator() () { return S_minimal; }
67  const Matrix3 & operator() () const { return S_minimal; }
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.S_minimal.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.S_minimal.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) = S_minimal;
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 * S_minimal).eval();
122 
123  Eigen::Matrix<Scalar,6,3,Options> result;
124 
125  // ANGULAR
126  result.template middleRows<3>(ANGULAR).noalias() = m.rotation () * S_minimal;
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  S_minimal,
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() * S_minimal;
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,S_minimal,res.template middleRows<3>(LINEAR));
162  cross(w,S_minimal,res.template middleRows<3>(ANGULAR));
163 
164  return res;
165  }
166 
167  // data
168  Matrix3 S_minimal;
169 
170  }; // struct ConstraintSphericalZYXTpl
171 
172  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
173  template <typename S1, int O1, typename S2, int O2>
174  Eigen::Matrix<S1,6,3,O1>
175  operator*(const InertiaTpl<S1,O1> & Y,
177  {
178  typedef typename InertiaTpl<S1,O1>::Symmetric3 Symmetric3;
179  typedef ConstraintSphericalZYXTpl<S2,O2> Constraint;
180  Eigen::Matrix<S1,6,3,O1> M;
181  alphaSkew (-Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::LINEAR));
182  M.template middleRows<3>(Constraint::ANGULAR) = (Y.inertia () -
183  typename Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ())).matrix();
184 
185  return (M * S.S_minimal).eval();
186  }
187 
188  /* [ABA] Y*S operator (Inertia Y,Constraint S) */
189  // inline Eigen::Matrix<double,6,3>
190  template<typename Matrix6Like, typename S2, int O2>
191  const typename MatrixMatrixProduct<
192  typename Eigen::internal::remove_const<typename SizeDepType<3>::ColsReturn<Matrix6Like>::ConstType>::type,
193  typename ConstraintSphericalZYXTpl<S2,O2>::Matrix3
194  >::type
195  operator*(const Eigen::MatrixBase<Matrix6Like> & Y,
197  {
198  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like,6,6);
199  return Y.derived().template middleCols<3>(Inertia::ANGULAR) * S.S_minimal;
200  }
201 
202  template<typename S1, int O1>
204  {
205  // typedef const typename Eigen::ProductReturnType<
206  // Eigen::Matrix <double,6,3,0>,
207  // Eigen::Matrix <double,3,3,0>
208  // >::Type Type;
209  typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
210  };
211 
212  template<typename S1, int O1, typename MotionDerived>
213  struct MotionAlgebraAction< ConstraintSphericalZYXTpl<S1,O1>, MotionDerived >
214  {
215  typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
216  };
217 
218  template<typename Scalar, int Options> struct JointSphericalZYXTpl;
219 
220  template<typename _Scalar, int _Options>
221  struct traits< JointSphericalZYXTpl<_Scalar,_Options> >
222  {
223  enum {
224  NQ = 3,
225  NV = 3
226  };
227  typedef _Scalar Scalar;
228  enum { Options = _Options };
235 
236  // [ABA]
237  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
238  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
239  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
240 
241  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
242 
243  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
244  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
245  };
246 
247  template<typename Scalar, int Options>
248  struct traits< JointDataSphericalZYXTpl<Scalar,Options> >
250 
251  template<typename Scalar, int Options>
252  struct traits< JointModelSphericalZYXTpl<Scalar,Options> >
254 
255  template<typename _Scalar, int _Options>
256  struct JointDataSphericalZYXTpl : public JointDataBase< JointDataSphericalZYXTpl<_Scalar,_Options> >
257  {
258  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
260  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
261  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
262 
263  Constraint_t S;
264  Transformation_t M;
265  Motion_t v;
266  Bias_t c;
267 
268  // [ABA] specific data
269  U_t U;
270  D_t Dinv;
271  UD_t UDinv;
272  D_t StU;
273 
274  JointDataSphericalZYXTpl () : M(1), U(), Dinv(), UDinv() {}
275 
276  static std::string classname() { return std::string("JointDataSphericalZYX"); }
277  std::string shortname() const { return classname(); }
278 
279  }; // strcut JointDataSphericalZYXTpl
280 
281  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalZYXTpl);
282  template<typename _Scalar, int _Options>
284  : public JointModelBase< JointModelSphericalZYXTpl<_Scalar,_Options> >
285  {
286  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
288  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
289 
291  using Base::id;
292  using Base::idx_q;
293  using Base::idx_v;
294  using Base::setIndexes;
295 
296  JointDataDerived createData() const { return JointDataDerived(); }
297 
298  template<typename ConfigVector>
299  void calc(JointDataDerived & data,
300  const typename Eigen::MatrixBase<ConfigVector> & qs) const
301  {
302  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q());
303 
304  typedef typename ConfigVector::Scalar S2;
305 
306  S2 c0,s0; SINCOS(q(0), &s0, &c0);
307  S2 c1,s1; SINCOS(q(1), &s1, &c1);
308  S2 c2,s2; SINCOS(q(2), &s2, &c2);
309 
310  data.M.rotation () << c0 * c1,
311  c0 * s1 * s2 - s0 * c2,
312  c0 * s1 * c2 + s0 * s2,
313  s0 * c1,
314  s0 * s1 * s2 + c0 * c2,
315  s0 * s1 * c2 - c0 * s2,
316  -s1,
317  c1 * s2,
318  c1 * c2;
319 
320  data.S.S_minimal
321  << -s1, Scalar(0), Scalar(1),
322  c1 * s2, c2, Scalar(0),
323  c1 * c2, -s2, Scalar(0);
324  }
325 
326  template<typename ConfigVector, typename TangentVector>
327  void calc(JointDataDerived & data,
328  const typename Eigen::MatrixBase<ConfigVector> & qs,
329  const typename Eigen::MatrixBase<TangentVector> & vs) const
330  {
331  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q());
332 
333  typedef typename ConfigVector::Scalar S2;
334 
335  S2 c0,s0; SINCOS(q(0), &s0, &c0);
336  S2 c1,s1; SINCOS(q(1), &s1, &c1);
337  S2 c2,s2; SINCOS(q(2), &s2, &c2);
338 
339  data.M.rotation () << c0 * c1,
340  c0 * s1 * s2 - s0 * c2,
341  c0 * s1 * c2 + s0 * s2,
342  s0 * c1,
343  s0 * s1 * s2 + c0 * c2,
344  s0 * s1 * c2 - c0 * s2,
345  -s1,
346  c1 * s2,
347  c1 * c2;
348 
349  data.S.S_minimal
350  << -s1, Scalar(0), Scalar(1),
351  c1 * s2, c2, Scalar(0),
352  c1 * c2, -s2, Scalar(0);
353 
354  typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type
355  & q_dot = vs.template segment<NV>(idx_v());
356 
357  data.v().noalias() = data.S.S_minimal * q_dot;
358 
359  data.c()(0) = -c1 * q_dot(0) * q_dot(1);
360  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);
361  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);
362  }
363 
364  template<typename Matrix6Like>
365  void calc_aba(JointDataDerived & data,
366  const Eigen::MatrixBase<Matrix6Like> & I,
367  const bool update_I) const
368  {
369  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.S_minimal;
370  data.StU.noalias() = data.S.S_minimal.transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
371 
372  // compute inverse
373 // data.Dinv.setIdentity();
374 // data.StU.llt().solveInPlace(data.Dinv);
375  internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
376 
377  data.UDinv.noalias() = data.U * data.Dinv;
378 
379  if (update_I)
380  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
381  }
382 
383  static std::string classname() { return std::string("JointModelSphericalZYX"); }
384  std::string shortname() const { return classname(); }
385 
387  template<typename NewScalar>
389  {
391  ReturnType res;
392  res.setIndexes(id(),idx_q(),idx_v());
393  return res;
394  }
395 
396  }; // struct JointModelSphericalZYXTpl
397 
398 } // namespace pinocchio
399 
400 #include <boost/type_traits.hpp>
401 
402 namespace boost
403 {
404  template<typename Scalar, int Options>
405  struct has_nothrow_constructor< ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> >
406  : public integral_constant<bool,true> {};
407 
408  template<typename Scalar, int Options>
409  struct has_nothrow_copy< ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> >
410  : public integral_constant<bool,true> {};
411 
412  template<typename Scalar, int Options>
413  struct has_nothrow_constructor< ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> >
414  : public integral_constant<bool,true> {};
415 
416  template<typename Scalar, int Options>
417  struct has_nothrow_copy< ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> >
418  : public integral_constant<bool,true> {};
419 }
420 
421 #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...
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.
void SINCOS(const Scalar &a, Scalar *sa, Scalar *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:40
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
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: 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)