pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
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;
20  typedef MotionPrismaticUnalignedTpl<double> MotionPrismaticUnaligned;
21 
22  template<typename Scalar, int Options>
23  struct SE3GroupAction< MotionPrismaticUnalignedTpl<Scalar,Options> >
24  {
25  typedef MotionTpl<Scalar,Options> ReturnType;
26  };
27 
28  template<typename Scalar, int Options, typename MotionDerived>
29  struct MotionAlgebraAction< MotionPrismaticUnalignedTpl<Scalar,Options>, MotionDerived>
30  {
31  typedef MotionTpl<Scalar,Options> ReturnType;
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,4,4,Options> Matrix4;
42  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
43  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
44  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
45  typedef Vector3 AngularType;
46  typedef Vector3 LinearType;
47  typedef const Vector3 ConstAngularType;
48  typedef const Vector3 ConstLinearType;
49  typedef Matrix6 ActionMatrixType;
50  typedef Matrix4 HomogeneousMatrixType;
51  typedef MotionTpl<Scalar,Options> MotionPlain;
52  typedef MotionPlain PlainReturnType;
53  enum {
54  LINEAR = 0,
55  ANGULAR = 3
56  };
57  }; // traits MotionPrismaticUnalignedTpl
58 
59  template<typename _Scalar, int _Options>
60  struct MotionPrismaticUnalignedTpl
61  : MotionBase < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
62  {
63  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64  MOTION_TYPEDEF_TPL(MotionPrismaticUnalignedTpl);
65 
66  MotionPrismaticUnalignedTpl() {}
67 
68  template<typename Vector3Like, typename S2>
69  MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
70  const S2 & v)
71  : m_axis(axis), m_v(v)
72  { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
73 
74  inline PlainReturnType plain() const
75  {
76  return PlainReturnType(m_axis*m_v,
77  PlainReturnType::Vector3::Zero());
78  }
79 
80  template<typename OtherScalar>
81  MotionPrismaticUnalignedTpl __mult__(const OtherScalar & alpha) const
82  {
83  return MotionPrismaticUnalignedTpl(m_axis,alpha*m_v);
84  }
85 
86  template<typename Derived>
87  void addTo(MotionDense<Derived> & other) const
88  {
89  other.linear() += m_axis * m_v;
90  }
91 
92  template<typename Derived>
93  void setTo(MotionDense<Derived> & other) const
94  {
95  other.linear().noalias() = m_axis*m_v;
96  other.angular().setZero();
97  }
98 
99  template<typename S2, int O2, typename D2>
100  void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
101  {
102  v.linear().noalias() = m_v * (m.rotation() * m_axis); // TODO: check efficiency
103  v.angular().setZero();
104  }
105 
106  template<typename S2, int O2>
107  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
108  {
109  MotionPlain res;
110  se3Action_impl(m,res);
111  return res;
112  }
113 
114  template<typename S2, int O2, typename D2>
115  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
116  {
117  // Linear
118  v.linear().noalias() = m_v * (m.rotation().transpose() * m_axis);
119 
120  // Angular
121  v.angular().setZero();
122  }
123 
124  template<typename S2, int O2>
125  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
126  {
127  MotionPlain res;
128  se3ActionInverse_impl(m,res);
129  return res;
130  }
131 
132  template<typename M1, typename M2>
133  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
134  {
135  // Linear
136  mout.linear().noalias() = v.angular().cross(m_axis);
137  mout.linear() *= m_v;
138 
139  // Angular
140  mout.angular().setZero();
141  }
142 
143  template<typename M1>
144  MotionPlain motionAction(const MotionDense<M1> & v) const
145  {
146  MotionPlain res;
147  motionAction(v,res);
148  return res;
149  }
150 
151  bool isEqual_impl(const MotionPrismaticUnalignedTpl & other) const
152  {
153  return m_axis == other.m_axis && m_v == other.m_v;
154  }
155 
156  const Scalar & linearRate() const { return m_v; }
157  Scalar & linearRate() { return m_v; }
158 
159  const Vector3 & axis() const { return m_axis; }
160  Vector3 & axis() { return m_axis; }
161 
162  protected:
163 
164  Vector3 m_axis;
165  Scalar m_v;
166  }; // struct MotionPrismaticUnalignedTpl
167 
168  template<typename Scalar, int Options, typename MotionDerived>
169  inline typename MotionDerived::MotionPlain
170  operator+(const MotionPrismaticUnalignedTpl<Scalar,Options> & m1, const MotionDense<MotionDerived> & m2)
171  {
172  typedef typename MotionDerived::MotionPlain ReturnType;
173  return ReturnType(m1.linearRate() * m1.axis() + m2.linear(), m2.angular());
174  }
175 
176  template<typename MotionDerived, typename S2, int O2>
177  inline typename MotionDerived::MotionPlain
178  operator^(const MotionDense<MotionDerived> & m1,
179  const MotionPrismaticUnalignedTpl<S2,O2> & m2)
180  {
181  return m2.motionAction(m1);
182  }
183 
184  template<typename Scalar, int Options> struct ConstraintPrismaticUnalignedTpl;
185 
186  template<typename _Scalar, int _Options>
187  struct traits< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
188  {
189  typedef _Scalar Scalar;
190  enum { Options = _Options };
191  enum {
192  LINEAR = 0,
193  ANGULAR = 3
194  };
195  typedef MotionPrismaticUnalignedTpl<Scalar,Options> JointMotion;
196  typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
197  typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
198  typedef DenseBase MatrixReturnType;
199  typedef const DenseBase ConstMatrixReturnType;
200 
201  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
202  }; // traits ConstraintPrismaticUnalignedTpl
203 
204  template<typename Scalar, int Options>
205  struct SE3GroupAction< ConstraintPrismaticUnalignedTpl<Scalar,Options> >
206  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
207 
208  template<typename Scalar, int Options, typename MotionDerived>
209  struct MotionAlgebraAction< ConstraintPrismaticUnalignedTpl<Scalar,Options>,MotionDerived >
210  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
211 
212  template<typename Scalar, int Options, typename ForceDerived>
213  struct ConstraintForceOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceDerived>
214  {
215  typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
216  typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
217  };
218 
219  template<typename Scalar, int Options, typename ForceSet>
220  struct ConstraintForceSetOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceSet>
221  {
222  typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
223  typedef typename MatrixMatrixProduct<Eigen::Transpose<const Vector3>,
224  typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
225  >::type ReturnType;
226  };
227 
228  template<typename _Scalar, int _Options>
229  struct ConstraintPrismaticUnalignedTpl
230  : ConstraintBase< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
231  {
232  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
233  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticUnalignedTpl)
234 
235  enum { NV = 1 };
236 
237  typedef typename traits<ConstraintPrismaticUnalignedTpl>::Vector3 Vector3;
238 
239  ConstraintPrismaticUnalignedTpl() {}
240 
241  template<typename Vector3Like>
242  ConstraintPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
243  : m_axis(axis)
244  { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
245 
246  template<typename Vector1Like>
247  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
248  {
249  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
250  return JointMotion(m_axis,v[0]);
251  }
252 
253  template<typename S1, int O1>
254  typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType
255  se3Action(const SE3Tpl<S1,O1> & m) const
256  {
257  typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType res;
258  MotionRef<DenseBase> v(res);
259  v.linear().noalias() = m.rotation()*m_axis;
260  v.angular().setZero();
261  return res;
262  }
263 
264  template<typename S1, int O1>
265  typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType
266  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
267  {
268  typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType res;
269  MotionRef<DenseBase> v(res);
270  v.linear().noalias() = m.rotation().transpose()*m_axis;
271  v.angular().setZero();
272  return res;
273  }
274 
275  int nv_impl() const { return NV; }
276 
277  struct TransposeConst
278  {
279  const ConstraintPrismaticUnalignedTpl & ref;
280  TransposeConst(const ConstraintPrismaticUnalignedTpl & ref) : ref(ref) {}
281 
282  template<typename ForceDerived>
283  typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType
284  operator* (const ForceDense<ForceDerived> & f) const
285  {
286  typedef typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType ReturnType;
287  ReturnType res;
288  res[0] = ref.axis().dot(f.linear());
289  return res;
290  }
291 
292  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
293  template<typename ForceSet>
294  typename ConstraintForceSetOp<ConstraintPrismaticUnalignedTpl,ForceSet>::ReturnType
295  operator*(const Eigen::MatrixBase<ForceSet> & F)
296  {
297  EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
298  /* Return ax.T * F[1:3,:] */
299  return ref.axis().transpose() * F.template middleRows<3>(LINEAR);
300  }
301 
302  };
303 
304  TransposeConst transpose() const { return TransposeConst(*this); }
305 
306  /* CRBA joint operators
307  * - ForceSet::Block = ForceSet
308  * - ForceSet operator* (Inertia Y,Constraint S)
309  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
310  * - SE3::act(ForceSet::Block)
311  */
312  DenseBase matrix_impl() const
313  {
314  DenseBase S;
315  S.template segment<3>(LINEAR) = m_axis;
316  S.template segment<3>(ANGULAR).setZero();
317  return S;
318  }
319 
320  template<typename MotionDerived>
321  DenseBase motionAction(const MotionDense<MotionDerived> & v) const
322  {
323  DenseBase res;
324  res.template segment<3>(LINEAR).noalias() = v.angular().cross(m_axis);
325  res.template segment<3>(ANGULAR).setZero();
326 
327  return res;
328  }
329 
330  const Vector3 & axis() const { return m_axis; }
331  Vector3 & axis() { return m_axis; }
332 
333  bool isEqual(const ConstraintPrismaticUnalignedTpl & other) const
334  {
335  return m_axis == other.m_axis;
336  }
337 
338  protected:
339 
340  Vector3 m_axis;
341 
342  }; // struct ConstraintPrismaticUnalignedTpl
343 
344  template<typename S1, int O1,typename S2, int O2>
345  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticUnalignedTpl<S2,O2> >
346  {
347  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
348  };
349 
350  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
351  namespace impl
352  {
353  template<typename S1, int O1, typename S2, int O2>
354  struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticUnalignedTpl<S2,O2> >
355  {
356  typedef InertiaTpl<S1,O1> Inertia;
357  typedef ConstraintPrismaticUnalignedTpl<S2,O2> Constraint;
358  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
359 
360  static inline ReturnType run(const Inertia & Y,
361  const Constraint & cpu)
362  {
363  ReturnType res;
364  /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
365  const S1 & m = Y.mass();
366  const typename Inertia::Vector3 & c = Y.lever();
367 
368  res.template segment<3>(Constraint::LINEAR).noalias() = m*cpu.axis();
369  res.template segment<3>(Constraint::ANGULAR).noalias() = c.cross(res.template segment<3>(Constraint::LINEAR));
370 
371  return res;
372  }
373  };
374  } // namespace impl
375 
376  template<typename M6Like, typename Scalar, int Options>
377  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
378  {
379  typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
380  typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
381 
382  typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint;
383  typedef typename Constraint::Vector3 Vector3;
384  typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
385  };
386 
387  /* [ABA] operator* (Inertia Y,Constraint S) */
388  namespace impl
389  {
390  template<typename M6Like, typename Scalar, int Options>
391  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
392  {
393  typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint;
394  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
395  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
396  const Constraint & cru)
397  {
398  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
399  return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis();
400  }
401  };
402  } // namespace impl
403 
404  template<typename Scalar, int Options> struct JointPrismaticUnalignedTpl;
405 
406  template<typename _Scalar, int _Options>
407  struct traits< JointPrismaticUnalignedTpl<_Scalar,_Options> >
408  {
409  enum {
410  NQ = 1,
411  NV = 1
412  };
413  typedef _Scalar Scalar;
414  enum { Options = _Options };
415  typedef JointDataPrismaticUnalignedTpl<Scalar,Options> JointDataDerived;
416  typedef JointModelPrismaticUnalignedTpl<Scalar,Options> JointModelDerived;
417  typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint_t;
418  typedef TransformTranslationTpl<Scalar,Options> Transformation_t;
419  typedef MotionPrismaticUnalignedTpl<Scalar,Options> Motion_t;
420  typedef MotionZeroTpl<Scalar,Options> Bias_t;
421 
422  // [ABA]
423  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
424  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
425  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
426 
427  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
428 
429  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
430  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
431  };
432 
433  template<typename Scalar, int Options>
434  struct traits< JointDataPrismaticUnalignedTpl<Scalar,Options> >
435  { typedef JointPrismaticUnalignedTpl<Scalar,Options> JointDerived; };
436 
437  template<typename _Scalar, int _Options>
438  struct JointDataPrismaticUnalignedTpl
439  : public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar,_Options> >
440  {
441  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
442  typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived;
443  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
444  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
445 
446  Transformation_t M;
447  Constraint_t S;
448  Motion_t v;
449  Bias_t c;
450 
451  // [ABA] specific data
452  U_t U;
453  D_t Dinv;
454  UD_t UDinv;
455 
456  JointDataPrismaticUnalignedTpl()
457  : M(Transformation_t::Vector3::Zero())
458  , S(Constraint_t::Vector3::Zero())
459  , v(Constraint_t::Vector3::Zero(),(Scalar)0)
460  , U(U_t::Zero())
461  , Dinv(D_t::Zero())
462  , UDinv(UD_t::Zero())
463  {}
464 
465  template<typename Vector3Like>
466  JointDataPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
467  : M()
468  , S(axis)
469  , v(axis,(Scalar)NAN)
470  , U(), Dinv(), UDinv()
471  {}
472 
473  static std::string classname() { return std::string("JointDataPrismaticUnaligned"); }
474  std::string shortname() const { return classname(); }
475 
476  }; // struct JointDataPrismaticUnalignedTpl
477 
478  template<typename Scalar, int Options>
479  struct traits< JointModelPrismaticUnalignedTpl<Scalar,Options> >
480  { typedef JointPrismaticUnalignedTpl<Scalar,Options> JointDerived; };
481 
482  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelPrismaticUnalignedTpl);
483  template<typename _Scalar, int _Options>
484  struct JointModelPrismaticUnalignedTpl
485  : public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar,_Options> >
486  {
487  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
488  typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived;
489  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
490 
491  typedef JointModelBase<JointModelPrismaticUnalignedTpl> Base;
492  using Base::id;
493  using Base::idx_q;
494  using Base::idx_v;
495  using Base::setIndexes;
496 
497  typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
498 
499  JointModelPrismaticUnalignedTpl(): axis(Vector3::UnitX()) {}
500  JointModelPrismaticUnalignedTpl(const Scalar & x,
501  const Scalar & y,
502  const Scalar & z)
503  : axis(x,y,z)
504  {
505  axis.normalize();
506  assert(isUnitary(axis) && "Translation axis is not unitary");
507  }
508 
509  template<typename Vector3Like>
510  JointModelPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
511  : axis(axis)
512  {
513  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
514  assert(isUnitary(axis) && "Translation axis is not unitary");
515  }
516 
517  JointDataDerived createData() const { return JointDataDerived(axis); }
518 
519  const std::vector<bool> hasConfigurationLimit() const
520  {
521  return {true};
522  }
523 
524  const std::vector<bool> hasConfigurationLimitInTangent() const
525  {
526  return {true};
527  }
528 
529  using Base::isEqual;
530  bool isEqual(const JointModelPrismaticUnalignedTpl & other) const
531  {
532  return Base::isEqual(other) && axis == other.axis;
533  }
534 
535  template<typename ConfigVector>
536  void calc(JointDataDerived & data,
537  const typename Eigen::MatrixBase<ConfigVector> & qs) const
538  {
539  typedef typename ConfigVector::Scalar Scalar;
540  const Scalar & q = qs[idx_q()];
541 
542  data.M.translation().noalias() = axis * q;
543  }
544 
545  template<typename ConfigVector, typename TangentVector>
546  void calc(JointDataDerived & data,
547  const typename Eigen::MatrixBase<ConfigVector> & qs,
548  const typename Eigen::MatrixBase<TangentVector> & vs) const
549  {
550  calc(data,qs.derived());
551 
552  typedef typename TangentVector::Scalar S2;
553  const S2 & v = vs[idx_v()];
554  data.v.linearRate() = v;
555  }
556 
557  template<typename Matrix6Like>
558  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
559  {
560  data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) * axis;
561  data.Dinv[0] = Scalar(1)/axis.dot(data.U.template segment <3> (Inertia::LINEAR));
562  data.UDinv.noalias() = data.U * data.Dinv;
563 
564  if (update_I)
565  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
566  }
567 
568  static std::string classname() { return std::string("JointModelPrismaticUnaligned"); }
569  std::string shortname() const { return classname(); }
570 
572  template<typename NewScalar>
573  JointModelPrismaticUnalignedTpl<NewScalar,Options> cast() const
574  {
575  typedef JointModelPrismaticUnalignedTpl<NewScalar,Options> ReturnType;
576  ReturnType res(axis.template cast<NewScalar>());
577  res.setIndexes(id(),idx_q(),idx_v());
578  return res;
579  }
580 
581  // data
582 
586  Vector3 axis;
587  }; // struct JointModelPrismaticUnalignedTpl
588 
589 } //namespace pinocchio
590 
591 #include <boost/type_traits.hpp>
592 
593 namespace boost
594 {
595  template<typename Scalar, int Options>
596  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
597  : public integral_constant<bool,true> {};
598 
599  template<typename Scalar, int Options>
600  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
601  : public integral_constant<bool,true> {};
602 
603  template<typename Scalar, int Options>
604  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
605  : public integral_constant<bool,true> {};
606 
607  template<typename Scalar, int Options>
608  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
609  : public integral_constant<bool,true> {};
610 }
611 
612 
613 #endif // ifndef __pinocchio_joint_prismatic_unaligned_hpp__
pinocchio::MatrixMatrixProduct
Definition: matrix.hpp:64
pinocchio::MotionPrismaticUnalignedTpl
Definition: joint-prismatic-unaligned.hpp:19
pinocchio::JointModelPrismaticUnalignedTpl::axis
Vector3 axis
3d main axis of the joint.
Definition: joint-prismatic-unaligned.hpp:586
pinocchio::MultiplicationOp
Forward declaration of the multiplication operation return type. Should be overloaded,...
Definition: binary-op.hpp:15
pinocchio::isUnitary
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
Definition: matrix.hpp:140
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::JointPrismaticUnalignedTpl
Definition: joint-prismatic-unaligned.hpp:404
pinocchio::JointDataBase
Definition: joint-data-base.hpp:82
pinocchio::JointModelPrismaticUnalignedTpl::cast
JointModelPrismaticUnalignedTpl< NewScalar, Options > cast() const
Definition: joint-prismatic-unaligned.hpp:573
pinocchio::ConstraintPrismaticUnalignedTpl::TransposeConst
Definition: joint-prismatic-unaligned.hpp:277
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::ForceSetTpl
Definition: force-set.hpp:14
pinocchio::ConstraintForceSetOp
Return type of the Constraint::Transpose * ForceSet operation.
Definition: constraint-base.hpp:42
pinocchio::JointModelPrismaticUnalignedTpl
Definition: fwd.hpp:57
pinocchio::SizeDepType::ColsReturn
Definition: matrix-block.hpp:43
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::ConstraintPrismaticUnalignedTpl
Definition: joint-prismatic-unaligned.hpp:184
pinocchio::MotionTpl< Scalar, Options >
pinocchio::JointDataPrismaticUnalignedTpl
Definition: fwd.hpp:60
pinocchio::TransformTranslationTpl
Definition: joint-translation.hpp:177
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11