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);
 
  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 joint_id);
 
  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 frame_id);
 
  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);
 
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType>
  const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs &
  computeKineticEnergyRegressor(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentVectorType> & v);
 
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType>
  const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs &
  computePotentialEnergyRegressor(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Eigen::MatrixBase<ConfigVectorType> & q);
} // namespace pinocchio
 
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/regressor.hxx"
 
#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
  #include "pinocchio/algorithm/regressor.txx"
#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
 
#endif // ifndef __pinocchio_algorithm_regressor_hpp__