joint-spherical.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_multibody_joint_spherical_hpp__
7 #define __pinocchio_multibody_joint_spherical_hpp__
8 
9 #include "pinocchio/macros.hpp"
15 
16 namespace pinocchio
17 {
18 
19  template<typename Scalar, int Options = context::Options>
22 
23  template<typename Scalar, int Options>
25  {
27  };
28 
29  template<typename Scalar, int Options, typename MotionDerived>
31  {
33  };
34 
35  template<typename _Scalar, int _Options>
36  struct traits<MotionSphericalTpl<_Scalar, _Options>>
37  {
38  typedef _Scalar Scalar;
39  enum
40  {
41  Options = _Options
42  };
43  typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
44  typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
45  typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
46  typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
47  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
48  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
51  typedef const Vector3 ConstAngularType;
52  typedef const Vector3 ConstLinearType;
57  enum
58  {
59  LINEAR = 0,
60  ANGULAR = 3
61  };
62  }; // traits MotionSphericalTpl
63 
64  template<typename _Scalar, int _Options>
65  struct MotionSphericalTpl : MotionBase<MotionSphericalTpl<_Scalar, _Options>>
66  {
67  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68 
70 
72  {
73  }
74 
75  template<typename Vector3Like>
76  MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w)
77  : m_w(w)
78  {
79  }
80 
82  {
83  return m_w;
84  }
85  const Vector3 & operator()() const
86  {
87  return m_w;
88  }
89 
90  inline PlainReturnType plain() const
91  {
92  return PlainReturnType(PlainReturnType::Vector3::Zero(), m_w);
93  }
94 
95  template<typename MotionDerived>
96  void addTo(MotionDense<MotionDerived> & other) const
97  {
98  other.angular() += m_w;
99  }
100 
101  template<typename Derived>
102  void setTo(MotionDense<Derived> & other) const
103  {
104  other.linear().setZero();
105  other.angular() = m_w;
106  }
107 
109  {
110  return MotionSphericalTpl(m_w + other.m_w);
111  }
112 
113  bool isEqual_impl(const MotionSphericalTpl & other) const
114  {
115  return internal::comparison_eq(m_w, other.m_w);
116  }
117 
118  template<typename MotionDerived>
119  bool isEqual_impl(const MotionDense<MotionDerived> & other) const
120  {
121  return internal::comparison_eq(other.angular(), m_w) && other.linear().isZero(0);
122  }
123 
124  template<typename S2, int O2, typename D2>
126  {
127  // Angular
128  v.angular().noalias() = m.rotation() * m_w;
129 
130  // Linear
131  v.linear().noalias() = m.translation().cross(v.angular());
132  }
133 
134  template<typename S2, int O2>
135  MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
136  {
137  MotionPlain res;
138  se3Action_impl(m, res);
139  return res;
140  }
141 
142  template<typename S2, int O2, typename D2>
144  {
145  // Linear
146  // TODO: use v.angular() as temporary variable
147  Vector3 v3_tmp;
148  v3_tmp.noalias() = m_w.cross(m.translation());
149  v.linear().noalias() = m.rotation().transpose() * v3_tmp;
150 
151  // Angular
152  v.angular().noalias() = m.rotation().transpose() * m_w;
153  }
154 
155  template<typename S2, int O2>
156  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
157  {
158  MotionPlain res;
160  return res;
161  }
162 
163  template<typename M1, typename M2>
164  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
165  {
166  // Linear
167  mout.linear().noalias() = v.linear().cross(m_w);
168 
169  // Angular
170  mout.angular().noalias() = v.angular().cross(m_w);
171  }
172 
173  template<typename M1>
174  MotionPlain motionAction(const MotionDense<M1> & v) const
175  {
176  MotionPlain res;
177  motionAction(v, res);
178  return res;
179  }
180 
181  const Vector3 & angular() const
182  {
183  return m_w;
184  }
186  {
187  return m_w;
188  }
189 
190  protected:
192  }; // struct MotionSphericalTpl
193 
194  template<typename S1, int O1, typename MotionDerived>
195  inline typename MotionDerived::MotionPlain
197  {
198  return typename MotionDerived::MotionPlain(m2.linear(), m2.angular() + m1.angular());
199  }
200 
201  template<typename Scalar, int Options>
203 
204  template<typename _Scalar, int _Options>
205  struct traits<JointMotionSubspaceSphericalTpl<_Scalar, _Options>>
206  {
207  typedef _Scalar Scalar;
208  enum
209  {
210  Options = _Options
211  };
212  enum
213  {
214  LINEAR = 0,
215  ANGULAR = 3
216  };
218  typedef Eigen::Matrix<Scalar, 3, 1, Options> JointForce;
219  typedef Eigen::Matrix<Scalar, 6, 3, Options> DenseBase;
220  typedef Eigen::Matrix<Scalar, 3, 3, Options> ReducedSquaredMatrix;
221 
224 
225  typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
226  }; // struct traits struct JointMotionSubspaceSphericalTpl
227 
228  template<typename _Scalar, int _Options>
230  : public JointMotionSubspaceBase<JointMotionSubspaceSphericalTpl<_Scalar, _Options>>
231  {
232  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
233 
235 
237  {
238  }
239 
240  enum
241  {
242  NV = 3
243  };
244 
245  int nv_impl() const
246  {
247  return NV;
248  }
249 
250  template<typename Vector3Like>
251  JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & w) const
252  {
253  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
254  return JointMotion(w);
255  }
256 
257  struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceSphericalTpl>
258  {
259  template<typename Derived>
261  {
262  return phi.angular();
263  }
264 
265  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
266  template<typename MatrixDerived>
267  const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
268  operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
269  {
270  assert(F.rows() == 6);
271  return F.derived().template middleRows<3>(Inertia::ANGULAR);
272  }
273  };
274 
276  {
277  return TransposeConst();
278  }
279 
280  DenseBase matrix_impl() const
281  {
282  DenseBase S;
283  S.template block<3, 3>(LINEAR, 0).setZero();
284  S.template block<3, 3>(ANGULAR, 0).setIdentity();
285  return S;
286  }
287 
288  template<typename S1, int O1>
289  Eigen::Matrix<S1, 6, 3, O1> se3Action(const SE3Tpl<S1, O1> & m) const
290  {
291  Eigen::Matrix<S1, 6, 3, O1> X_subspace;
292  cross(m.translation(), m.rotation(), X_subspace.template middleRows<3>(LINEAR));
293  X_subspace.template middleRows<3>(ANGULAR) = m.rotation();
294 
295  return X_subspace;
296  }
297 
298  template<typename S1, int O1>
299  Eigen::Matrix<S1, 6, 3, O1> se3ActionInverse(const SE3Tpl<S1, O1> & m) const
300  {
301  Eigen::Matrix<S1, 6, 3, O1> X_subspace;
302  XAxis::cross(m.translation(), X_subspace.template middleRows<3>(ANGULAR).col(0));
303  YAxis::cross(m.translation(), X_subspace.template middleRows<3>(ANGULAR).col(1));
304  ZAxis::cross(m.translation(), X_subspace.template middleRows<3>(ANGULAR).col(2));
305 
306  X_subspace.template middleRows<3>(LINEAR).noalias() =
307  m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR);
308  X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose();
309 
310  return X_subspace;
311  }
312 
313  template<typename MotionDerived>
314  DenseBase motionAction(const MotionDense<MotionDerived> & m) const
315  {
316  const typename MotionDerived::ConstLinearType v = m.linear();
317  const typename MotionDerived::ConstAngularType w = m.angular();
318 
319  DenseBase res;
320  skew(v, res.template middleRows<3>(LINEAR));
321  skew(w, res.template middleRows<3>(ANGULAR));
322 
323  return res;
324  }
325 
327  {
328  return true;
329  }
330 
331  }; // struct JointMotionSubspaceSphericalTpl
332 
333  template<typename MotionDerived, typename S2, int O2>
334  inline typename MotionDerived::MotionPlain
336  {
337  return m2.motionAction(m1);
338  }
339 
340  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
341  template<typename S1, int O1, typename S2, int O2>
342  inline Eigen::Matrix<S2, 6, 3, O2>
344  {
345  typedef InertiaTpl<S1, O1> Inertia;
346  typedef typename Inertia::Symmetric3 Symmetric3;
347  Eigen::Matrix<S2, 6, 3, O2> M;
348  // M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
349  M.template block<3, 3>(Inertia::LINEAR, 0) = alphaSkew(-Y.mass(), Y.lever());
350  M.template block<3, 3>(Inertia::ANGULAR, 0) =
351  (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
352  return M;
353  }
354 
355  /* [ABA] Y*S operator*/
356  template<typename M6Like, typename S2, int O2>
357  inline typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
358  operator*(const Eigen::MatrixBase<M6Like> & Y, const JointMotionSubspaceSphericalTpl<S2, O2> &)
359  {
360  typedef JointMotionSubspaceSphericalTpl<S2, O2> Constraint;
361  return Y.derived().template middleCols<3>(Constraint::ANGULAR);
362  }
363 
364  template<typename S1, int O1>
366  {
367  typedef Eigen::Matrix<S1, 6, 3, O1> ReturnType;
368  };
369 
370  template<typename S1, int O1, typename MotionDerived>
372  {
373  typedef Eigen::Matrix<S1, 6, 3, O1> ReturnType;
374  };
375 
376  template<typename Scalar, int Options>
378 
379  template<typename _Scalar, int _Options>
380  struct traits<JointSphericalTpl<_Scalar, _Options>>
381  {
382  enum
383  {
384  NQ = 4,
385  NV = 3
386  };
387  typedef _Scalar Scalar;
388  enum
389  {
390  Options = _Options
391  };
398 
399  // [ABA]
400  typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
401  typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
402  typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
403 
404  typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
405  typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
406 
408  };
409 
410  template<typename _Scalar, int _Options>
411  struct traits<JointDataSphericalTpl<_Scalar, _Options>>
412  {
414  typedef _Scalar Scalar;
415  };
416 
417  template<typename _Scalar, int _Options>
418  struct traits<JointModelSphericalTpl<_Scalar, _Options>>
419  {
421  typedef _Scalar Scalar;
422  };
423 
424  template<typename _Scalar, int _Options>
425  struct JointDataSphericalTpl : public JointDataBase<JointDataSphericalTpl<_Scalar, _Options>>
426  {
427  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
428 
432 
433  ConfigVector_t joint_q;
434  TangentVector_t joint_v;
435 
436  Constraint_t S;
437  Transformation_t M;
438  Motion_t v;
439  Bias_t c;
440 
441  // [ABA] specific data
442  U_t U;
443  D_t Dinv;
444  UD_t UDinv;
445  D_t StU;
446 
448  : joint_q(Scalar(0), Scalar(0), Scalar(0), Scalar(1))
449  , joint_v(TangentVector_t::Zero())
450  , M(Transformation_t::Identity())
451  , v(Motion_t::Vector3::Zero())
452  , U(U_t::Zero())
453  , Dinv(D_t::Zero())
454  , UDinv(UD_t::Zero())
455  , StU(D_t::Zero())
456  {
457  }
458 
459  static std::string classname()
460  {
461  return std::string("JointDataSpherical");
462  }
463  std::string shortname() const
464  {
465  return classname();
466  }
467 
468  }; // struct JointDataSphericalTpl
469 
470  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalTpl);
471  template<typename _Scalar, int _Options>
472  struct JointModelSphericalTpl : public JointModelBase<JointModelSphericalTpl<_Scalar, _Options>>
473  {
474  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
475 
478 
480  using Base::id;
481  using Base::idx_q;
482  using Base::idx_v;
483  using Base::setIndexes;
484 
485  JointDataDerived createData() const
486  {
487  return JointDataDerived();
488  }
489 
490  const std::vector<bool> hasConfigurationLimit() const
491  {
492  return {false, false, false, false};
493  }
494 
495  const std::vector<bool> hasConfigurationLimitInTangent() const
496  {
497  return {false, false, false};
498  }
499 
500  template<typename ConfigVectorLike>
501  inline void forwardKinematics(
502  Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
503  {
504  typedef typename ConfigVectorLike::Scalar Scalar;
505  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t, ConfigVectorLike);
506  typedef
507  typename Eigen::Quaternion<Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options>
508  Quaternion;
509  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
510 
511  ConstQuaternionMap quat(q_joint.derived().data());
512  // assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename
513  // V::Scalar>::epsilon())); TODO: check validity of the rhs precision
514  assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm() - 1)) <= 1e-4);
515 
516  M.rotation(quat.matrix());
517  M.translation().setZero();
518  }
519 
520  template<typename QuaternionDerived>
521  void calc(
522  JointDataDerived & data, const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
523  {
524  data.joint_q = quat.coeffs();
525  data.M.rotation(quat.matrix());
526  }
527 
528  template<typename ConfigVector>
529  EIGEN_DONT_INLINE void
530  calc(JointDataDerived & data, const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
531  {
532  typedef typename Eigen::Quaternion<typename ConfigVector::Scalar, ConfigVector::Options>
533  Quaternion;
534  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
535 
536  ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data());
537  calc(data, quat);
538  }
539 
540  template<typename ConfigVector>
541  EIGEN_DONT_INLINE void
542  calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
543  {
544  typedef typename Eigen::Quaternion<Scalar, Options> Quaternion;
545 
546  const Quaternion quat(qs.template segment<NQ>(idx_q()));
547  calc(data, quat);
548  }
549 
550  template<typename TangentVector>
551  void
552  calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
553  const
554  {
555  data.joint_v = vs.template segment<NV>(idx_v());
556  data.v.angular() = data.joint_v;
557  }
558 
559  template<typename ConfigVector, typename TangentVector>
560  void calc(
561  JointDataDerived & data,
562  const typename Eigen::MatrixBase<ConfigVector> & qs,
563  const typename Eigen::MatrixBase<TangentVector> & vs) const
564  {
565  calc(data, qs.derived());
566  data.joint_v = vs.template segment<NV>(idx_v());
567  data.v.angular() = data.joint_v;
568  }
569 
570  template<typename VectorLike, typename Matrix6Like>
571  void calc_aba(
572  JointDataDerived & data,
573  const Eigen::MatrixBase<VectorLike> & armature,
574  const Eigen::MatrixBase<Matrix6Like> & I,
575  const bool update_I) const
576  {
577  data.U = I.template block<6, 3>(0, Inertia::ANGULAR);
578  data.StU = data.U.template middleRows<3>(Inertia::ANGULAR);
579  data.StU.diagonal() += armature;
580 
582  data.UDinv.noalias() = data.U * data.Dinv;
583 
584  if (update_I)
585  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
586  }
587 
588  static std::string classname()
589  {
590  return std::string("JointModelSpherical");
591  }
592  std::string shortname() const
593  {
594  return classname();
595  }
596 
598  template<typename NewScalar>
600  {
602  ReturnType res;
603  res.setIndexes(id(), idx_q(), idx_v());
604  return res;
605  }
606 
607  }; // struct JointModelSphericalTpl
608 
609 } // namespace pinocchio
610 
611 #include <boost/type_traits.hpp>
612 
613 namespace boost
614 {
615  template<typename Scalar, int Options>
616  struct has_nothrow_constructor<::pinocchio::JointModelSphericalTpl<Scalar, Options>>
617  : public integral_constant<bool, true>
618  {
619  };
620 
621  template<typename Scalar, int Options>
622  struct has_nothrow_copy<::pinocchio::JointModelSphericalTpl<Scalar, Options>>
623  : public integral_constant<bool, true>
624  {
625  };
626 
627  template<typename Scalar, int Options>
628  struct has_nothrow_constructor<::pinocchio::JointDataSphericalTpl<Scalar, Options>>
629  : public integral_constant<bool, true>
630  {
631  };
632 
633  template<typename Scalar, int Options>
634  struct has_nothrow_copy<::pinocchio::JointDataSphericalTpl<Scalar, Options>>
635  : public integral_constant<bool, true>
636  {
637  };
638 } // namespace boost
639 
640 #endif // ifndef __pinocchio_multibody_joint_spherical_hpp__
pinocchio::JointMotionSubspaceSphericalTpl::matrix_impl
DenseBase matrix_impl() const
Definition: joint-spherical.hpp:280
pinocchio::MotionSphericalTpl::MOTION_TYPEDEF_TPL
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MOTION_TYPEDEF_TPL(MotionSphericalTpl)
pinocchio::InertiaTpl
Definition: spatial/fwd.hpp:58
pinocchio::traits< JointModelSphericalTpl< _Scalar, _Options > >::JointDerived
JointSphericalTpl< _Scalar, _Options > JointDerived
Definition: joint-spherical.hpp:420
pinocchio::traits< JointMotionSubspaceSphericalTpl< _Scalar, _Options > >::JointForce
Eigen::Matrix< Scalar, 3, 1, Options > JointForce
Definition: joint-spherical.hpp:218
pinocchio::MotionSpherical
MotionSphericalTpl< context::Scalar > MotionSpherical
Definition: joint-spherical.hpp:20
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
Definition: joint-data-base.hpp:137
sincos.hpp
pinocchio::Symmetric3
Symmetric3Tpl< context::Scalar, context::Options > Symmetric3
Definition: spatial/fwd.hpp:70
test-cpp2pybind11.m
m
Definition: test-cpp2pybind11.py:25
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::ConfigVector_t
Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
Definition: joint-spherical.hpp:404
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::Motion_t
MotionSphericalTpl< Scalar, Options > Motion_t
Definition: joint-spherical.hpp:396
pinocchio::traits< JointMotionSubspaceSphericalTpl< _Scalar, _Options > >::DenseBase
Eigen::Matrix< Scalar, 6, 3, Options > DenseBase
Definition: joint-spherical.hpp:219
pinocchio::JointDataSphericalTpl::UDinv
UD_t UDinv
Definition: joint-spherical.hpp:444
pinocchio::JointMotionSubspaceSphericalTpl::TransposeConst::operator*
const SizeDepType< 3 >::RowsReturn< MatrixDerived >::ConstType operator*(const Eigen::MatrixBase< MatrixDerived > &F) const
Definition: joint-spherical.hpp:268
macros.hpp
pinocchio::CartesianAxis::cross
static void cross(const Eigen::MatrixBase< V3_in > &vin, const Eigen::MatrixBase< V3_out > &vout)
pinocchio::MotionSphericalTpl::addTo
void addTo(MotionDense< MotionDerived > &other) const
Definition: joint-spherical.hpp:96
pinocchio::JointDataSphericalTpl::Dinv
D_t Dinv
Definition: joint-spherical.hpp:443
pinocchio::JointModelSphericalTpl::classname
static std::string classname()
Definition: joint-spherical.hpp:588
pinocchio::traits< JointDataSphericalTpl< _Scalar, _Options > >::JointDerived
JointSphericalTpl< _Scalar, _Options > JointDerived
Definition: joint-spherical.hpp:413
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::JointMotionSubspaceSphericalTpl::TransposeConst::operator*
ForceDense< Derived >::ConstAngularType operator*(const ForceDense< Derived > &phi)
Definition: joint-spherical.hpp:260
pinocchio::JointModelBase
Definition: joint-model-base.hpp:75
pinocchio::MotionAlgebraAction< MotionSphericalTpl< Scalar, Options >, MotionDerived >::ReturnType
MotionTpl< Scalar, Options > ReturnType
Definition: joint-spherical.hpp:32
quat
quat
pinocchio::JointDataSphericalTpl::M
Transformation_t M
Definition: joint-spherical.hpp:437
pinocchio::JointModelSphericalTpl::calc
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
Definition: joint-spherical.hpp:560
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::Options
Options
Definition: joint-configuration.hpp:1082
pinocchio::JointMotionSubspaceBase
Definition: joint-motion-subspace-base.hpp:59
pinocchio::MotionSphericalTpl::MotionSphericalTpl
MotionSphericalTpl(const Eigen::MatrixBase< Vector3Like > &w)
Definition: joint-spherical.hpp:76
pinocchio::Inertia
InertiaTpl< context::Scalar, context::Options > Inertia
Definition: spatial/fwd.hpp:69
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::JointDataDerived
JointDataSphericalTpl< Scalar, Options > JointDataDerived
Definition: joint-spherical.hpp:392
PINOCCHIO_EIGEN_CONST_CAST
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
Definition: eigen-macros.hpp:51
pinocchio::SE3Tpl
Definition: context/casadi.hpp:29
pinocchio::MotionSphericalTpl::isEqual_impl
bool isEqual_impl(const MotionSphericalTpl &other) const
Definition: joint-spherical.hpp:113
pinocchio::MotionSphericalTpl::se3ActionInverse_impl
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
Definition: joint-spherical.hpp:156
pinocchio::internal::PerformStYSInversion::run
static EIGEN_STRONG_INLINE void run(const Eigen::MatrixBase< M1 > &StYS, const Eigen::MatrixBase< M2 > &Dinv)
Definition: joint-common-operations.hpp:25
pinocchio::JointDataBase
Definition: joint-data-base.hpp:161
pinocchio::JointDataSphericalTpl::classname
static std::string classname()
Definition: joint-spherical.hpp:459
pinocchio::JointModelSphericalTpl::calc
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::PlainObjectBase< ConfigVector > &qs) const
Definition: joint-spherical.hpp:530
pinocchio::PINOCCHIO_EIGEN_REF_CONST_TYPE
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
Definition: joint-free-flyer.hpp:144
setup.data
data
Definition: cmake/cython/setup.in.py:48
inertia.hpp
pinocchio::traits< JointMotionSubspaceSphericalTpl< _Scalar, _Options > >::MatrixReturnType
DenseBase MatrixReturnType
Definition: joint-spherical.hpp:222
pinocchio::JointMotionSubspaceSphericalTpl::TransposeConst
Definition: joint-spherical.hpp:257
pinocchio::MotionDense
Definition: context/casadi.hpp:36
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::operator+
MotionDerived::MotionPlain operator+(const MotionHelicalUnalignedTpl< S1, O1 > &m1, const MotionDense< MotionDerived > &m2)
Definition: joint-helical-unaligned.hpp:211
boost
pinocchio::JointModelSphericalTpl::shortname
std::string shortname() const
Definition: joint-spherical.hpp:592
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::HomogeneousMatrixType
Matrix4 HomogeneousMatrixType
Definition: joint-spherical.hpp:54
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::MotionPlain
MotionTpl< Scalar, Options > MotionPlain
Definition: joint-spherical.hpp:55
pinocchio::JointModelBase::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
pinocchio::traits< JointMotionSubspaceSphericalTpl< _Scalar, _Options > >::ConstMatrixReturnType
const typedef DenseBase ConstMatrixReturnType
Definition: joint-spherical.hpp:223
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
pinocchio::MotionSphericalTpl::__plus__
MotionSphericalTpl __plus__(const MotionSphericalTpl &other) const
Definition: joint-spherical.hpp:108
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::LinearType
Vector3 LinearType
Definition: joint-spherical.hpp:50
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: spatial/motion.hpp:45
pinocchio::MotionSphericalTpl::operator()
Vector3 & operator()()
Definition: joint-spherical.hpp:81
pinocchio::JointMotionSubspaceSphericalTpl::NV
@ NV
Definition: joint-spherical.hpp:242
pinocchio::JointModelSphericalTpl::calc_aba
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< VectorLike > &armature, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
Definition: joint-spherical.hpp:571
pinocchio::operator*
TridiagonalSymmetricMatrixApplyOnTheLeftReturnType< LhsMatrixType, TridiagonalSymmetricMatrixTpl< S, O > > operator*(const Eigen::MatrixBase< LhsMatrixType > &lhs, const TridiagonalSymmetricMatrixTpl< S, O > &rhs)
Definition: math/tridiagonal-matrix.hpp:319
pinocchio::JointModelSphericalTpl::hasConfigurationLimitInTangent
const std::vector< bool > hasConfigurationLimitInTangent() const
Definition: joint-spherical.hpp:495
pinocchio::Blank
Blank type.
Definition: fwd.hpp:76
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: joint-spherical.hpp:38
pinocchio::MotionDense::linear
ConstLinearType linear() const
Definition: motion-base.hpp:36
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::ConstAngularType
const typedef Vector3 ConstAngularType
Definition: joint-spherical.hpp:51
pinocchio::MotionSphericalTpl::motionAction
MotionPlain motionAction(const MotionDense< M1 > &v) const
Definition: joint-spherical.hpp:174
pinocchio::SE3GroupAction
Definition: spatial/se3.hpp:39
pinocchio::JointModelSphericalTpl::calc
void calc(JointDataDerived &data, const Blank, const typename Eigen::MatrixBase< TangentVector > &vs) const
Definition: joint-spherical.hpp:552
skew.hpp
pinocchio::MotionZeroTpl
Definition: context/casadi.hpp:23
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
Definition: joint-data-base.hpp:55
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::Constraint_t
JointMotionSubspaceSphericalTpl< Scalar, Options > Constraint_t
Definition: joint-spherical.hpp:394
pinocchio::python::context::Quaternion
Eigen::Quaternion< Scalar, Options > Quaternion
Definition: bindings/python/context/generic.hpp:49
pinocchio::JointMotionSubspaceSphericalTpl::se3Action
Eigen::Matrix< S1, 6, 3, O1 > se3Action(const SE3Tpl< S1, O1 > &m) const
Definition: joint-spherical.hpp:289
pinocchio::JointMotionSubspaceSphericalTpl::nv_impl
int nv_impl() const
Definition: joint-spherical.hpp:245
pinocchio::python::context::Symmetric3
Symmetric3Tpl< Scalar, Options > Symmetric3
Definition: bindings/python/context/generic.hpp:59
pinocchio::MotionSphericalTpl
Definition: joint-spherical.hpp:20
simulation-contact-dynamics.S
S
Definition: simulation-contact-dynamics.py:80
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: joint-spherical.hpp:387
pinocchio::SizeDepType::RowsReturn
Definition: matrix-block.hpp:70
pinocchio::JointModelSphericalTpl::PINOCCHIO_JOINT_TYPEDEF_TEMPLATE
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
pinocchio::MotionSphericalTpl::motionAction
void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
Definition: joint-spherical.hpp:164
pinocchio::MotionSphericalTpl::operator()
const Vector3 & operator()() const
Definition: joint-spherical.hpp:85
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::Vector6
Eigen::Matrix< Scalar, 6, 1, Options > Vector6
Definition: joint-spherical.hpp:44
pinocchio::JointDataSphericalTpl::c
Bias_t c
Definition: joint-spherical.hpp:439
pinocchio::PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
Free-flyer joint in .
pinocchio::JointDataSphericalTpl::U
U_t U
Definition: joint-spherical.hpp:442
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:22
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::TangentVector_t
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
Definition: joint-spherical.hpp:405
pinocchio::traits< JointMotionSubspaceSphericalTpl< _Scalar, _Options > >::JointMotion
MotionSphericalTpl< Scalar, Options > JointMotion
Definition: joint-spherical.hpp:217
pinocchio::JointModelSphericalTpl::Base
JointModelBase< JointModelSphericalTpl > Base
Definition: joint-spherical.hpp:479
pinocchio::JointDataSphericalTpl::shortname
std::string shortname() const
Definition: joint-spherical.hpp:463
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::ActionMatrixType
Matrix6 ActionMatrixType
Definition: joint-spherical.hpp:53
pinocchio::JointMotionSubspaceSphericalTpl::__mult__
JointMotion __mult__(const Eigen::MatrixBase< Vector3Like > &w) const
Definition: joint-spherical.hpp:251
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::D_t
Eigen::Matrix< Scalar, NV, NV, Options > D_t
Definition: joint-spherical.hpp:401
pinocchio::MotionSphericalTpl::setTo
void setTo(MotionDense< Derived > &other) const
Definition: joint-spherical.hpp:102
pinocchio::JointDataSphericalTpl::S
Constraint_t S
Definition: joint-spherical.hpp:436
joint-base.hpp
pinocchio::JointDataSphericalTpl::JointDataSphericalTpl
JointDataSphericalTpl()
Definition: joint-spherical.hpp:447
pinocchio::JointModelSphericalTpl
Definition: multibody/joint/fwd.hpp:73
pinocchio::JointModelSphericalTpl::hasConfigurationLimit
const std::vector< bool > hasConfigurationLimit() const
Definition: joint-spherical.hpp:490
pinocchio::MotionSphericalTpl::m_w
Vector3 m_w
Definition: joint-spherical.hpp:191
pinocchio::JointModelSphericalTpl::JointDerived
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointSphericalTpl< _Scalar, _Options > JointDerived
Definition: joint-spherical.hpp:476
pinocchio::JointDataSphericalTpl
Definition: multibody/joint/fwd.hpp:77
M
M
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
pinocchio::SE3GroupAction< MotionSphericalTpl< Scalar, Options > >::ReturnType
MotionTpl< Scalar, Options > ReturnType
Definition: joint-spherical.hpp:26
ur5x4.w
w
Definition: ur5x4.py:50
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: joint-spherical.hpp:43
pinocchio::JointDataSphericalTpl::v
Motion_t v
Definition: joint-spherical.hpp:438
pinocchio::JointDataSphericalTpl::JointDerived
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointSphericalTpl< _Scalar, _Options > JointDerived
Definition: joint-spherical.hpp:429
pinocchio::MotionSphericalTpl::isEqual_impl
bool isEqual_impl(const MotionDense< MotionDerived > &other) const
Definition: joint-spherical.hpp:119
pinocchio::JointMotionSubspaceSphericalTpl::transpose
TransposeConst transpose() const
Definition: joint-spherical.hpp:275
pinocchio::JointMotionSubspaceSphericalTpl::motionAction
DenseBase motionAction(const MotionDense< MotionDerived > &m) const
Definition: joint-spherical.hpp:314
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::Matrix4
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
Definition: joint-spherical.hpp:45
pinocchio::JointModelSphericalTpl::forwardKinematics
void forwardKinematics(Transformation_t &M, const Eigen::MatrixBase< ConfigVectorLike > &q_joint) const
Definition: joint-spherical.hpp:501
pinocchio::MotionSphericalTpl::MotionSphericalTpl
MotionSphericalTpl()
Definition: joint-spherical.hpp:71
pinocchio::MotionSphericalTpl::se3Action_impl
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
Definition: joint-spherical.hpp:125
pinocchio::traits< JointDataSphericalTpl< _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: joint-spherical.hpp:414
pinocchio::JointModelBase::idx_v
int idx_v() const
Definition: joint-model-base.hpp:164
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
pinocchio::internal::comparison_eq
bool comparison_eq(const LhsType &lhs_value, const RhsType &rhs_value)
Definition: utils/static-if.hpp:120
pinocchio::JointDataSphericalTpl::StU
D_t StU
Definition: joint-spherical.hpp:445
pinocchio::JointMotionSubspaceSphericalTpl::se3ActionInverse
Eigen::Matrix< S1, 6, 3, O1 > se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
Definition: joint-spherical.hpp:299
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::ConstLinearType
const typedef Vector3 ConstLinearType
Definition: joint-spherical.hpp:52
joint-motion-subspace.hpp
pinocchio::JointModelBase::idx_q
int idx_q() const
Definition: joint-model-base.hpp:160
pinocchio::MotionSphericalTpl::angular
Vector3 & angular()
Definition: joint-spherical.hpp:185
pinocchio::MotionSphericalTpl::plain
PlainReturnType plain() const
Definition: joint-spherical.hpp:90
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::PlainReturnType
MotionPlain PlainReturnType
Definition: joint-spherical.hpp:56
pinocchio::MotionSphericalTpl::se3ActionInverse_impl
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
Definition: joint-spherical.hpp:143
pinocchio::ForceDense
Definition: context/casadi.hpp:34
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:228
pinocchio::traits< JointMotionSubspaceSphericalTpl< _Scalar, _Options > >::ReducedSquaredMatrix
Eigen::Matrix< Scalar, 3, 3, Options > ReducedSquaredMatrix
Definition: joint-spherical.hpp:220
pinocchio::operator^
EIGEN_STRONG_INLINE MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionHelicalUnalignedTpl< S2, O2 > &m2)
Definition: joint-helical-unaligned.hpp:220
pinocchio::traits< JointMotionSubspaceSphericalTpl< _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: joint-spherical.hpp:207
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::Bias_t
MotionZeroTpl< Scalar, Options > Bias_t
Definition: joint-spherical.hpp:397
pinocchio::MotionAlgebraAction< JointMotionSubspaceSphericalTpl< S1, O1 >, MotionDerived >::ReturnType
Eigen::Matrix< S1, 6, 3, O1 > ReturnType
Definition: joint-spherical.hpp:373
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
Definition: joint-motion-subspace-base.hpp:36
Y
Y
pinocchio::JointMotionSubspaceSphericalTpl::isEqual
bool isEqual(const JointMotionSubspaceSphericalTpl &) const
Definition: joint-spherical.hpp:326
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::JointModelDerived
JointModelSphericalTpl< Scalar, Options > JointModelDerived
Definition: joint-spherical.hpp:393
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:71
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::UD_t
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
Definition: joint-spherical.hpp:402
pinocchio::JointModelSphericalTpl::calc
void calc(JointDataDerived &data, const typename Eigen::QuaternionBase< QuaternionDerived > &quat) const
Definition: joint-spherical.hpp:521
pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
pinocchio::JointDataSphericalTpl::joint_q
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR ConfigVector_t joint_q
Definition: joint-spherical.hpp:433
pinocchio::MotionSphericalTpl::se3Action_impl
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
Definition: joint-spherical.hpp:135
PINOCCHIO_EIGEN_REF_TYPE
#define PINOCCHIO_EIGEN_REF_TYPE(D)
Definition: eigen-macros.hpp:32
pinocchio::JointModelSphericalTpl::calc
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
Definition: joint-spherical.hpp:542
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:134
pinocchio::JointMotionSubspaceSphericalTpl
Definition: joint-spherical.hpp:202
pinocchio::JointMotionSubspaceTransposeBase
Definition: joint-motion-subspace-base.hpp:185
pinocchio::MotionTpl< Scalar, Options >
pinocchio::MotionSphericalTpl::angular
const Vector3 & angular() const
Definition: joint-spherical.hpp:181
dcrba.NV
NV
Definition: dcrba.py:536
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::Matrix6
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
Definition: joint-spherical.hpp:46
dpendulum.NQ
int NQ
Definition: dpendulum.py:9
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::U_t
Eigen::Matrix< Scalar, 6, NV, Options > U_t
Definition: joint-spherical.hpp:400
pinocchio::JointDataSphericalTpl::joint_v
TangentVector_t joint_v
Definition: joint-spherical.hpp:434
meshcat-viewer.qs
qs
Definition: meshcat-viewer.py:128
pinocchio::traits< JointMotionSubspaceSphericalTpl< _Scalar, _Options > >::StDiagonalMatrixSOperationReturnType
ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType
Definition: joint-spherical.hpp:225
pinocchio::traits< MotionSphericalTpl< _Scalar, _Options > >::AngularType
Vector3 AngularType
Definition: joint-spherical.hpp:49
pinocchio::ForceDense::angular
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:47
pinocchio::JointSphericalTpl
Definition: joint-spherical.hpp:377
pinocchio::traits< JointModelSphericalTpl< _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: joint-spherical.hpp:421
pinocchio::MotionDense::angular
ConstAngularType angular() const
Definition: motion-base.hpp:32
pinocchio::JointModelSphericalTpl::createData
JointDataDerived createData() const
Definition: joint-spherical.hpp:485
pinocchio::SE3GroupAction< JointMotionSubspaceSphericalTpl< S1, O1 > >::ReturnType
Eigen::Matrix< S1, 6, 3, O1 > ReturnType
Definition: joint-spherical.hpp:367
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::JointDataSphericalTpl::PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
pinocchio::traits< JointSphericalTpl< _Scalar, _Options > >::Transformation_t
SE3Tpl< Scalar, Options > Transformation_t
Definition: joint-spherical.hpp:395
pinocchio::JointModelBase::id
JointIndex id() const
Definition: joint-model-base.hpp:168
pinocchio::JointModelSphericalTpl::cast
JointModelSphericalTpl< NewScalar, Options > cast() const
Definition: joint-spherical.hpp:599


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:11