stomp.h

Go to the documentation of this file.

#ifndef INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_H_
#define INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_H_

#include <atomic>
#include <stomp_core/utils.h>
#include <XmlRpc.h>
#include "stomp_core/task.h"

namespace stomp_core
{

class Stomp
{
public:
  Stomp(const StompConfiguration& config,TaskPtr task);

  bool solve(const std::vector<double>& first,const std::vector<double>& last,
             Eigen::MatrixXd& parameters_optimized);

  bool solve(const Eigen::VectorXd& first,const Eigen::VectorXd& last,
             Eigen::MatrixXd& parameters_optimized);

  bool solve(const Eigen::MatrixXd& initial_parameters,
             Eigen::MatrixXd& parameters_optimized);

  void setConfig(const StompConfiguration& config);

  bool cancel();

  bool clear();


protected:

  // initialization methods
  bool resetVariables();

  bool computeInitialTrajectory(const std::vector<double>& first,const std::vector<double>& last);

  // optimization steps
  bool runSingleIteration();

  bool generateNoisyRollouts();

  bool filterNoisyRollouts();

  bool computeNoisyRolloutsCosts();

  bool computeRolloutsStateCosts();

  bool computeRolloutsControlCosts();

  bool computeProbabilities();

  bool updateParameters();

  bool computeOptimizedCost();

protected:

  // process control
  std::atomic<bool> proceed_;                      
  TaskPtr task_;                                   
  StompConfiguration config_;                      
  unsigned int current_iteration_;                 
  // optimized parameters
  bool parameters_valid_;                          
  bool parameters_valid_prev_;                     
  double parameters_total_cost_;                   
  double current_lowest_cost_;                     
  Eigen::MatrixXd parameters_optimized_;           
  Eigen::MatrixXd parameters_updates_;             
  Eigen::VectorXd parameters_state_costs_;         
  Eigen::MatrixXd parameters_control_costs_;       
  // rollouts
  std::vector<Rollout> noisy_rollouts_;            
  std::vector<Rollout> reused_rollouts_;           
  int num_active_rollouts_;                        
  // finite difference and optimization matrices
  int num_timesteps_padded_;                       
  int start_index_padded_;                         
  Eigen::MatrixXd finite_diff_matrix_A_padded_;    
  Eigen::MatrixXd control_cost_matrix_R_padded_;   
  Eigen::MatrixXd control_cost_matrix_R_;          
  Eigen::MatrixXd inv_control_cost_matrix_R_;      
};

} /* namespace stomp */

#endif /* INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_H_ */