regressor.hpp

Go to the documentation of this file.

//
// Copyright (c) 2018-2020 CNRS INRIA
//

#ifndef __pinocchio_algorithm_regressor_hpp__
#define __pinocchio_algorithm_regressor_hpp__

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

namespace pinocchio
{
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
  void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const JointIndex joint_id,
                                      const ReferenceFrame rf,
                                      const SE3Tpl<Scalar,Options> & placement,
                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);


  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x
  computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                 const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                 const JointIndex joint_id,
                                 const ReferenceFrame rf,
                                 const SE3Tpl<Scalar,Options> & placement)
  {
    typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType;
    ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6));
    
    computeJointKinematicRegressor(model,data,joint_id,rf,placement,res);
    
    return res;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
  void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const JointIndex joint_id,
                                      const ReferenceFrame rf,
                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x
  computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                 const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                 const JointIndex joint_id,
                                 const ReferenceFrame rf)
  {
    typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType;
    ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6));
    
    computeJointKinematicRegressor(model,data,joint_id,rf,res);
    
    return res;
  }
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
  void computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const FrameIndex frame_id,
                                      const ReferenceFrame rf,
                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x
  computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                 DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                 const FrameIndex frame_id,
                                 const ReferenceFrame rf)
  {
    typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType;
    ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6));
    
    computeFrameKinematicRegressor(model,data,frame_id,rf,res);
    
    return res;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
  computeStaticRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                         DataTpl<Scalar,Options,JointCollectionTpl> & data,
                         const Eigen::MatrixBase<ConfigVectorType> & q);

  namespace regressor
  {
    
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
    inline PINOCCHIO_DEPRECATED typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
    computeStaticRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                           DataTpl<Scalar,Options,JointCollectionTpl> & data,
                           const Eigen::MatrixBase<ConfigVectorType> & q)
    {
        return ::pinocchio::computeStaticRegressor(model,data,q);
    }
  }

  template<typename MotionVelocity, typename MotionAcceleration, typename OutputType>
  inline void
  bodyRegressor(const MotionDense<MotionVelocity> & v,
                const MotionDense<MotionAcceleration> & a,
                const Eigen::MatrixBase<OutputType> & regressor);

  template<typename MotionVelocity, typename MotionAcceleration>
  inline Eigen::Matrix<typename MotionVelocity::Scalar,6,10,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options>
  bodyRegressor(const MotionDense<MotionVelocity> & v,
                const MotionDense<MotionAcceleration> & a);

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline typename DataTpl<Scalar,Options,JointCollectionTpl>::BodyRegressorType &
  jointBodyRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                     DataTpl<Scalar,Options,JointCollectionTpl> & data,
                     JointIndex jointId);

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline typename DataTpl<Scalar,Options,JointCollectionTpl>::BodyRegressorType &
  frameBodyRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                     DataTpl<Scalar,Options,JointCollectionTpl> & data,
                     FrameIndex frameId);

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
  inline typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
  computeJointTorqueRegressor(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);

} // namespace pinocchio

/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/regressor.hxx"

#endif // ifndef __pinocchio_algorithm_regressor_hpp__