pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
aba-derivatives.hpp
1 //
2 // Copyright (c) 2018 CNRS, INRIA
3 //
4 
5 #ifndef __pinocchio_aba_derivatives_hpp__
6 #define __pinocchio_aba_derivatives_hpp__
7 
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 
11 namespace pinocchio
12 {
37  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
38  typename MatrixType1, typename MatrixType2, typename MatrixType3>
39  inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
40  DataTpl<Scalar,Options,JointCollectionTpl> & data,
41  const Eigen::MatrixBase<ConfigVectorType> & q,
42  const Eigen::MatrixBase<TangentVectorType1> & v,
43  const Eigen::MatrixBase<TangentVectorType2> & tau,
44  const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
45  const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
46  const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
72  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
73  typename MatrixType1, typename MatrixType2, typename MatrixType3>
74  inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
75  DataTpl<Scalar,Options,JointCollectionTpl> & data,
76  const Eigen::MatrixBase<ConfigVectorType> & q,
77  const Eigen::MatrixBase<TangentVectorType1> & v,
78  const Eigen::MatrixBase<TangentVectorType2> & tau,
79  const container::aligned_vector< ForceTpl<Scalar,Options> > & fext,
80  const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
81  const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
82  const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
83 
104  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
105  inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
106  DataTpl<Scalar,Options,JointCollectionTpl> & data,
107  const Eigen::MatrixBase<ConfigVectorType> & q,
108  const Eigen::MatrixBase<TangentVectorType1> & v,
109  const Eigen::MatrixBase<TangentVectorType2> & tau)
110  {
111  computeABADerivatives(model,data,q,v,tau,
112  data.ddq_dq,data.ddq_dv,data.Minv);
113  }
114 
136  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
137  inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
138  DataTpl<Scalar,Options,JointCollectionTpl> & data,
139  const Eigen::MatrixBase<ConfigVectorType> & q,
140  const Eigen::MatrixBase<TangentVectorType1> & v,
141  const Eigen::MatrixBase<TangentVectorType2> & tau,
142  const container::aligned_vector< ForceTpl<Scalar,Options> > & fext)
143  {
144  computeABADerivatives(model,data,q,v,tau,fext,
145  data.ddq_dq,data.ddq_dv,data.Minv);
146  }
147 
148 } // namespace pinocchio
149 
150 /* --- Details -------------------------------------------------------------------- */
151 #include "pinocchio/algorithm/aba-derivatives.hxx"
152 
153 #endif // ifndef __pinocchio_aba_derivatives_hpp__
pinocchio::DataTpl::ddq_dv
MatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Definition: data.hpp:299
pinocchio::DataTpl::Minv
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:154
pinocchio::DataTpl::ddq_dq
MatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Definition: data.hpp:296
pinocchio::container::aligned_vector
Specialization of an std::vector with an aligned allocator. This specialization might be used when th...
Definition: aligned-vector.hpp:26
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::computeABADerivatives
void computeABADerivatives(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, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
The derivatives of the Articulated-Body algorithm.