joint-free-flyer.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_free_flyer_hpp__
7 #define __pinocchio_multibody_joint_free_flyer_hpp__
8 
9 #include "pinocchio/macros.hpp"
14 #include "pinocchio/math/fwd.hpp"
16 
17 namespace pinocchio
18 {
19 
20  template<typename Scalar, int Options>
22 
23  template<typename _Scalar, int _Options>
24  struct traits<JointMotionSubspaceIdentityTpl<_Scalar, _Options>>
25  {
26  typedef _Scalar Scalar;
27  enum
28  {
29  Options = _Options
30  };
31  typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
32  enum
33  {
34  LINEAR = 0,
35  ANGULAR = 3
36  };
38  typedef Eigen::Matrix<Scalar, 6, 1, Options> JointForce;
39  typedef Eigen::Matrix<Scalar, 6, 6, Options> DenseBase;
40  typedef Eigen::Matrix<Scalar, 6, 6, Options> ReducedSquaredMatrix;
41 
42  typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType;
43  typedef typename Matrix6::IdentityReturnType MatrixReturnType;
44  typedef typename Matrix6::IdentityReturnType StDiagonalMatrixSOperationReturnType;
45  }; // traits ConstraintRevolute
46 
47  template<typename _Scalar, int _Options>
49  : JointMotionSubspaceBase<JointMotionSubspaceIdentityTpl<_Scalar, _Options>>
50  {
51  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53 
54  enum
55  {
56  NV = 6
57  };
58 
59  template<typename Vector6Like>
60  JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const
61  {
62  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6);
63  return JointMotion(vj);
64  }
65 
66  template<typename S1, int O1>
68  {
69  return m.toActionMatrix();
70  }
71 
72  template<typename S1, int O1>
74  {
75  return m.toActionMatrixInverse();
76  }
77 
78  int nv_impl() const
79  {
80  return NV;
81  }
82 
83  struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceIdentityTpl>
84  {
85  template<typename Derived>
88  {
89  return phi.toVector();
90  }
91 
92  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
93  template<typename MatrixDerived>
94  typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
95  operator*(const Eigen::MatrixBase<MatrixDerived> & F)
96  {
97  return F.derived();
98  }
99  };
100 
102  {
103  return TransposeConst();
104  }
105  MatrixReturnType matrix_impl() const
106  {
107  return DenseBase::Identity();
108  }
109 
110  template<typename MotionDerived>
111  typename MotionDerived::ActionMatrixType motionAction(const MotionBase<MotionDerived> & v) const
112  {
113  return v.toActionMatrix();
114  }
115 
117  {
118  return true;
119  }
120 
121  }; // struct JointMotionSubspaceIdentityTpl
122 
123  template<typename Scalar, int Options, typename Vector6Like>
126  const Eigen::MatrixBase<Vector6Like> & v)
127  {
128  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6);
129  // typedef typename JointMotionSubspaceIdentityTpl<Scalar,Options>::Motion Motion;
131  return Motion(v.derived());
132  }
133 
134  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
135  template<typename S1, int O1, typename S2, int O2>
136  inline typename InertiaTpl<S1, O1>::Matrix6
138  {
139  return Y.matrix();
140  }
141 
142  /* [ABA] Y*S operator*/
143  template<typename Matrix6Like, typename S2, int O2>
144  inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(
145  const Eigen::MatrixBase<Matrix6Like> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &)
146  {
147  return Y.derived();
148  }
149 
150  template<typename S1, int O1>
152  {
154  };
155 
156  template<typename S1, int O1, typename MotionDerived>
158  {
160  };
161 
162  template<typename Scalar, int Options>
164 
165  template<typename _Scalar, int _Options>
166  struct traits<JointFreeFlyerTpl<_Scalar, _Options>>
167  {
168  enum
169  {
170  NQ = 7,
171  NV = 6
172  };
173  typedef _Scalar Scalar;
174  enum
175  {
176  Options = _Options
177  };
184 
185  // [ABA]
186  typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
187  typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
188  typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
189 
190  typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
191  typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
192 
194  };
195 
196  template<typename _Scalar, int _Options>
197  struct traits<JointDataFreeFlyerTpl<_Scalar, _Options>>
198  {
200  typedef _Scalar Scalar;
201  };
202 
203  template<typename _Scalar, int _Options>
204  struct traits<JointModelFreeFlyerTpl<_Scalar, _Options>>
205  {
207  typedef _Scalar Scalar;
208  };
209 
210  template<typename _Scalar, int _Options>
211  struct JointDataFreeFlyerTpl : public JointDataBase<JointDataFreeFlyerTpl<_Scalar, _Options>>
212  {
213  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
217 
218  ConfigVector_t joint_q;
219  TangentVector_t joint_v;
220 
221  Constraint_t S;
222  Transformation_t M;
223  Motion_t v;
224  Bias_t c;
225 
226  // [ABA] specific data
227  U_t U;
228  D_t Dinv;
229  UD_t UDinv;
230  D_t StU;
231 
233  : joint_q(ConfigVector_t::Zero())
234  , joint_v(TangentVector_t::Zero())
235  , M(Transformation_t::Identity())
236  , v(Motion_t::Zero())
237  , U(U_t::Zero())
238  , Dinv(D_t::Zero())
239  , UDinv(UD_t::Identity())
240  , StU(D_t::Zero())
241  {
242  joint_q[6] = Scalar(1);
243  }
244 
245  static std::string classname()
246  {
247  return std::string("JointDataFreeFlyer");
248  }
249  std::string shortname() const
250  {
251  return classname();
252  }
253 
254  }; // struct JointDataFreeFlyerTpl
255 
280  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl);
281  template<typename _Scalar, int _Options>
282  struct JointModelFreeFlyerTpl : public JointModelBase<JointModelFreeFlyerTpl<_Scalar, _Options>>
283  {
284  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
287 
289  using Base::id;
290  using Base::idx_q;
291  using Base::idx_v;
292  using Base::setIndexes;
293 
294  JointDataDerived createData() const
295  {
296  return JointDataDerived();
297  }
298 
299  const std::vector<bool> hasConfigurationLimit() const
300  {
301  return {true, true, true, false, false, false, false};
302  }
303 
304  const std::vector<bool> hasConfigurationLimitInTangent() const
305  {
306  return {true, true, true, false, false, false};
307  }
308 
309  template<typename ConfigVectorLike>
310  inline void forwardKinematics(
311  Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
312  {
313  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t, ConfigVectorLike);
314  typedef typename Eigen::Quaternion<
315  typename ConfigVectorLike::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options>
316  Quaternion;
317  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
318 
319  ConstQuaternionMap quat(q_joint.template tail<4>().data());
320  // assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename
321  // V::Scalar>::epsilon())); TODO: check validity of the rhs precision
322  assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm() - 1)) <= 1e-4);
323 
324  M.rotation(quat.matrix());
325  M.translation(q_joint.template head<3>());
326  }
327 
328  template<typename Vector3Derived, typename QuaternionDerived>
329  EIGEN_DONT_INLINE void calc(
330  JointDataDerived & data,
331  const typename Eigen::MatrixBase<Vector3Derived> & trans,
332  const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
333  {
334  data.M.translation(trans);
335  data.M.rotation(quat.matrix());
336  }
337 
338  template<typename ConfigVector>
339  EIGEN_DONT_INLINE void
340  calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
341  {
342  typedef typename Eigen::Quaternion<Scalar, Options> Quaternion;
343  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
344 
345  data.joint_q = qs.template segment<NQ>(idx_q());
346  ConstQuaternionMap quat(data.joint_q.template tail<4>().data());
347 
348  calc(data, data.joint_q.template head<3>(), quat);
349  }
350 
351  template<typename TangentVector>
352  EIGEN_DONT_INLINE void
353  calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
354  const
355  {
356  data.joint_v = vs.template segment<NV>(idx_v());
357  data.v = data.joint_v;
358  }
359 
360  template<typename ConfigVector, typename TangentVector>
361  EIGEN_DONT_INLINE void calc(
362  JointDataDerived & data,
363  const typename Eigen::MatrixBase<ConfigVector> & qs,
364  const typename Eigen::MatrixBase<TangentVector> & vs) const
365  {
366  calc(data, qs.derived());
367 
368  data.joint_v = vs.template segment<NV>(idx_v());
369  data.v = data.joint_v;
370  }
371 
372  template<typename VectorLike, typename Matrix6Like>
373  void calc_aba(
374  JointDataDerived & data,
375  const Eigen::MatrixBase<VectorLike> & armature,
376  const Eigen::MatrixBase<Matrix6Like> & I,
377  const bool update_I) const
378  {
379  data.U = I;
380  data.StU = I;
381  data.StU.diagonal() += armature;
382 
384  data.UDinv.noalias() = I * data.Dinv;
385 
386  if (update_I)
387  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
388  }
389 
390  static std::string classname()
391  {
392  return std::string("JointModelFreeFlyer");
393  }
394  std::string shortname() const
395  {
396  return classname();
397  }
398 
400  template<typename NewScalar>
402  {
404  ReturnType res;
405  res.setIndexes(id(), idx_q(), idx_v());
406  return res;
407  }
408 
409  }; // struct JointModelFreeFlyerTpl
410 
411 } // namespace pinocchio
412 
413 #include <boost/type_traits.hpp>
414 
415 namespace boost
416 {
417  template<typename Scalar, int Options>
418  struct has_nothrow_constructor<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>>
419  : public integral_constant<bool, true>
420  {
421  };
422 
423  template<typename Scalar, int Options>
424  struct has_nothrow_copy<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>>
425  : public integral_constant<bool, true>
426  {
427  };
428 
429  template<typename Scalar, int Options>
430  struct has_nothrow_constructor<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>>
431  : public integral_constant<bool, true>
432  {
433  };
434 
435  template<typename Scalar, int Options>
436  struct has_nothrow_copy<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>>
437  : public integral_constant<bool, true>
438  {
439  };
440 } // namespace boost
441 
442 #endif // ifndef __pinocchio_multibody_joint_free_flyer_hpp__
pinocchio::traits< JointDataFreeFlyerTpl< _Scalar, _Options > >::JointDerived
JointFreeFlyerTpl< _Scalar, _Options > JointDerived
Definition: joint-free-flyer.hpp:199
pinocchio::InertiaTpl
Definition: spatial/fwd.hpp:58
pinocchio::JointModelFreeFlyerTpl::shortname
std::string shortname() const
Definition: joint-free-flyer.hpp:394
pinocchio::JointMotionSubspaceIdentityTpl::NV
@ NV
Definition: joint-free-flyer.hpp:56
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
Definition: joint-data-base.hpp:137
pinocchio::JointMotionSubspaceIdentityTpl::TransposeConst
Definition: joint-free-flyer.hpp:83
quaternion.hpp
pinocchio::JointModelFreeFlyerTpl::Base
JointModelBase< JointModelFreeFlyerTpl > Base
Definition: joint-free-flyer.hpp:288
test-cpp2pybind11.m
m
Definition: test-cpp2pybind11.py:25
pinocchio::JointDataFreeFlyerTpl
Definition: multibody/joint/fwd.hpp:114
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::Bias_t
MotionZeroTpl< Scalar, Options > Bias_t
Definition: joint-free-flyer.hpp:183
macros.hpp
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:75
quat
quat
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::JointMotionSubspaceIdentityTpl::motionAction
MotionDerived::ActionMatrixType motionAction(const MotionBase< MotionDerived > &v) const
Definition: joint-free-flyer.hpp:111
pinocchio::JointMotionSubspaceBase
Definition: joint-motion-subspace-base.hpp:59
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::JointModelFreeFlyerTpl::forwardKinematics
void forwardKinematics(Transformation_t &M, const Eigen::MatrixBase< ConfigVectorLike > &q_joint) const
Definition: joint-free-flyer.hpp:310
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::SE3GroupAction< JointMotionSubspaceIdentityTpl< S1, O1 > >::ReturnType
SE3Tpl< S1, O1 >::ActionMatrixType ReturnType
Definition: joint-free-flyer.hpp:153
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
pinocchio::traits< JointMotionSubspaceIdentityTpl< _Scalar, _Options > >::MatrixReturnType
Matrix6::IdentityReturnType MatrixReturnType
Definition: joint-free-flyer.hpp:43
inertia.hpp
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::UD_t
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
Definition: joint-free-flyer.hpp:188
pinocchio::traits< JointMotionSubspaceIdentityTpl< _Scalar, _Options > >::StDiagonalMatrixSOperationReturnType
Matrix6::IdentityReturnType StDiagonalMatrixSOperationReturnType
Definition: joint-free-flyer.hpp:44
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::D_t
Eigen::Matrix< Scalar, NV, NV, Options > D_t
Definition: joint-free-flyer.hpp:187
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::traits< JointMotionSubspaceIdentityTpl< _Scalar, _Options > >::JointForce
Eigen::Matrix< Scalar, 6, 1, Options > JointForce
Definition: joint-free-flyer.hpp:38
boost
explog.hpp
pinocchio::JointModelBase::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
pinocchio::JointModelFreeFlyerTpl
Definition: multibody/joint/fwd.hpp:110
pinocchio::JointMotionSubspaceIdentityTpl::matrix_impl
MatrixReturnType matrix_impl() const
Definition: joint-free-flyer.hpp:105
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
pinocchio::traits< JointModelFreeFlyerTpl< _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: joint-free-flyer.hpp:207
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::JointModelDerived
JointModelFreeFlyerTpl< Scalar, Options > JointModelDerived
Definition: joint-free-flyer.hpp:179
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: spatial/motion.hpp:45
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::Transformation_t
SE3Tpl< Scalar, Options > Transformation_t
Definition: joint-free-flyer.hpp:181
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::JointModelFreeFlyerTpl::calc
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
Definition: joint-free-flyer.hpp:340
pinocchio::JointMotionSubspaceIdentityTpl::nv_impl
int nv_impl() const
Definition: joint-free-flyer.hpp:78
pinocchio::Blank
Blank type.
Definition: fwd.hpp:76
pinocchio::traits< JointDataFreeFlyerTpl< _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: joint-free-flyer.hpp:200
pinocchio::MotionBase
Definition: spatial/fwd.hpp:42
pinocchio::traits< JointMotionSubspaceIdentityTpl< _Scalar, _Options > >::DenseBase
Eigen::Matrix< Scalar, 6, 6, Options > DenseBase
Definition: joint-free-flyer.hpp:39
pinocchio::SE3GroupAction
Definition: spatial/se3.hpp:39
pinocchio::MotionZeroTpl
Definition: context/casadi.hpp:23
pinocchio::JointDataFreeFlyerTpl::v
Motion_t v
Definition: joint-free-flyer.hpp:223
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
Definition: joint-data-base.hpp:55
pinocchio::JointDataFreeFlyerTpl::classname
static std::string classname()
Definition: joint-free-flyer.hpp:245
pinocchio::python::context::Quaternion
Eigen::Quaternion< Scalar, Options > Quaternion
Definition: bindings/python/context/generic.hpp:49
pinocchio::traits< JointMotionSubspaceIdentityTpl< _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: joint-free-flyer.hpp:26
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::U_t
Eigen::Matrix< Scalar, 6, NV, Options > U_t
Definition: joint-free-flyer.hpp:186
pinocchio::JointModelFreeFlyerTpl::cast
JointModelFreeFlyerTpl< NewScalar, Options > cast() const
Definition: joint-free-flyer.hpp:401
pinocchio::traits< JointModelFreeFlyerTpl< _Scalar, _Options > >::JointDerived
JointFreeFlyerTpl< _Scalar, _Options > JointDerived
Definition: joint-free-flyer.hpp:206
pinocchio::PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
Free-flyer joint in .
pinocchio::JointModelFreeFlyerTpl::calc
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const Blank, const typename Eigen::MatrixBase< TangentVector > &vs) const
Definition: joint-free-flyer.hpp:353
pinocchio::JointModelFreeFlyerTpl::JointDerived
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointFreeFlyerTpl< _Scalar, _Options > JointDerived
Definition: joint-free-flyer.hpp:285
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::Motion_t
MotionTpl< Scalar, Options > Motion_t
Definition: joint-free-flyer.hpp:182
pinocchio::JointMotionSubspaceIdentityTpl::transpose
TransposeConst transpose() const
Definition: joint-free-flyer.hpp:101
pinocchio::JointDataFreeFlyerTpl::PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::ConfigVector_t
Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
Definition: joint-free-flyer.hpp:190
fwd.hpp
pinocchio::JointDataFreeFlyerTpl::Dinv
D_t Dinv
Definition: joint-free-flyer.hpp:228
pinocchio::JointModelFreeFlyerTpl::hasConfigurationLimitInTangent
const std::vector< bool > hasConfigurationLimitInTangent() const
Definition: joint-free-flyer.hpp:304
pinocchio::JointDataFreeFlyerTpl::joint_q
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR ConfigVector_t joint_q
Definition: joint-free-flyer.hpp:218
joint-base.hpp
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::JointDataDerived
JointDataFreeFlyerTpl< Scalar, Options > JointDataDerived
Definition: joint-free-flyer.hpp:178
pinocchio::JointDataFreeFlyerTpl::S
Constraint_t S
Definition: joint-free-flyer.hpp:221
M
M
pinocchio::JointDataFreeFlyerTpl::c
Bias_t c
Definition: joint-free-flyer.hpp:224
pinocchio::JointModelFreeFlyerTpl::calc_aba
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< VectorLike > &armature, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
Definition: joint-free-flyer.hpp:373
pinocchio::JointFreeFlyerTpl
Definition: joint-free-flyer.hpp:163
pinocchio::Motion
MotionTpl<::CppAD::AD< double >, 0 > Motion
Definition: context/cppad.hpp:37
pinocchio::traits< JointMotionSubspaceIdentityTpl< _Scalar, _Options > >::ReducedSquaredMatrix
Eigen::Matrix< Scalar, 6, 6, Options > ReducedSquaredMatrix
Definition: joint-free-flyer.hpp:40
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::TangentVector_t
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
Definition: joint-free-flyer.hpp:191
pinocchio::JointMotionSubspaceIdentityTpl::__mult__
JointMotion __mult__(const Eigen::MatrixBase< Vector6Like > &vj) const
Definition: joint-free-flyer.hpp:60
pinocchio::JointDataFreeFlyerTpl::UDinv
UD_t UDinv
Definition: joint-free-flyer.hpp:229
pinocchio::JointDataFreeFlyerTpl::StU
D_t StU
Definition: joint-free-flyer.hpp:230
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::JointDataFreeFlyerTpl::M
Transformation_t M
Definition: joint-free-flyer.hpp:222
pinocchio::JointModelFreeFlyerTpl::calc
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
Definition: joint-free-flyer.hpp:361
pinocchio::JointMotionSubspaceIdentityTpl::se3ActionInverse
SE3Tpl< S1, O1 >::ActionMatrixType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
Definition: joint-free-flyer.hpp:73
joint-motion-subspace.hpp
pinocchio::JointDataFreeFlyerTpl::shortname
std::string shortname() const
Definition: joint-free-flyer.hpp:249
pinocchio::JointModelBase::idx_q
int idx_q() const
Definition: joint-model-base.hpp:160
pinocchio::JointDataFreeFlyerTpl::JointDerived
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointFreeFlyerTpl< _Scalar, _Options > JointDerived
Definition: joint-free-flyer.hpp:214
pinocchio::JointDataFreeFlyerTpl::joint_v
TangentVector_t joint_v
Definition: joint-free-flyer.hpp:219
pinocchio::ForceDense
Definition: context/casadi.hpp:34
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::Constraint_t
JointMotionSubspaceIdentityTpl< Scalar, Options > Constraint_t
Definition: joint-free-flyer.hpp:180
pinocchio::JointModelFreeFlyerTpl::classname
static std::string classname()
Definition: joint-free-flyer.hpp:390
pinocchio::JointModelFreeFlyerTpl::PINOCCHIO_JOINT_TYPEDEF_TEMPLATE
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
Definition: joint-motion-subspace-base.hpp:36
Y
Y
pinocchio::traits< JointFreeFlyerTpl< _Scalar, _Options > >::Scalar
_Scalar Scalar
Definition: joint-free-flyer.hpp:173
pinocchio::JointModelFreeFlyerTpl::hasConfigurationLimit
const std::vector< bool > hasConfigurationLimit() const
Definition: joint-free-flyer.hpp:299
pinocchio::JointMotionSubspaceIdentityTpl::TransposeConst::PINOCCHIO_EIGEN_REF_CONST_TYPE
PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived) operator*(const Eigen
Definition: joint-free-flyer.hpp:94
pinocchio::JointMotionSubspaceIdentityTpl::isEqual
bool isEqual(const JointMotionSubspaceIdentityTpl &) const
Definition: joint-free-flyer.hpp:116
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:71
pinocchio::traits< JointMotionSubspaceIdentityTpl< _Scalar, _Options > >::Matrix6
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
Definition: joint-free-flyer.hpp:31
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::JointMotionSubspaceTransposeBase
Definition: joint-motion-subspace-base.hpp:185
pinocchio::MotionTpl< Scalar, Options >
dcrba.NV
NV
Definition: dcrba.py:536
pinocchio::JointMotionSubspaceIdentityTpl::TransposeConst::operator*
ForceDense< Derived >::ToVectorConstReturnType operator*(const ForceDense< Derived > &phi)
Definition: joint-free-flyer.hpp:87
dpendulum.NQ
int NQ
Definition: dpendulum.py:9
meshcat-viewer.qs
qs
Definition: meshcat-viewer.py:128
pinocchio::MotionRef
Definition: context/casadi.hpp:38
pinocchio::traits< JointMotionSubspaceIdentityTpl< _Scalar, _Options > >::JointMotion
MotionTpl< Scalar, Options > JointMotion
Definition: joint-free-flyer.hpp:37
pinocchio::MotionAlgebraAction< JointMotionSubspaceIdentityTpl< S1, O1 >, MotionDerived >::ReturnType
SE3Tpl< S1, O1 >::ActionMatrixType ReturnType
Definition: joint-free-flyer.hpp:159
pinocchio::JointMotionSubspaceIdentityTpl
Definition: joint-free-flyer.hpp:21
pinocchio::JointMotionSubspaceIdentityTpl::se3Action
SE3Tpl< S1, O1 >::ActionMatrixType se3Action(const SE3Tpl< S1, O1 > &m) const
Definition: joint-free-flyer.hpp:67
pinocchio::JointDataFreeFlyerTpl::JointDataFreeFlyerTpl
JointDataFreeFlyerTpl()
Definition: joint-free-flyer.hpp:232
pinocchio::traits< JointMotionSubspaceIdentityTpl< _Scalar, _Options > >::ConstMatrixReturnType
Matrix6::IdentityReturnType ConstMatrixReturnType
Definition: joint-free-flyer.hpp:42
pinocchio::JointModelFreeFlyerTpl::createData
JointDataDerived createData() const
Definition: joint-free-flyer.hpp:294
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::JointDataFreeFlyerTpl::U
U_t U
Definition: joint-free-flyer.hpp:227
pinocchio::JointModelFreeFlyerTpl::calc
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< Vector3Derived > &trans, const typename Eigen::QuaternionBase< QuaternionDerived > &quat) const
Definition: joint-free-flyer.hpp:329
pinocchio::JointModelBase::id
JointIndex id() const
Definition: joint-model-base.hpp:168


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