5 #ifndef __pinocchio_aba_hpp__ 6 #define __pinocchio_aba_hpp__ 8 #include "pinocchio/multibody/model.hpp" 9 #include "pinocchio/multibody/data.hpp" 10 #include "pinocchio/algorithm/check.hpp" 32 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType1,
typename TangentVectorType2>
34 aba(
const ModelTpl<Scalar,Options,JointCollectionTpl> &
model,
35 DataTpl<Scalar,Options,JointCollectionTpl> &
data,
36 const Eigen::MatrixBase<ConfigVectorType> &
q,
37 const Eigen::MatrixBase<TangentVectorType1> &
v,
38 const Eigen::MatrixBase<TangentVectorType2> &
tau);
60 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType1,
typename TangentVectorType2,
typename ForceDerived>
62 aba(
const ModelTpl<Scalar,Options,JointCollectionTpl> &
model,
63 DataTpl<Scalar,Options,JointCollectionTpl> &
data,
64 const Eigen::MatrixBase<ConfigVectorType> &
q,
65 const Eigen::MatrixBase<TangentVectorType1> &
v,
66 const Eigen::MatrixBase<TangentVectorType2> &
tau,
67 const container::aligned_vector<ForceDerived> & fext);
82 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
85 DataTpl<Scalar,Options,JointCollectionTpl> &
data,
86 const Eigen::MatrixBase<ConfigVectorType> &
q);
94 #include "pinocchio/algorithm/aba.hxx" 96 #endif // ifndef __pinocchio_aba_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor|Options > RowMatrixXs
const DataTpl< Scalar, Options, JointCollectionTpl >::RowMatrixXs & computeMinverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the inverse of the joint space inertia matrix using Articulated Body formulation.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
PINOCCHIO_DEFINE_ALGO_CHECKER(ABA)
Main pinocchio namespace.
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).