pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-spherical.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_hpp__
7 #define __pinocchio_joint_spherical_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/constraint.hpp"
12 #include "pinocchio/math/sincos.hpp"
13 #include "pinocchio/spatial/inertia.hpp"
14 #include "pinocchio/spatial/skew.hpp"
15 
16 namespace pinocchio
17 {
18 
19  template<typename Scalar, int Options = 0> struct MotionSphericalTpl;
20  typedef MotionSphericalTpl<double> MotionSpherical;
21 
22  template<typename Scalar, int Options>
23  struct SE3GroupAction< MotionSphericalTpl<Scalar,Options> >
24  {
25  typedef MotionTpl<Scalar,Options> ReturnType;
26  };
27 
28  template<typename Scalar, int Options, typename MotionDerived>
29  struct MotionAlgebraAction< MotionSphericalTpl<Scalar,Options>, MotionDerived>
30  {
31  typedef MotionTpl<Scalar,Options> ReturnType;
32  };
33 
34  template<typename _Scalar, int _Options>
35  struct traits< MotionSphericalTpl<_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 MotionSphericalTpl
58 
59  template<typename _Scalar, int _Options>
60  struct MotionSphericalTpl
61  : MotionBase< MotionSphericalTpl<_Scalar,_Options> >
62  {
63  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64 
65  MOTION_TYPEDEF_TPL(MotionSphericalTpl);
66 
67  MotionSphericalTpl() {}
68 
69  template<typename Vector3Like>
70  MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w)
71  : m_w(w)
72  {}
73 
74  Vector3 & operator() () { return m_w; }
75  const Vector3 & operator() () const { return m_w; }
76 
77  inline PlainReturnType plain() const
78  {
79  return PlainReturnType(PlainReturnType::Vector3::Zero(), m_w);
80  }
81 
82  template<typename MotionDerived>
83  void addTo(MotionDense<MotionDerived> & other) const
84  {
85  other.angular() += m_w;
86  }
87 
88  template<typename Derived>
89  void setTo(MotionDense<Derived> & other) const
90  {
91  other.linear().setZero();
92  other.angular() = m_w;
93  }
94 
95  MotionSphericalTpl __plus__(const MotionSphericalTpl & other) const
96  {
97  return MotionSphericalTpl(m_w + other.m_w);
98  }
99 
100  bool isEqual_impl(const MotionSphericalTpl & other) const
101  {
102  return m_w == other.m_w;
103  }
104 
105  template<typename MotionDerived>
106  bool isEqual_impl(const MotionDense<MotionDerived> & other) const
107  {
108  return other.angular() == m_w && other.linear().isZero(0);
109  }
110 
111  template<typename S2, int O2, typename D2>
112  void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
113  {
114  // Angular
115  v.angular().noalias() = m.rotation() * m_w;
116 
117  // Linear
118  v.linear().noalias() = m.translation().cross(v.angular());
119  }
120 
121  template<typename S2, int O2>
122  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
123  {
124  MotionPlain res;
125  se3Action_impl(m,res);
126  return res;
127  }
128 
129  template<typename S2, int O2, typename D2>
130  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
131  {
132  // Linear
133  // TODO: use v.angular() as temporary variable
134  Vector3 v3_tmp;
135  v3_tmp.noalias() = m_w.cross(m.translation());
136  v.linear().noalias() = m.rotation().transpose() * v3_tmp;
137 
138  // Angular
139  v.angular().noalias() = m.rotation().transpose() * m_w;
140  }
141 
142  template<typename S2, int O2>
143  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
144  {
145  MotionPlain res;
146  se3ActionInverse_impl(m,res);
147  return res;
148  }
149 
150  template<typename M1, typename M2>
151  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
152  {
153  // Linear
154  mout.linear().noalias() = v.linear().cross(m_w);
155 
156  // Angular
157  mout.angular().noalias() = v.angular().cross(m_w);
158  }
159 
160  template<typename M1>
161  MotionPlain motionAction(const MotionDense<M1> & v) const
162  {
163  MotionPlain res;
164  motionAction(v,res);
165  return res;
166  }
167 
168  const Vector3 & angular() const { return m_w; }
169  Vector3 & angular() { return m_w; }
170 
171  protected:
172 
173  Vector3 m_w;
174  }; // struct MotionSphericalTpl
175 
176  template<typename S1, int O1, typename MotionDerived>
177  inline typename MotionDerived::MotionPlain
178  operator+(const MotionSphericalTpl<S1,O1> & m1, const MotionDense<MotionDerived> & m2)
179  {
180  return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.angular());
181  }
182 
183  template<typename Scalar, int Options> struct ConstraintSphericalTpl;
184 
185  template<typename _Scalar, int _Options>
186  struct traits< ConstraintSphericalTpl<_Scalar,_Options> >
187  {
188  typedef _Scalar Scalar;
189  enum { Options = _Options };
190  enum {
191  LINEAR = 0,
192  ANGULAR = 3
193  };
194  typedef MotionSphericalTpl<Scalar,Options> JointMotion;
195  typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
196  typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
197  typedef DenseBase MatrixReturnType;
198  typedef const DenseBase ConstMatrixReturnType;
199  }; // struct traits struct ConstraintSphericalTpl
200 
201  template<typename _Scalar, int _Options>
202  struct ConstraintSphericalTpl
203  : public ConstraintBase< ConstraintSphericalTpl<_Scalar,_Options> >
204  {
205  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
206 
207  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalTpl)
208 
209  ConstraintSphericalTpl() {}
210 
211  enum { NV = 3 };
212 
213  int nv_impl() const { return NV; }
214 
215  template<typename Vector3Like>
216  JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & w) const
217  {
218  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
219  return JointMotion(w);
220  }
221 
222  struct TransposeConst
223  {
224  template<typename Derived>
225  typename ForceDense<Derived>::ConstAngularType
226  operator* (const ForceDense<Derived> & phi)
227  { return phi.angular(); }
228 
229  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
230  template<typename MatrixDerived>
231  const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
232  operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
233  {
234  assert(F.rows()==6);
235  return F.derived().template middleRows<3>(Inertia::ANGULAR);
236  }
237  };
238 
239  TransposeConst transpose() const { return TransposeConst(); }
240 
241  DenseBase matrix_impl() const
242  {
243  DenseBase S;
244  S.template block <3,3>(LINEAR,0).setZero();
245  S.template block <3,3>(ANGULAR,0).setIdentity();
246  return S;
247  }
248 
249  template<typename S1, int O1>
250  Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const
251  {
252  Eigen::Matrix<S1,6,3,O1> X_subspace;
253  cross(m.translation(),m.rotation(),X_subspace.template middleRows<3>(LINEAR));
254  X_subspace.template middleRows<3>(ANGULAR) = m.rotation();
255 
256  return X_subspace;
257  }
258 
259  template<typename S1, int O1>
260  Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const
261  {
262  Eigen::Matrix<S1,6,3,O1> X_subspace;
263  AxisX::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(0));
264  AxisY::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(1));
265  AxisZ::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(2));
266 
267  X_subspace.template middleRows<3>(LINEAR).noalias()
268  = m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR);
269  X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose();
270 
271  return X_subspace;
272  }
273 
274  template<typename MotionDerived>
275  DenseBase motionAction(const MotionDense<MotionDerived> & m) const
276  {
277  const typename MotionDerived::ConstLinearType v = m.linear();
278  const typename MotionDerived::ConstAngularType w = m.angular();
279 
280  DenseBase res;
281  skew(v,res.template middleRows<3>(LINEAR));
282  skew(w,res.template middleRows<3>(ANGULAR));
283 
284  return res;
285  }
286 
287  bool isEqual(const ConstraintSphericalTpl &) const { return true; }
288 
289  }; // struct ConstraintSphericalTpl
290 
291  template<typename MotionDerived, typename S2, int O2>
292  inline typename MotionDerived::MotionPlain
293  operator^(const MotionDense<MotionDerived> & m1,
294  const MotionSphericalTpl<S2,O2> & m2)
295  {
296  return m2.motionAction(m1);
297  }
298 
299  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
300  template<typename S1, int O1, typename S2, int O2>
301  inline Eigen::Matrix<S2,6,3,O2>
302  operator*(const InertiaTpl<S1,O1> & Y,
303  const ConstraintSphericalTpl<S2,O2> &)
304  {
305  typedef InertiaTpl<S1,O1> Inertia;
306  typedef typename Inertia::Symmetric3 Symmetric3;
307  Eigen::Matrix<S2,6,3,O2> M;
308  // M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
309  M.template block<3,3>(Inertia::LINEAR,0) = alphaSkew(-Y.mass(), Y.lever());
310  M.template block<3,3>(Inertia::ANGULAR,0) = (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
311  return M;
312  }
313 
314  /* [ABA] Y*S operator*/
315  template<typename M6Like, typename S2, int O2>
316  inline typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
317  operator*(const Eigen::MatrixBase<M6Like> & Y,
318  const ConstraintSphericalTpl<S2,O2> &)
319  {
320  typedef ConstraintSphericalTpl<S2,O2> Constraint;
321  return Y.derived().template middleCols<3>(Constraint::ANGULAR);
322  }
323 
324  template<typename S1, int O1>
325  struct SE3GroupAction< ConstraintSphericalTpl<S1,O1> >
326  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
327 
328  template<typename S1, int O1, typename MotionDerived>
329  struct MotionAlgebraAction< ConstraintSphericalTpl<S1,O1>,MotionDerived >
330  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
331 
332  template<typename Scalar, int Options> struct JointSphericalTpl;
333 
334  template<typename _Scalar, int _Options>
335  struct traits< JointSphericalTpl<_Scalar,_Options> >
336  {
337  enum {
338  NQ = 4,
339  NV = 3
340  };
341  typedef _Scalar Scalar;
342  enum { Options = _Options };
343  typedef JointDataSphericalTpl<Scalar,Options> JointDataDerived;
344  typedef JointModelSphericalTpl<Scalar,Options> JointModelDerived;
345  typedef ConstraintSphericalTpl<Scalar,Options> Constraint_t;
346  typedef SE3Tpl<Scalar,Options> Transformation_t;
347  typedef MotionSphericalTpl<Scalar,Options> Motion_t;
348  typedef MotionZeroTpl<Scalar,Options> Bias_t;
349 
350  // [ABA]
351  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
352  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
353  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
354 
355  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
356 
357  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
358  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
359  };
360 
361  template<typename Scalar, int Options>
362  struct traits< JointDataSphericalTpl<Scalar,Options> >
363  { typedef JointSphericalTpl<Scalar,Options> JointDerived; };
364 
365  template<typename Scalar, int Options>
366  struct traits< JointModelSphericalTpl<Scalar,Options> >
367  { typedef JointSphericalTpl<Scalar,Options> JointDerived; };
368 
369  template<typename _Scalar, int _Options>
370  struct JointDataSphericalTpl : public JointDataBase< JointDataSphericalTpl<_Scalar,_Options> >
371  {
372  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
373 
374  typedef JointSphericalTpl<_Scalar,_Options> JointDerived;
375  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
376  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
377 
378  Constraint_t S;
379  Transformation_t M;
380  Motion_t v;
381  Bias_t c;
382 
383  // [ABA] specific data
384  U_t U;
385  D_t Dinv;
386  UD_t UDinv;
387 
388  JointDataSphericalTpl ()
389  : M(Transformation_t::Identity())
390  , v(Motion_t::Vector3::Zero())
391  , U(U_t::Zero())
392  , Dinv(D_t::Zero())
393  , UDinv(UD_t::Zero())
394  {}
395 
396  static std::string classname() { return std::string("JointDataSpherical"); }
397  std::string shortname() const { return classname(); }
398 
399  }; // struct JointDataSphericalTpl
400 
401  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalTpl);
402  template<typename _Scalar, int _Options>
403  struct JointModelSphericalTpl
404  : public JointModelBase< JointModelSphericalTpl<_Scalar,_Options> >
405  {
406  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
407 
408  typedef JointSphericalTpl<_Scalar,_Options> JointDerived;
409  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
410 
411  typedef JointModelBase<JointModelSphericalTpl> Base;
412  using Base::id;
413  using Base::idx_q;
414  using Base::idx_v;
415  using Base::setIndexes;
416 
417  JointDataDerived createData() const { return JointDataDerived(); }
418 
419  const std::vector<bool> hasConfigurationLimit() const
420  {
421  return {false, false, false, false};
422  }
423 
424  const std::vector<bool> hasConfigurationLimitInTangent() const
425  {
426  return {false, false, false};
427  }
428 
429  template<typename ConfigVectorLike>
430  inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
431  {
432  typedef typename ConfigVectorLike::Scalar Scalar;
433  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
434  typedef typename Eigen::Quaternion<Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
435  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
436 
437  ConstQuaternionMap quat(q_joint.derived().data());
438  //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
439  assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4);
440 
441  M.rotation(quat.matrix());
442  M.translation().setZero();
443  }
444 
445  template<typename QuaternionDerived>
446  void calc(JointDataDerived & data,
447  const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
448  {
449  data.M.rotation(quat.matrix());
450  }
451 
452  template<typename ConfigVector>
453  EIGEN_DONT_INLINE
454  void calc(JointDataDerived & data,
455  const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
456  {
457  typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
458  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
459 
460  ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data());
461  calc(data,quat);
462  }
463 
464  template<typename ConfigVector>
465  EIGEN_DONT_INLINE
466  void calc(JointDataDerived & data,
467  const typename Eigen::MatrixBase<ConfigVector> & qs) const
468  {
469  typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
470 
471  const Quaternion quat(qs.template segment<NQ>(idx_q()));
472  calc(data,quat);
473  }
474 
475  template<typename ConfigVector, typename TangentVector>
476  void calc(JointDataDerived & data,
477  const typename Eigen::MatrixBase<ConfigVector> & qs,
478  const typename Eigen::MatrixBase<TangentVector> & vs) const
479  {
480  calc(data,qs.derived());
481 
482  data.v.angular() = vs.template segment<NV>(idx_v());
483  }
484 
485  template<typename Matrix6Like>
486  void calc_aba(JointDataDerived & data,
487  const Eigen::MatrixBase<Matrix6Like> & I,
488  const bool update_I) const
489  {
490  data.U = I.template block<6,3>(0,Inertia::ANGULAR);
491 
492  // compute inverse
493 // data.Dinv.setIdentity();
494 // data.U.template middleRows<3>(Inertia::ANGULAR).llt().solveInPlace(data.Dinv);
495  internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::ANGULAR),data.Dinv);
496 
497  data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity(); // can be put in data constructor
498  data.UDinv.template middleRows<3>(Inertia::LINEAR).noalias() = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv;
499 
500  if (update_I)
501  {
502  Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I);
503  I_.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR)
504  -= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I_.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
505  I_.template block<6,3>(0,Inertia::ANGULAR).setZero();
506  I_.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero();
507  }
508  }
509 
510  static std::string classname() { return std::string("JointModelSpherical"); }
511  std::string shortname() const { return classname(); }
512 
514  template<typename NewScalar>
515  JointModelSphericalTpl<NewScalar,Options> cast() const
516  {
517  typedef JointModelSphericalTpl<NewScalar,Options> ReturnType;
518  ReturnType res;
519  res.setIndexes(id(),idx_q(),idx_v());
520  return res;
521  }
522 
523  }; // struct JointModelSphericalTpl
524 
525 } // namespace pinocchio
526 
527 #include <boost/type_traits.hpp>
528 
529 namespace boost
530 {
531  template<typename Scalar, int Options>
532  struct has_nothrow_constructor< ::pinocchio::JointModelSphericalTpl<Scalar,Options> >
533  : public integral_constant<bool,true> {};
534 
535  template<typename Scalar, int Options>
536  struct has_nothrow_copy< ::pinocchio::JointModelSphericalTpl<Scalar,Options> >
537  : public integral_constant<bool,true> {};
538 
539  template<typename Scalar, int Options>
540  struct has_nothrow_constructor< ::pinocchio::JointDataSphericalTpl<Scalar,Options> >
541  : public integral_constant<bool,true> {};
542 
543  template<typename Scalar, int Options>
544  struct has_nothrow_copy< ::pinocchio::JointDataSphericalTpl<Scalar,Options> >
545  : public integral_constant<bool,true> {};
546 }
547 
548 #endif // ifndef __pinocchio_joint_spherical_hpp__
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::ConstraintSphericalTpl
Definition: joint-spherical.hpp:183
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
pinocchio::MotionBase
Definition: fwd.hpp:40
pinocchio::SE3GroupAction
Definition: se3.hpp:39
pinocchio::MotionZeroTpl
Definition: fwd.hpp:44
pinocchio::MotionSphericalTpl
Definition: joint-spherical.hpp:19
pinocchio::SizeDepType::RowsReturn
Definition: matrix-block.hpp:70
pinocchio::skew
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition: skew.hpp:21
pinocchio::JointModelSphericalTpl
Definition: fwd.hpp:42
pinocchio::JointDataSphericalTpl
Definition: fwd.hpp:45
pinocchio::ConstraintSphericalTpl::TransposeConst
Definition: joint-spherical.hpp:222
pinocchio::JointModelSphericalTpl::cast
JointModelSphericalTpl< NewScalar, Options > cast() const
Definition: joint-spherical.hpp:515
pinocchio::ForceDense
Definition: force-dense.hpp:24
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::MotionTpl< Scalar, Options >
pinocchio::ForceDense::angular
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
pinocchio::JointSphericalTpl
Definition: joint-spherical.hpp:332
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11