aba-derivatives.hpp

//
// Copyright (c) 2018 CNRS, INRIA
//
 
#ifndef __pinocchio_aba_derivatives_hpp__
#define __pinocchio_aba_derivatives_hpp__
 
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
 
namespace pinocchio
{
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
           typename MatrixType1, typename MatrixType2, typename MatrixType3>
  inline 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);
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
  typename MatrixType1, typename MatrixType2, typename MatrixType3>
  inline 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 container::aligned_vector< ForceTpl<Scalar,Options> > & fext,
                                    const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
                                    const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
                                    const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
  inline 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)
  {
    computeABADerivatives(model,data,q,v,tau,
                          data.ddq_dq,data.ddq_dv,data.Minv);
  }
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
  inline 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 container::aligned_vector< ForceTpl<Scalar,Options> > & fext)
  {
    computeABADerivatives(model,data,q,v,tau,fext,
                          data.ddq_dq,data.ddq_dv,data.Minv);
  }
 
} // namespace pinocchio
 
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/aba-derivatives.hxx"
 
#endif // ifndef __pinocchio_aba_derivatives_hpp__