kinematics-derivatives.hpp

//
// Copyright (c) 2017-2019 CNRS INRIA
//

#ifndef __pinocchio_kinematics_derivatives_hpp__
#define __pinocchio_kinematics_derivatives_hpp__

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"

#include "pinocchio/algorithm/jacobian.hpp"

namespace pinocchio
{
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
  inline void computeForwardKinematicsDerivatives(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);
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2>
  inline void getJointVelocityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                          DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                          const Model::JointIndex jointId,
                                          const ReferenceFrame rf,
                                          const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
                                          const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
  inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                              DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                              const Model::JointIndex jointId,
                                              const ReferenceFrame rf,
                                              const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
                                              const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
                                              const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
                                              const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5>
  inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                              DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                              const Model::JointIndex jointId,
                                              const ReferenceFrame rf,
                                              const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
                                              const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
                                              const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
                                              const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
                                              const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da);

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void
  computeJointKinematicHessians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                DataTpl<Scalar,Options,JointCollectionTpl> & data);

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline void
  computeJointKinematicHessians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                const Eigen::MatrixBase<ConfigVectorType> & q)
  {
    computeJointJacobians(model,data,q);
    computeJointKinematicHessians(model,data);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void
  getJointKinematicHessian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                           const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                           const Model::JointIndex joint_id,
                           const ReferenceFrame rf,
                           Tensor<Scalar,3,Options> & kinematic_hessian);

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline Tensor<Scalar,3,Options>
  getJointKinematicHessian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                           const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                           const Model::JointIndex joint_id,
                           const ReferenceFrame rf)
  {
    typedef Tensor<Scalar,3,Options> ReturnType;
    ReturnType res(6,model.nv,model.nv); res.setZero();
    getJointKinematicHessian(model,data,joint_id,rf,res);
    return res;
  }

} // namespace pinocchio 

#include "pinocchio/algorithm/kinematics-derivatives.hxx"

#endif // ifndef __pinocchio_kinematics_derivatives_hpp__