5 #ifndef __pinocchio_algorithm_rnea_hpp__ 6 #define __pinocchio_algorithm_rnea_hpp__ 8 #include "pinocchio/multibody/model.hpp" 9 #include "pinocchio/multibody/data.hpp" 29 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType1,
typename TangentVectorType2>
31 rnea(
const ModelTpl<Scalar,Options,JointCollectionTpl> &
model,
32 DataTpl<Scalar,Options,JointCollectionTpl> &
data,
33 const Eigen::MatrixBase<ConfigVectorType> &
q,
34 const Eigen::MatrixBase<TangentVectorType1> &
v,
35 const Eigen::MatrixBase<TangentVectorType2> &
a);
55 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType1,
typename TangentVectorType2,
typename ForceDerived>
57 rnea(
const ModelTpl<Scalar,Options,JointCollectionTpl> &
model,
58 DataTpl<Scalar,Options,JointCollectionTpl> &
data,
59 const Eigen::MatrixBase<ConfigVectorType> &
q,
60 const Eigen::MatrixBase<TangentVectorType1> &
v,
61 const Eigen::MatrixBase<TangentVectorType2> &
a,
62 const container::aligned_vector<ForceDerived> & fext);
81 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
84 DataTpl<Scalar,Options,JointCollectionTpl> &
data,
85 const Eigen::MatrixBase<ConfigVectorType> &
q,
86 const Eigen::MatrixBase<TangentVectorType> &
v);
103 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
106 DataTpl<Scalar,Options,JointCollectionTpl> &
data,
107 const Eigen::MatrixBase<ConfigVectorType> &
q);
126 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
129 DataTpl<Scalar,Options,JointCollectionTpl> &
data,
130 const Eigen::MatrixBase<ConfigVectorType> &
q,
131 const container::aligned_vector< ForceTpl<Scalar,Options> > & fext);
150 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
153 DataTpl<Scalar,Options,JointCollectionTpl> &
data,
154 const Eigen::MatrixBase<ConfigVectorType> &
q,
155 const Eigen::MatrixBase<TangentVectorType> &
v);
171 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
174 DataTpl<Scalar,Options,JointCollectionTpl> &
data);
179 #include "pinocchio/algorithm/rnea.hxx" 181 #endif // ifndef __pinocchio_algorithm_rnea_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & getCoriolisMatrix(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Retrives the Coriolis Matrix of the Lagrangian dynamics: after a call to the dynamics derivativ...
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & computeStaticTorque(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const container::aligned_vector< ForceTpl< Scalar, Options > > &fext)
Computes the generalized static torque contribution of the Lagrangian dynamics: ...
void rnea(const int num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & computeGeneralizedGravity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the generalized gravity contribution of the Lagrangian dynamics:
Main pinocchio namespace.
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & computeCoriolisMatrix(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Coriolis Matrix of the Lagrangian dynamics:
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & nonLinearEffects(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects), also called the bias terms of the Lagrangian dynamics:
JointCollectionTpl & model
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc). It also handles the notion of co-tangent vector (e.g. torque, etc).