rnea-second-order-derivatives.hpp

Go to the documentation of this file.

//
// 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__