pinocchio  2.2.1-dirty
joint-prismatic-unaligned.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_joint_prismatic_unaligned_hpp__
7 #define __pinocchio_joint_prismatic_unaligned_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/joint/joint-translation.hpp"
12 #include "pinocchio/multibody/constraint.hpp"
13 #include "pinocchio/spatial/inertia.hpp"
14 #include "pinocchio/math/matrix.hpp"
15 
16 namespace pinocchio
17 {
18 
19  template<typename Scalar, int Options=0> struct MotionPrismaticUnalignedTpl;
21 
22  template<typename Scalar, int Options>
23  struct SE3GroupAction< MotionPrismaticUnalignedTpl<Scalar,Options> >
24  {
26  };
27 
28  template<typename Scalar, int Options, typename MotionDerived>
29  struct MotionAlgebraAction< MotionPrismaticUnalignedTpl<Scalar,Options>, MotionDerived>
30  {
32  };
33 
34  template<typename _Scalar, int _Options>
35  struct traits< MotionPrismaticUnalignedTpl<_Scalar,_Options> >
36  {
37  typedef _Scalar Scalar;
38  enum { Options = _Options };
39  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
40  typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
41  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
42  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
43  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
44  typedef Vector3 AngularType;
45  typedef Vector3 LinearType;
46  typedef const Vector3 ConstAngularType;
47  typedef const Vector3 ConstLinearType;
48  typedef Matrix6 ActionMatrixType;
50  typedef MotionPlain PlainReturnType;
51  enum {
52  LINEAR = 0,
53  ANGULAR = 3
54  };
55  }; // traits MotionPrismaticUnalignedTpl
56 
57  template<typename _Scalar, int _Options>
59  : MotionBase < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
60  {
61  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62  MOTION_TYPEDEF_TPL(MotionPrismaticUnalignedTpl);
63 
65 
66  template<typename Vector3Like, typename S2>
67  MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
68  const S2 & rate)
69  : axis(axis), rate(rate)
70  { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
71 
72  inline PlainReturnType plain() const
73  {
74  return PlainReturnType(axis*rate,
75  PlainReturnType::Vector3::Zero());
76  }
77 
78  template<typename OtherScalar>
79  MotionPrismaticUnalignedTpl __mult__(const OtherScalar & alpha) const
80  {
81  return MotionPrismaticUnalignedTpl(axis,alpha*rate);
82  }
83 
84  template<typename Derived>
85  void addTo(MotionDense<Derived> & other) const
86  {
87  other.linear() += axis * rate;
88  }
89 
90  template<typename Derived>
91  void setTo(MotionDense<Derived> & other) const
92  {
93  other.linear().noalias() = axis*rate;
94  other.angular().setZero();
95  }
96 
97  template<typename S2, int O2, typename D2>
98  void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
99  {
100  v.linear().noalias() = rate * (m.rotation() * axis); // TODO: check efficiency
101  v.angular().setZero();
102  }
103 
104  template<typename S2, int O2>
105  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
106  {
107  MotionPlain res;
108  se3Action_impl(m,res);
109  return res;
110  }
111 
112  template<typename S2, int O2, typename D2>
113  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
114  {
115  // Linear
116  v.linear().noalias() = rate * (m.rotation().transpose() * axis);
117 
118  // Angular
119  v.angular().setZero();
120  }
121 
122  template<typename S2, int O2>
123  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
124  {
125  MotionPlain res;
126  se3ActionInverse_impl(m,res);
127  return res;
128  }
129 
130  template<typename M1, typename M2>
131  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
132  {
133  // Linear
134  mout.linear().noalias() = v.angular().cross(axis);
135  mout.linear() *= rate;
136 
137  // Angular
138  mout.angular().setZero();
139  }
140 
141  template<typename M1>
142  MotionPlain motionAction(const MotionDense<M1> & v) const
143  {
144  MotionPlain res;
145  motionAction(v,res);
146  return res;
147  }
148 
149  // data
150  Vector3 axis;
151  Scalar rate;
152  }; // struct MotionPrismaticUnalignedTpl
153 
154  template<typename Scalar, int Options, typename MotionDerived>
155  inline typename MotionDerived::MotionPlain
157  {
158  typedef typename MotionDerived::MotionPlain ReturnType;
159  return ReturnType(m1.rate*m1.axis + m2.linear(), m2.angular());
160  }
161 
162  template<typename MotionDerived, typename S2, int O2>
163  inline typename MotionDerived::MotionPlain
164  operator^(const MotionDense<MotionDerived> & m1,
166  {
167  return m2.motionAction(m1);
168  }
169 
170  template<typename Scalar, int Options> struct ConstraintPrismaticUnalignedTpl;
171 
172  template<typename _Scalar, int _Options>
173  struct traits< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
174  {
175  typedef _Scalar Scalar;
176  enum { Options = _Options };
177  enum {
178  LINEAR = 0,
179  ANGULAR = 3
180  };
182  typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
183  typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
184  typedef DenseBase MatrixReturnType;
185  typedef const DenseBase ConstMatrixReturnType;
186 
187  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
188  }; // traits ConstraintPrismaticUnalignedTpl
189 
190  template<typename Scalar, int Options>
192  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
193 
194  template<typename Scalar, int Options, typename MotionDerived>
195  struct MotionAlgebraAction< ConstraintPrismaticUnalignedTpl<Scalar,Options>,MotionDerived >
196  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
197 
198  template<typename Scalar, int Options, typename ForceDerived>
199  struct ConstraintForceOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceDerived>
200  {
201  typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
202  typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
203  };
204 
205  template<typename Scalar, int Options, typename ForceSet>
207  {
208  typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
210  typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
211  >::type ReturnType;
212  };
213 
214  template<typename _Scalar, int _Options>
216  : ConstraintBase< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
217  {
218  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
219  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticUnalignedTpl)
220 
221  enum { NV = 1 };
222 
223  typedef typename traits<ConstraintPrismaticUnalignedTpl>::Vector3 Vector3;
224 
226 
227  template<typename Vector3Like>
228  ConstraintPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
229  : axis(axis)
230  { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
231 
232  template<typename Vector1Like>
233  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
234  {
235  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
236  return JointMotion(axis,v[0]);
237  }
238 
239  template<typename S1, int O1>
241  se3Action(const SE3Tpl<S1,O1> & m) const
242  {
244  MotionRef<DenseBase> v(res);
245  v.linear().noalias() = m.rotation()*axis;
246  v.angular().setZero();
247  return res;
248  }
249 
250  template<typename S1, int O1>
252  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
253  {
255  MotionRef<DenseBase> v(res);
256  v.linear().noalias() = m.rotation().transpose()*axis;
257  v.angular().setZero();
258  return res;
259  }
260 
261  int nv_impl() const { return NV; }
262 
264  {
266  TransposeConst(const ConstraintPrismaticUnalignedTpl & ref) : ref(ref) {}
267 
268  template<typename ForceDerived>
269  typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType
270  operator* (const ForceDense<ForceDerived> & f) const
271  {
272  typedef typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType ReturnType;
273  ReturnType res;
274  res[0] = ref.axis.dot(f.linear());
275  return res;
276  }
277 
278  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
279  template<typename ForceSet>
280  typename ConstraintForceSetOp<ConstraintPrismaticUnalignedTpl,ForceSet>::ReturnType
281  operator*(const Eigen::MatrixBase<ForceSet> & F)
282  {
283  EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
284  /* Return ax.T * F[1:3,:] */
285  return ref.axis.transpose() * F.template middleRows<3>(LINEAR);
286  }
287 
288  };
289  TransposeConst transpose() const { return TransposeConst(*this); }
290 
291 
292  /* CRBA joint operators
293  * - ForceSet::Block = ForceSet
294  * - ForceSet operator* (Inertia Y,Constraint S)
295  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
296  * - SE3::act(ForceSet::Block)
297  */
298  DenseBase matrix_impl() const
299  {
300  DenseBase S;
301  S << axis, Vector3::Zero();
302  return S;
303  }
304 
305  template<typename MotionDerived>
306  DenseBase motionAction(const MotionDense<MotionDerived> & v) const
307  {
308  DenseBase res;
309  res << v.angular().cross(axis), Vector3::Zero();
310 
311  return res;
312  }
313 
314  // data
315  Vector3 axis;
316 
317  }; // struct ConstraintPrismaticUnalignedTpl
318 
319  template<typename S1, int O1,typename S2, int O2>
321  {
322  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
323  };
324 
325  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
326  namespace impl
327  {
328  template<typename S1, int O1, typename S2, int O2>
330  {
331  typedef InertiaTpl<S1,O1> Inertia;
333  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
334 
335  static inline ReturnType run(const Inertia & Y,
336  const Constraint & cpu)
337  {
338  ReturnType res;
339  /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
340  const S1 & m = Y.mass();
341  const typename Inertia::Vector3 & c = Y.lever();
342 
343  res.template head<3>().noalias() = m*cpu.axis;
344  res.template tail<3>().noalias() = c.cross(res.template head<3>());
345 
346  return res;
347  }
348  };
349  } // namespace impl
350 
351  template<typename M6Like, typename Scalar, int Options>
352  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
353  {
354  typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
355  typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
356 
358  typedef typename Constraint::Vector3 Vector3;
359  typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
360  };
361 
362  /* [ABA] operator* (Inertia Y,Constraint S) */
363  namespace impl
364  {
365  template<typename M6Like, typename Scalar, int Options>
366  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
367  {
369  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
370  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
371  const Constraint & cru)
372  {
373  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
374  return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis;
375  }
376  };
377  } // namespace impl
378 
379  template<typename Scalar, int Options> struct JointPrismaticUnalignedTpl;
380 
381  template<typename _Scalar, int _Options>
382  struct traits< JointPrismaticUnalignedTpl<_Scalar,_Options> >
383  {
384  enum {
385  NQ = 1,
386  NV = 1
387  };
388  typedef _Scalar Scalar;
389  enum { Options = _Options };
396 
397  // [ABA]
398  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
399  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
400  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
401 
402  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
403 
404  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
405  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
406  };
407 
408  template<typename Scalar, int Options>
409  struct traits< JointDataPrismaticUnalignedTpl<Scalar,Options> >
411 
412  template<typename _Scalar, int _Options>
414  : public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar,_Options> >
415  {
416  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
418  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
419  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
420 
421  Transformation_t M;
422  Constraint_t S;
423  Motion_t v;
424  Bias_t c;
425 
426  // [ABA] specific data
427  U_t U;
428  D_t Dinv;
429  UD_t UDinv;
430 
432 
433  template<typename Vector3Like>
434  JointDataPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
435  : M()
436  , S(axis)
437  , v(axis,(Scalar)NAN)
438  , U(), Dinv(), UDinv()
439  {}
440 
441  static std::string classname() { return std::string("JointDataPrismaticUnaligned"); }
442  std::string shortname() const { return classname(); }
443 
444  }; // struct JointDataPrismaticUnalignedTpl
445 
446  template<typename Scalar, int Options>
447  struct traits< JointModelPrismaticUnalignedTpl<Scalar,Options> >
449 
450  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelPrismaticUnalignedTpl);
451  template<typename _Scalar, int _Options>
453  : public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar,_Options> >
454  {
455  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
457  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
458 
460  using Base::id;
461  using Base::idx_q;
462  using Base::idx_v;
463  using Base::setIndexes;
464 
465  typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
466 
468  JointModelPrismaticUnalignedTpl(const Scalar & x,
469  const Scalar & y,
470  const Scalar & z)
471  : axis(x,y,z)
472  {
473  axis.normalize();
474  assert(isUnitary(axis) && "Translation axis is not unitary");
475  }
476 
477  template<typename Vector3Like>
478  JointModelPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
479  : axis(axis)
480  {
481  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
482  assert(isUnitary(axis) && "Translation axis is not unitary");
483  }
484 
485  JointDataDerived createData() const { return JointDataDerived(axis); }
486 
487  using Base::isEqual;
488  bool isEqual(const JointModelPrismaticUnalignedTpl & other) const
489  {
490  return Base::isEqual(other) && axis == other.axis;
491  }
492 
493  template<typename ConfigVector>
494  void calc(JointDataDerived & data,
495  const typename Eigen::MatrixBase<ConfigVector> & qs) const
496  {
497  typedef typename ConfigVector::Scalar Scalar;
498  const Scalar & q = qs[idx_q()];
499 
500  data.M.translation().noalias() = axis * q;
501  }
502 
503  template<typename ConfigVector, typename TangentVector>
504  void calc(JointDataDerived & data,
505  const typename Eigen::MatrixBase<ConfigVector> & qs,
506  const typename Eigen::MatrixBase<TangentVector> & vs) const
507  {
508  calc(data,qs.derived());
509 
510  typedef typename TangentVector::Scalar S2;
511  const S2 & v = vs[idx_v()];
512  data.v.rate = v;
513  }
514 
515  template<typename Matrix6Like>
516  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
517  {
518  data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) * axis;
519  data.Dinv[0] = Scalar(1)/axis.dot(data.U.template segment <3> (Inertia::LINEAR));
520  data.UDinv.noalias() = data.U * data.Dinv;
521 
522  if (update_I)
523  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
524  }
525 
526  static std::string classname() { return std::string("JointModelPrismaticUnaligned"); }
527  std::string shortname() const { return classname(); }
528 
530  template<typename NewScalar>
532  {
534  ReturnType res(axis.template cast<NewScalar>());
535  res.setIndexes(id(),idx_q(),idx_v());
536  return res;
537  }
538 
539  // data
540 
544  Vector3 axis;
545  }; // struct JointModelPrismaticUnalignedTpl
546 
547 } //namespace pinocchio
548 
549 #include <boost/type_traits.hpp>
550 
551 namespace boost
552 {
553  template<typename Scalar, int Options>
554  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
555  : public integral_constant<bool,true> {};
556 
557  template<typename Scalar, int Options>
558  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
559  : public integral_constant<bool,true> {};
560 
561  template<typename Scalar, int Options>
562  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
563  : public integral_constant<bool,true> {};
564 
565  template<typename Scalar, int Options>
566  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
567  : public integral_constant<bool,true> {};
568 }
569 
570 
571 #endif // ifndef __pinocchio_joint_prismatic_unaligned_hpp__
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
Definition: binary-op.hpp:15
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 Constraint::Transpose * ForceSet operation.
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.
Source from #include <cppad/example/cppad_eigen.hpp>
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointModelPrismaticUnalignedTpl< NewScalar, Options > cast() const
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:47
Return type of the Constraint::Transpose * Force operation.
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)