.. _program_listing_file__tmp_ws_src_stomp_include_stomp_utils.h:

Program Listing for File utils.h
================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_stomp_include_stomp_utils.h>` (``/tmp/ws/src/stomp/include/stomp/utils.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   #ifndef INDUSTRIAL_MOVEIT_STOMP_INCLUDE_STOMP_STOMP_UTILS_H_
   #define INDUSTRIAL_MOVEIT_STOMP_INCLUDE_STOMP_STOMP_UTILS_H_
   
   #include <string>
   #include <vector>
   #include <Eigen/Core>
   
   namespace stomp
   {
   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 DerivativeOrders
   
   namespace TrajectoryInitializations
   {
   enum TrajectoryInitialization
   {
     LINEAR_INTERPOLATION = 1,       
     CUBIC_POLYNOMIAL_INTERPOLATION, 
     MININUM_CONTROL_COST            
   };
   }  // namespace TrajectoryInitializations
   
   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_INCLUDE_STOMP_STOMP_UTILS_H_ */