polynomial.h

Go to the documentation of this file.

#ifndef INCLUDE_STOMP_MOVEIT_UTILS_POLYNOMIAL_H_
#define INCLUDE_STOMP_MOVEIT_UTILS_POLYNOMIAL_H_

#include <stomp_core/utils.h>
#include <moveit/robot_model/robot_model.h>
#include <Eigen/Core>

namespace stomp_moveit
{

namespace utils
{

namespace polynomial
{
  struct PolyFitRequest
  {
    int d;                                          
    Eigen::Matrix<double, 2, Eigen::Dynamic>  xy;   
    Eigen::Matrix<double, 2, Eigen::Dynamic>  xyfp; 
    Eigen::Matrix<double, 2, Eigen::Dynamic>  xyfs; 
    Eigen::VectorXd output_domain;                  
    bool hasConstraints() const
    {
      return ((xyfp.cols() > 0) || (xyfs.cols() > 0));
    }

    bool hasPositionConstraints() const
    {
      return (xyfp.cols() > 0);
    }

    bool hasSlopeConstraints() const
    {
      return (xyfs.cols() > 0);
    }

  };

  struct PolyFitResults
  {
    Eigen::VectorXd p;   
    Eigen::VectorXd x;   
    Eigen::VectorXd y;   
    bool successful;     
  };


  PolyFitResults polyFit(const PolyFitRequest &request);

  void generateMinimumDomainValues(const std::vector<const  moveit::core::JointModel*> joint_models, const Eigen::MatrixXd &parameters,
                                   Eigen::VectorXd &domain_values);

  void fillVandermondeMatrix(const Eigen::ArrayXd &domain_vals, const int &order, Eigen::MatrixXd &v);

  bool applyPolynomialSmoothing(moveit::core::RobotModelConstPtr robot_model, const std::string& group_name, Eigen::MatrixXd& parameters,
                                       int poly_order = 5, double joint_limit_margin = 1e-5);

} // end of namespace smoothing
} // end of namespace utils
} // end of namespace stomp_moveit


#endif /* INCLUDE_STOMP_MOVEIT_UTILS_POLYNOMIAL_H_ */