pinocchio  2.3.1-dirty
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>
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>
139  const Eigen::MatrixBase<ConfigVectorType> & q,
140  const Eigen::MatrixBase<TangentVectorType1> & v,
141  const Eigen::MatrixBase<TangentVectorType2> & tau,
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__
MatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
MatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Specialization of an std::vector with an aligned allocator. This specialization might be used when th...
Main pinocchio namespace.
Definition: treeview.dox:24
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.