5 #ifndef __pinocchio_algorithm_centroidal_hpp__ 6 #define __pinocchio_algorithm_centroidal_hpp__ 8 #include "pinocchio/multibody/model.hpp" 9 #include "pinocchio/multibody/data.hpp" 10 #include "pinocchio/algorithm/kinematics.hpp" 28 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
31 DataTpl<Scalar,Options,JointCollectionTpl> &
data);
50 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
51 typename ConfigVectorType,
typename TangentVectorType>
55 const Eigen::MatrixBase<ConfigVectorType> &
q,
56 const Eigen::MatrixBase<TangentVectorType> &
v)
66 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
67 typename ConfigVectorType,
typename TangentVectorType>
72 const Eigen::MatrixBase<ConfigVectorType> &
q,
73 const Eigen::MatrixBase<TangentVectorType> &
v)
92 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
117 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
118 typename ConfigVectorType,
typename TangentVectorType1,
typename TangentVectorType2>
122 const Eigen::MatrixBase<ConfigVectorType> &
q,
123 const Eigen::MatrixBase<TangentVectorType1> &
v,
124 const Eigen::MatrixBase<TangentVectorType2> &
a)
134 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
135 typename ConfigVectorType,
typename TangentVectorType1,
typename TangentVectorType2>
140 const Eigen::MatrixBase<ConfigVectorType> &
q,
141 const Eigen::MatrixBase<TangentVectorType1> &
v,
142 const Eigen::MatrixBase<TangentVectorType2> &
a)
164 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
168 const Eigen::MatrixBase<ConfigVectorType> &
q,
169 const Eigen::MatrixBase<TangentVectorType> &
v);
185 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
189 const Eigen::MatrixBase<ConfigVectorType> & q);
209 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
213 const Eigen::MatrixBase<ConfigVectorType> & q,
214 const Eigen::MatrixBase<TangentVectorType> & v);
232 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
236 const Eigen::MatrixBase<ConfigVectorType> & q,
237 const Eigen::MatrixBase<TangentVectorType> & v);
242 #include "pinocchio/algorithm/centroidal.hxx" 244 #endif // ifndef __pinocchio_algorithm_centroidal_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeCentroidalMapTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Centroidal Momentum Matrix time derivative.
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeCentroidalMap(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the Centroidal Momentum Matrix,.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
const DataTpl< Scalar, Options, JointCollectionTpl >::Force & computeCentroidalMomentumTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes the Centroidal momemtum and its time derivatives, a.k.a. the total momenta of the system and...
PINOCCHIO_DEPRECATED const DataTpl< Scalar, Options, JointCollectionTpl >::Force & computeCentroidalDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Centroidal momentum, a.k.a. the total momenta of the system expressed around the center ...
ForceTpl< Scalar, Options > Force
const DataTpl< Scalar, Options, JointCollectionTpl >::Force & computeCentroidalMomentum(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes the Centroidal momentum, a.k.a. the total momenta of the system expressed around the center ...
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & dccrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration...
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & ccrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal m...
Main pinocchio namespace.
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
JointCollectionTpl & model