utils.h

#ifndef INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_UTILS_H_
#define INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_UTILS_H_

#include <string>
#include <vector>
#include <Eigen/Core>

namespace stomp_core
{

struct Rollout
{
  Eigen::MatrixXd noise;                   
  Eigen::MatrixXd parameters_noise;        
  Eigen::VectorXd state_costs;             
  Eigen::MatrixXd control_costs;           
  Eigen::MatrixXd total_costs;             
  Eigen::MatrixXd probabilities;           
  std::vector<double> full_probabilities; 
  std::vector<double> full_costs;         
  double importance_weight;               
  double total_cost;                      
};


namespace DerivativeOrders
{
enum DerivativeOrder
{
  STOMP_POSITION = 0,     
  STOMP_VELOCITY = 1,     
  STOMP_ACCELERATION = 2, 
  STOMP_JERK = 3          
};
}

namespace TrajectoryInitializations
{
enum TrajectoryInitialization
{
  LINEAR_INTERPOLATION = 1,       
  CUBIC_POLYNOMIAL_INTERPOLATION, 
  MININUM_CONTROL_COST            
};
}

struct StompConfiguration
{
  // General settings
  int num_iterations;                    
  int num_iterations_after_valid;        
  int num_timesteps;                     
  int num_dimensions;                    
  double delta_t;                        
  int initialization_method;             
  // Probability Calculation
  double exponentiated_cost_sensitivity; 
  // Noisy trajectory generation
  int num_rollouts;                      
  int max_rollouts;                      
  // Cost calculation
  double control_cost_weight;            
};

static const int FINITE_DIFF_RULE_LENGTH = 7;

static const double FINITE_CENTRAL_DIFF_COEFFS[FINITE_DIFF_RULE_LENGTH][FINITE_DIFF_RULE_LENGTH] = {
    {0, 0        , 0        , 1        , 0        , 0         , 0      }, // position
    {0, 1.0/12.0 , -2.0/3.0 , 0        , 2.0/3.0  , -1.0/12.0 , 0      }, // velocity
    {0, -1/12.0  , 16/12.0  , -30/12.0 , 16/12.0  , -1/12.0   , 0      }, // acceleration (five point stencil)
    {0, 1/12.0   , -17/12.0 , 46/12.0  , -46/12.0 , 17/12.0   , -1/12.0}  // jerk
};

static const double FINITE_FORWARD_DIFF_COEFFS[FINITE_DIFF_RULE_LENGTH][FINITE_DIFF_RULE_LENGTH] = {
    {1          , 0         , 0         , 0      , 0        , 0         , 0    }, // position
    {-25.0/12.0 , 4.0       , -3.0      , 4.0/3.0, -1.0/4.0 , 0         , 0    }, // velocity
    {15.0/4.0   , -77.0/6.0 , 107.0/6.0 , -13.0  , 61.0/12.0, -5.0/6.0  , 0    }, // acceleration (five point stencil)
    {-49/8      , 29        , -461/8    , 62     , -307/8   , 13        , -15/8}  // jerk
};

void generateFiniteDifferenceMatrix(int num_time_steps, DerivativeOrders::DerivativeOrder order, double dt,
                                    Eigen::MatrixXd& diff_matrix);

void differentiate(const Eigen::VectorXd& parameters, DerivativeOrders::DerivativeOrder order,
                          double dt, Eigen::VectorXd& derivatives );

void generateSmoothingMatrix(int num_time_steps, double dt, Eigen::MatrixXd& projection_matrix_M);

void toVector(const Eigen::MatrixXd& m,std::vector<Eigen::VectorXd>& v);

std::string toString(const std::vector<Eigen::VectorXd>& data);

std::string toString(const Eigen::VectorXd& data);

std::string toString(const Eigen::MatrixXd& data);

} /* namespace stomp */

#endif /* INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_UTILS_H_ */