rnea-second-order-derivatives.hpp

//
// Copyright (c) 2017-2019 CNRS INRIA
 
#ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hpp__
#define __pinocchio_algorithm_rnea_second_order_derivatives_hpp__
 
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/model.hpp"
 
namespace pinocchio {
template <typename Scalar, int Options,
          template <typename, int> class JointCollectionTpl,
          typename ConfigVectorType, typename TangentVectorType1,
          typename TangentVectorType2, typename Tensor1,
          typename Tensor2, typename Tensor3, typename Tensor4>
inline void ComputeRNEASecondOrderDerivatives(
    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> &a,
    const Tensor1 &d2tau_dqdq, const Tensor2 &d2tau_dvdv,
    const Tensor3 &dtau_dqdv, const Tensor4 &dtau_dadq);
 
 
template <typename Scalar, int Options,
          template <typename, int> class JointCollectionTpl,
          typename ConfigVectorType, typename TangentVectorType1,
          typename TangentVectorType2>
inline void ComputeRNEASecondOrderDerivatives(
    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> &a) {
  (data.d2tau_dqdq).setZero();
  (data.d2tau_dvdv).setZero();
  (data.d2tau_dqdv).setZero();
  (data.d2tau_dadq).setZero();
 
  ComputeRNEASecondOrderDerivatives(model, data, q.derived(), v.derived(), a.derived(),
                           data.d2tau_dqdq, data.d2tau_dvdv, data.d2tau_dqdv,
                           data.d2tau_dadq);
}
 
} // namespace pinocchio
 
#include "pinocchio/algorithm/rnea-second-order-derivatives.hxx"
 
#endif // ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hpp__