jacobian.hpp

//
// Copyright (c) 2015-2018 CNRS
//

#ifndef __pinocchio_jacobian_hpp__
#define __pinocchio_jacobian_hpp__

#include "pinocchio/multibody/fwd.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>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
  computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<ConfigVectorType> & q);
  
  PINOCCHIO_DEPRECATED
  inline const Data::Matrix6x &
  computeJacobians(const Model & model,
                   Data & data,
                   const Eigen::VectorXd & q)
  { return computeJointJacobians(model,data,q.derived()); }
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
  computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        DataTpl<Scalar,Options,JointCollectionTpl> & data);
  
  PINOCCHIO_DEPRECATED
  inline const Data::Matrix6x &
  computeJacobians(const Model & model,
                   Data & data)
  { return computeJointJacobians(model,data); }
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6Like>
  inline void getJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                               const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex jointId,
                               const ReferenceFrame rf,
                               const Eigen::MatrixBase<Matrix6Like> & J);
  
  template<ReferenceFrame rf>
  PINOCCHIO_DEPRECATED
  void getJointJacobian(const Model & model,
                        const Data & data,
                        const Model::JointIndex jointId,
                        Data::Matrix6x & J)
  { getJointJacobian(model,data,jointId,rf,J); }
  
  template<ReferenceFrame rf>
  PINOCCHIO_DEPRECATED
  void getJacobian(const Model & model,
                   const Data & data,
                   const Model::JointIndex jointId,
                   Data::Matrix6x & J)
  { getJointJacobian<rf>(model,data,jointId,J); }
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6Like>
  inline void jointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                            DataTpl<Scalar,Options,JointCollectionTpl> & data,
                            const Eigen::MatrixBase<ConfigVectorType> & q,
                            const JointIndex jointId,
                            const Eigen::MatrixBase<Matrix6Like> & J);
  
  PINOCCHIO_DEPRECATED
  inline void jacobian(const Model & model,
                       Data & data,
                       const Eigen::VectorXd & q,
                       const Model::JointIndex jointId,
                       Data::Matrix6x & J)
  { jointJacobian(model,data,q.derived(),jointId,J); }
  
  PINOCCHIO_DEPRECATED
  inline const Data::Matrix6x &
  jointJacobian(const Model & model,
                Data & data,
                const Eigen::VectorXd & q,
                const Model::JointIndex jointId)
  {
    data.J.setZero();
    jointJacobian(model,data,q,jointId,data.J);

    return data.J;
  }
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
  computeJointJacobiansTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                     DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                     const Eigen::MatrixBase<ConfigVectorType> & q,
                                     const Eigen::MatrixBase<TangentVectorType> & v);
  
  PINOCCHIO_DEPRECATED
  inline const Data::Matrix6x &
  computeJacobiansTimeVariation(const Model & model,
                                Data & data,
                                const Eigen::VectorXd & q,
                                const Eigen::VectorXd & v)
  { return computeJointJacobiansTimeVariation(model,data,q,v); }
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6Like>
  inline void getJointJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                            const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                            const JointIndex jointId,
                                            const ReferenceFrame rf,
                                            const Eigen::MatrixBase<Matrix6Like> & dJ);
  
  template<ReferenceFrame rf>
  PINOCCHIO_DEPRECATED
  inline void getJointJacobianTimeVariation(const Model & model,
                                            const Data & data,
                                            const Model::JointIndex jointId,
                                            Data::Matrix6x & dJ)
  { getJointJacobianTimeVariation(model,data,jointId,rf,dJ); }
  
  template<ReferenceFrame rf>
  PINOCCHIO_DEPRECATED
  inline void getJacobianTimeVariation(const Model & model,
                                       const Data & data,
                                       const Model::JointIndex jointId,
                                       Data::Matrix6x & dJ)
  { getJointJacobianTimeVariation<rf>(model,data,jointId,dJ); }
  
} // namespace pinocchio 

/* --- Details -------------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------------- */

#include "pinocchio/algorithm/jacobian.hxx"

#endif // ifndef __pinocchio_jacobian_hpp__