pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-prismatic.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_prismatic_hpp__
7 #define __pinocchio_joint_prismatic_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/constraint.hpp"
12 #include "pinocchio/spatial/inertia.hpp"
13 #include "pinocchio/spatial/spatial-axis.hpp"
14 #include "pinocchio/utils/axis-label.hpp"
15 
16 namespace pinocchio
17 {
18 
19  template<typename Scalar, int Options, int _axis> struct MotionPrismaticTpl;
20 
21  template<typename Scalar, int Options, int axis>
22  struct SE3GroupAction< MotionPrismaticTpl<Scalar,Options,axis> >
23  {
24  typedef MotionTpl<Scalar,Options> ReturnType;
25  };
26 
27  template<typename Scalar, int Options, int axis, typename MotionDerived>
28  struct MotionAlgebraAction< MotionPrismaticTpl<Scalar,Options,axis>, MotionDerived>
29  {
30  typedef MotionTpl<Scalar,Options> ReturnType;
31  };
32 
33  template<typename _Scalar, int _Options, int _axis>
34  struct traits < MotionPrismaticTpl<_Scalar,_Options,_axis> >
35  {
36  typedef _Scalar Scalar;
37  enum { Options = _Options };
38  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
39  typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
40  typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
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;
49  typedef Matrix4 HomogeneousMatrixType;
50  typedef MotionTpl<Scalar,Options> MotionPlain;
51  typedef MotionPlain PlainReturnType;
52  enum {
53  LINEAR = 0,
54  ANGULAR = 3
55  };
56  }; // struct traits MotionPrismaticTpl
57 
58  template<typename _Scalar, int _Options, int _axis>
59  struct MotionPrismaticTpl
60  : MotionBase < MotionPrismaticTpl<_Scalar,_Options,_axis> >
61  {
62  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63  MOTION_TYPEDEF_TPL(MotionPrismaticTpl);
64 
65  enum { axis = _axis };
66 
67  typedef SpatialAxis<_axis+LINEAR> Axis;
68  typedef typename Axis::CartesianAxis3 CartesianAxis3;
69 
70  MotionPrismaticTpl() {}
71  MotionPrismaticTpl(const Scalar & v) : m_v(v) {}
72 
73  inline PlainReturnType plain() const { return Axis() * m_v; }
74 
75  template<typename OtherScalar>
76  MotionPrismaticTpl __mult__(const OtherScalar & alpha) const
77  {
78  return MotionPrismaticTpl(alpha*m_v);
79  }
80 
81  template<typename Derived>
82  void addTo(MotionDense<Derived> & other) const
83  {
84  typedef typename MotionDense<Derived>::Scalar OtherScalar;
85  other.linear()[_axis] += (OtherScalar) m_v;
86  }
87 
88  template<typename MotionDerived>
89  void setTo(MotionDense<MotionDerived> & other) const
90  {
91  for(Eigen::DenseIndex k = 0; k < 3; ++k)
92  other.linear()[k] = k == axis ? m_v : (Scalar)0;
93  other.angular().setZero();
94  }
95 
96  template<typename S2, int O2, typename D2>
97  void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
98  {
99  v.angular().setZero();
100  v.linear().noalias() = m_v * (m.rotation().col(axis));
101  }
102 
103  template<typename S2, int O2>
104  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
105  {
106  MotionPlain res;
107  se3Action_impl(m,res);
108  return res;
109  }
110 
111  template<typename S2, int O2, typename D2>
112  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
113  {
114  // Linear
115  v.linear().noalias() = m_v * (m.rotation().transpose().col(axis));
116 
117  // Angular
118  v.angular().setZero();
119  }
120 
121  template<typename S2, int O2>
122  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
123  {
124  MotionPlain res;
125  se3ActionInverse_impl(m,res);
126  return res;
127  }
128 
129  template<typename M1, typename M2>
130  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
131  {
132  // Linear
133  CartesianAxis3::alphaCross(-m_v,v.angular(),mout.linear());
134 
135  // Angular
136  mout.angular().setZero();
137  }
138 
139  template<typename M1>
140  MotionPlain motionAction(const MotionDense<M1> & v) const
141  {
142  MotionPlain res;
143  motionAction(v,res);
144  return res;
145  }
146 
147  Scalar & linearRate() { return m_v; }
148  const Scalar & linearRate() const { return m_v; }
149 
150  bool isEqual_impl(const MotionPrismaticTpl & other) const
151  {
152  return m_v == other.m_v;
153  }
154 
155  protected:
156 
157  Scalar m_v;
158  }; // struct MotionPrismaticTpl
159 
160  template<typename Scalar, int Options, int axis, typename MotionDerived>
161  typename MotionDerived::MotionPlain
162  operator+(const MotionPrismaticTpl<Scalar,Options,axis> & m1,
163  const MotionDense<MotionDerived> & m2)
164  {
165  typename MotionDerived::MotionPlain res(m2);
166  res += m1;
167  return res;
168  }
169 
170  template<typename MotionDerived, typename S2, int O2, int axis>
171  EIGEN_STRONG_INLINE
172  typename MotionDerived::MotionPlain
173  operator^(const MotionDense<MotionDerived> & m1, const MotionPrismaticTpl<S2,O2,axis> & m2)
174  {
175  return m2.motionAction(m1);
176  }
177 
178  template<typename Scalar, int Options, int axis> struct TransformPrismaticTpl;
179 
180  template<typename _Scalar, int _Options, int _axis>
181  struct traits< TransformPrismaticTpl<_Scalar,_Options,_axis> >
182  {
183  enum {
184  axis = _axis,
185  Options = _Options,
186  LINEAR = 0,
187  ANGULAR = 3
188  };
189  typedef _Scalar Scalar;
190  typedef SE3Tpl<Scalar,Options> PlainType;
191  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
192  typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
193  typedef typename Matrix3::IdentityReturnType AngularType;
194  typedef AngularType AngularRef;
195  typedef AngularType ConstAngularRef;
196  typedef Vector3 LinearType;
197  typedef const Vector3 LinearRef;
198  typedef const Vector3 ConstLinearRef;
199  typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
200  typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
201  }; // traits TransformPrismaticTpl
202 
203  template<typename Scalar, int Options, int axis>
204  struct SE3GroupAction< TransformPrismaticTpl<Scalar,Options,axis> >
205  { typedef typename traits <TransformPrismaticTpl<Scalar,Options,axis> >::PlainType ReturnType; };
206 
207  template<typename _Scalar, int _Options, int axis>
208  struct TransformPrismaticTpl : SE3Base< TransformPrismaticTpl<_Scalar,_Options,axis> >
209  {
210  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
211  PINOCCHIO_SE3_TYPEDEF_TPL(TransformPrismaticTpl);
212 
213  typedef SpatialAxis<axis+LINEAR> Axis;
214  typedef typename Axis::CartesianAxis3 CartesianAxis3;
215 
216  TransformPrismaticTpl() {}
217  TransformPrismaticTpl(const Scalar & displacement)
218  : m_displacement(displacement)
219  {}
220 
221  PlainType plain() const
222  {
223  PlainType res(PlainType::Identity());
224  res.rotation().setIdentity();
225  res.translation()[axis] = m_displacement;
226 
227  return res;
228  }
229 
230  operator PlainType() const { return plain(); }
231 
232  template<typename S2, int O2>
233  typename SE3GroupAction<TransformPrismaticTpl>::ReturnType
234  se3action(const SE3Tpl<S2,O2> & m) const
235  {
236  typedef typename SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType;
237  ReturnType res(m);
238  res.translation()[axis] += m_displacement;
239 
240  return res;
241  }
242 
243  const Scalar & displacement() const { return m_displacement; }
244  Scalar & displacement() { return m_displacement; }
245 
246  ConstLinearRef translation() const { return CartesianAxis3()*displacement(); };
247  AngularType rotation() const { return AngularType(3,3); }
248 
249  bool isEqual(const TransformPrismaticTpl & other) const
250  {
251  return m_displacement == other.m_displacement;
252  }
253 
254  protected:
255 
256  Scalar m_displacement;
257  };
258 
259  template<typename Scalar, int Options, int axis> struct ConstraintPrismaticTpl;
260 
261  template<typename _Scalar, int _Options, int axis>
262  struct traits< ConstraintPrismaticTpl<_Scalar,_Options,axis> >
263  {
264  typedef _Scalar Scalar;
265  enum { Options = _Options };
266  enum {
267  LINEAR = 0,
268  ANGULAR = 3
269  };
270  typedef MotionPrismaticTpl<Scalar,Options,axis> JointMotion;
271  typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
272  typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
273  typedef DenseBase MatrixReturnType;
274  typedef const DenseBase ConstMatrixReturnType;
275  }; // traits ConstraintRevolute
276 
277  template<typename Scalar, int Options, int axis>
278  struct SE3GroupAction< ConstraintPrismaticTpl<Scalar,Options,axis> >
279  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
280 
281  template<typename Scalar, int Options, int axis, typename MotionDerived>
282  struct MotionAlgebraAction< ConstraintPrismaticTpl<Scalar,Options,axis>, MotionDerived >
283  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
284 
285  template<typename Scalar, int Options, int axis, typename ForceDerived>
286  struct ConstraintForceOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceDerived>
287  { typedef typename ForceDense<ForceDerived>::ConstLinearType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
288 
289  template<typename Scalar, int Options, int axis, typename ForceSet>
290  struct ConstraintForceSetOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceSet>
291  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
292 
293  template<typename _Scalar, int _Options, int axis>
294  struct ConstraintPrismaticTpl
295  : ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> >
296  {
297  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
298  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticTpl)
299  enum { NV = 1 };
300 
301  typedef SpatialAxis<LINEAR+axis> Axis;
302 
303  ConstraintPrismaticTpl() {};
304 
305  template<typename Vector1Like>
306  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
307  {
308  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
309  assert(v.size() == 1);
310  return JointMotion(v[0]);
311  }
312 
313  template<typename S2, int O2>
314  typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
315  se3Action(const SE3Tpl<S2,O2> & m) const
316  {
317  typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
318  MotionRef<DenseBase> v(res);
319  v.linear() = m.rotation().col(axis);
320  v.angular().setZero();
321  return res;
322  }
323 
324  template<typename S2, int O2>
325  typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
326  se3ActionInverse(const SE3Tpl<S2,O2> & m) const
327  {
328  typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
329  MotionRef<DenseBase> v(res);
330  v.linear() = m.rotation().transpose().col(axis);
331  v.angular().setZero();
332  return res;
333  }
334 
335  int nv_impl() const { return NV; }
336 
337  struct TransposeConst
338  {
339  const ConstraintPrismaticTpl & ref;
340  TransposeConst(const ConstraintPrismaticTpl & ref) : ref(ref) {}
341 
342  template<typename ForceDerived>
343  typename ConstraintForceOp<ConstraintPrismaticTpl,ForceDerived>::ReturnType
344  operator* (const ForceDense<ForceDerived> & f) const
345  { return f.linear().template segment<1>(axis); }
346 
347  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
348  template<typename Derived>
349  typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
350  operator*(const Eigen::MatrixBase<Derived> & F )
351  {
352  assert(F.rows()==6);
353  return F.row(LINEAR+axis);
354  }
355 
356  }; // struct TransposeConst
357  TransposeConst transpose() const { return TransposeConst(*this); }
358 
359  /* CRBA joint operators
360  * - ForceSet::Block = ForceSet
361  * - ForceSet operator* (Inertia Y,Constraint S)
362  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
363  * - SE3::act(ForceSet::Block)
364  */
365  DenseBase matrix_impl() const
366  {
367  DenseBase S;
368  MotionRef<DenseBase> v(S);
369  v << Axis();
370  return S;
371  }
372 
373  template<typename MotionDerived>
374  typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType
375  motionAction(const MotionDense<MotionDerived> & m) const
376  {
377  typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType res;
378  MotionRef<DenseBase> v(res);
379  v = m.cross(Axis());
380  return res;
381  }
382 
383  bool isEqual(const ConstraintPrismaticTpl &) const { return true; }
384 
385  }; // struct ConstraintPrismaticTpl
386 
387  template<typename S1, int O1,typename S2, int O2, int axis>
388  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,axis> >
389  {
390  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
391  };
392 
393  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
394  namespace impl
395  {
396  template<typename S1, int O1, typename S2, int O2>
397  struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,0> >
398  {
399  typedef InertiaTpl<S1,O1> Inertia;
400  typedef ConstraintPrismaticTpl<S2,O2,0> Constraint;
401  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
402  static inline ReturnType run(const Inertia & Y,
403  const Constraint & /*constraint*/)
404  {
405  ReturnType res;
406 
407  /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
408  const S1
409  &m = Y.mass(),
410  &y = Y.lever()[1],
411  &z = Y.lever()[2];
412  res << m, S1(0), S1(0), S1(0), m*z, -m*y;
413 
414  return res;
415  }
416  };
417 
418  template<typename S1, int O1, typename S2, int O2>
419  struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,1> >
420  {
421  typedef InertiaTpl<S1,O1> Inertia;
422  typedef ConstraintPrismaticTpl<S2,O2,1> Constraint;
423  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
424  static inline ReturnType run(const Inertia & Y,
425  const Constraint & /*constraint*/)
426  {
427  ReturnType res;
428 
429  /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
430  const S1
431  &m = Y.mass(),
432  &x = Y.lever()[0],
433  &z = Y.lever()[2];
434 
435  res << S1(0), m, S1(0), -m*z, S1(0), m*x;
436 
437  return res;
438  }
439  };
440 
441  template<typename S1, int O1, typename S2, int O2>
442  struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,2> >
443  {
444  typedef InertiaTpl<S1,O1> Inertia;
445  typedef ConstraintPrismaticTpl<S2,O2,2> Constraint;
446  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
447  static inline ReturnType run(const Inertia & Y,
448  const Constraint & /*constraint*/)
449  {
450  ReturnType res;
451 
452  /* Y(:,2) = ( 0,0, 1, y , -x , 0) */
453  const S1
454  &m = Y.mass(),
455  &x = Y.lever()[0],
456  &y = Y.lever()[1];
457 
458  res << S1(0), S1(0), m, m*y, -m*x, S1(0);
459 
460  return res;
461  }
462  };
463  } // namespace impl
464 
465  template<typename M6Like,typename S2, int O2, int axis>
466  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<S2,O2,axis> >
467  {
468  typedef typename M6Like::ConstColXpr ReturnType;
469  };
470 
471  /* [ABA] operator* (Inertia Y,Constraint S) */
472  namespace impl
473  {
474  template<typename M6Like, typename Scalar, int Options, int axis>
475  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<Scalar,Options,axis> >
476  {
477  typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint;
478  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
479  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
480  const Constraint & /*constraint*/)
481  {
482  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
483  return Y.derived().col(Inertia::LINEAR + axis);
484  }
485  };
486  } // namespace impl
487 
488  template<typename _Scalar, int _Options, int _axis>
489  struct JointPrismaticTpl
490  {
491  typedef _Scalar Scalar;
492 
493  enum
494  {
495  Options = _Options,
496  axis = _axis
497  };
498  };
499 
500  template<typename _Scalar, int _Options, int axis>
501  struct traits< JointPrismaticTpl<_Scalar,_Options,axis> >
502  {
503  enum {
504  NQ = 1,
505  NV = 1
506  };
507  typedef _Scalar Scalar;
508  enum { Options = _Options };
509  typedef JointDataPrismaticTpl<Scalar,Options,axis> JointDataDerived;
510  typedef JointModelPrismaticTpl<Scalar,Options,axis> JointModelDerived;
511  typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint_t;
512  typedef TransformPrismaticTpl<Scalar,Options,axis> Transformation_t;
513  typedef MotionPrismaticTpl<Scalar,Options,axis> Motion_t;
514  typedef MotionZeroTpl<Scalar,Options> Bias_t;
515 
516  // [ABA]
517  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
518  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
519  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
520 
521  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
522 
523  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
524  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
525  };
526 
527  template<typename Scalar, int Options, int axis>
528  struct traits< JointDataPrismaticTpl<Scalar,Options,axis> >
529  { typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; };
530 
531  template<typename Scalar, int Options, int axis>
532  struct traits< JointModelPrismaticTpl<Scalar,Options,axis> >
533  { typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; };
534 
535  template<typename _Scalar, int _Options, int axis>
536  struct JointDataPrismaticTpl : public JointDataBase< JointDataPrismaticTpl<_Scalar,_Options,axis> >
537  {
538  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
539  typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
540  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
541  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
542 
543  Constraint_t S;
544  Transformation_t M;
545  Motion_t v;
546  Bias_t c;
547 
548  // [ABA] specific data
549  U_t U;
550  D_t Dinv;
551  UD_t UDinv;
552 
553  JointDataPrismaticTpl()
554  : M((Scalar)0)
555  , v((Scalar)0)
556  , U(U_t::Zero())
557  , Dinv(D_t::Zero())
558  , UDinv(UD_t::Zero())
559  {}
560 
561  static std::string classname()
562  {
563  return std::string("JointDataP") + axisLabel<axis>();
564  }
565  std::string shortname() const { return classname(); }
566 
567  }; // struct JointDataPrismaticTpl
568 
569  template<typename NewScalar, typename Scalar, int Options, int axis>
570  struct CastType< NewScalar, JointModelPrismaticTpl<Scalar,Options,axis> >
571  {
572  typedef JointModelPrismaticTpl<NewScalar,Options,axis> type;
573  };
574 
575  template<typename _Scalar, int _Options, int axis>
576  struct JointModelPrismaticTpl
577  : public JointModelBase< JointModelPrismaticTpl<_Scalar,_Options,axis> >
578  {
579  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
580  typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
581  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
582 
583  typedef JointModelBase<JointModelPrismaticTpl> Base;
584  using Base::id;
585  using Base::idx_q;
586  using Base::idx_v;
587  using Base::setIndexes;
588 
589  JointDataDerived createData() const { return JointDataDerived(); }
590 
591  const std::vector<bool> hasConfigurationLimit() const
592  {
593  return {true};
594  }
595 
596  const std::vector<bool> hasConfigurationLimitInTangent() const
597  {
598  return {true};
599  }
600 
601  template<typename ConfigVector>
602  void calc(JointDataDerived & data,
603  const typename Eigen::MatrixBase<ConfigVector> & qs) const
604  {
605  typedef typename ConfigVector::Scalar Scalar;
606  const Scalar & q = qs[idx_q()];
607  data.M.displacement() = q;
608  }
609 
610  template<typename ConfigVector, typename TangentVector>
611  void calc(JointDataDerived & data,
612  const typename Eigen::MatrixBase<ConfigVector> & qs,
613  const typename Eigen::MatrixBase<TangentVector> & vs) const
614  {
615  calc(data,qs.derived());
616 
617  typedef typename TangentVector::Scalar S2;
618  const S2 & v = vs[idx_v()];
619  data.v.linearRate() = v;
620  }
621 
622  template<typename Matrix6Like>
623  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
624  {
625  data.U = I.col(Inertia::LINEAR + axis);
626  data.Dinv[0] = Scalar(1)/I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
627  data.UDinv.noalias() = data.U * data.Dinv[0];
628 
629  if (update_I)
630  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
631  }
632 
633  static std::string classname()
634  {
635  return std::string("JointModelP") + axisLabel<axis>();
636  }
637  std::string shortname() const { return classname(); }
638 
640  template<typename NewScalar>
641  JointModelPrismaticTpl<NewScalar,Options,axis> cast() const
642  {
643  typedef JointModelPrismaticTpl<NewScalar,Options,axis> ReturnType;
644  ReturnType res;
645  res.setIndexes(id(),idx_q(),idx_v());
646  return res;
647  }
648 
649  }; // struct JointModelPrismaticTpl
650 
651  typedef JointPrismaticTpl<double,0,0> JointPX;
652  typedef JointDataPrismaticTpl<double,0,0> JointDataPX;
653  typedef JointModelPrismaticTpl<double,0,0> JointModelPX;
654 
655  typedef JointPrismaticTpl<double,0,1> JointPY;
656  typedef JointDataPrismaticTpl<double,0,1> JointDataPY;
657  typedef JointModelPrismaticTpl<double,0,1> JointModelPY;
658 
659  typedef JointPrismaticTpl<double,0,2> JointPZ;
660  typedef JointDataPrismaticTpl<double,0,2> JointDataPZ;
661  typedef JointModelPrismaticTpl<double,0,2> JointModelPZ;
662 
663 } //namespace pinocchio
664 
665 #include <boost/type_traits.hpp>
666 
667 namespace boost
668 {
669  template<typename Scalar, int Options, int axis>
670  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
671  : public integral_constant<bool,true> {};
672 
673  template<typename Scalar, int Options, int axis>
674  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
675  : public integral_constant<bool,true> {};
676 
677  template<typename Scalar, int Options, int axis>
678  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
679  : public integral_constant<bool,true> {};
680 
681  template<typename Scalar, int Options, int axis>
682  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
683  : public integral_constant<bool,true> {};
684 }
685 
686 #endif // ifndef __pinocchio_joint_prismatic_hpp__
pinocchio::TransformPrismaticTpl
Definition: joint-prismatic.hpp:178
pinocchio::MultiplicationOp
Forward declaration of the multiplication operation return type. Should be overloaded,...
Definition: binary-op.hpp:15
pinocchio::MotionPrismaticTpl
Definition: joint-prismatic.hpp:19
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::JointModelBase
Definition: joint-model-base.hpp:67
pinocchio::ConstraintForceOp
Return type of the Constraint::Transpose * Force operation.
Definition: constraint-base.hpp:35
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::JointDataPrismaticTpl
Definition: fwd.hpp:55
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::JointPrismaticTpl
Definition: joint-prismatic.hpp:489
pinocchio::JointDataBase
Definition: joint-data-base.hpp:82
pinocchio::JointModelPrismaticTpl::cast
JointModelPrismaticTpl< NewScalar, Options, axis > cast() const
Definition: joint-prismatic.hpp:641
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
pinocchio::impl::LhsMultiplicationOp
Definition: binary-op.hpp:20
pinocchio::MotionBase
Definition: fwd.hpp:40
pinocchio::SE3GroupAction
Definition: se3.hpp:39
pinocchio::MotionZeroTpl
Definition: fwd.hpp:44
pinocchio::ForceDense::linear
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:42
pinocchio::JointModelPrismaticTpl
Definition: fwd.hpp:54
pinocchio::ConstraintPrismaticTpl::TransposeConst
Definition: joint-prismatic.hpp:337
pinocchio::ForceSetTpl
Definition: force-set.hpp:14
pinocchio::ConstraintForceSetOp
Return type of the Constraint::Transpose * ForceSet operation.
Definition: constraint-base.hpp:42
pinocchio::SE3Base
Base class for rigid transformation.
Definition: se3-base.hpp:30
pinocchio::ConstraintPrismaticTpl
Definition: joint-prismatic.hpp:259
pinocchio::ForceDense
Definition: force-dense.hpp:24
pinocchio::ConstraintBase
Definition: constraint-base.hpp:48
pinocchio::InertiaTpl
Definition: fwd.hpp:52
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::MotionTpl< Scalar, Options >
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:55
pinocchio::MotionRef
Definition: fwd.hpp:42
pinocchio::SpatialAxis
Definition: spatial-axis.hpp:15
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11