00001 00026 #ifndef INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_H_ 00027 #define INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_H_ 00028 00029 #include <atomic> 00030 #include <stomp_core/utils.h> 00031 #include <XmlRpc.h> 00032 #include "stomp_core/task.h" 00033 00034 namespace stomp_core 00035 { 00036 00038 class Stomp 00039 { 00040 public: 00046 Stomp(const StompConfiguration& config,TaskPtr task); 00047 00055 bool solve(const std::vector<double>& first,const std::vector<double>& last, 00056 Eigen::MatrixXd& parameters_optimized); 00057 00065 bool solve(const Eigen::VectorXd& first,const Eigen::VectorXd& last, 00066 Eigen::MatrixXd& parameters_optimized); 00067 00074 bool solve(const Eigen::MatrixXd& initial_parameters, 00075 Eigen::MatrixXd& parameters_optimized); 00076 00081 void setConfig(const StompConfiguration& config); 00082 00088 bool cancel(); 00089 00094 bool clear(); 00095 00096 00097 protected: 00098 00099 // initialization methods 00104 bool resetVariables(); 00105 00112 bool computeInitialTrajectory(const std::vector<double>& first,const std::vector<double>& last); 00113 00114 // optimization steps 00120 bool runSingleIteration(); 00121 00126 bool generateNoisyRollouts(); 00127 00132 bool filterNoisyRollouts(); 00133 00138 bool computeNoisyRolloutsCosts(); 00139 00144 bool computeRolloutsStateCosts(); 00145 00153 bool computeRolloutsControlCosts(); 00154 00159 bool computeProbabilities(); 00160 00165 bool updateParameters(); 00166 00173 bool computeOptimizedCost(); 00174 00175 protected: 00176 00177 // process control 00178 std::atomic<bool> proceed_; 00179 TaskPtr task_; 00180 StompConfiguration config_; 00181 unsigned int current_iteration_; 00183 // optimized parameters 00184 bool parameters_valid_; 00185 double parameters_total_cost_; 00186 double current_lowest_cost_; 00187 Eigen::MatrixXd parameters_optimized_; 00188 Eigen::MatrixXd parameters_updates_; 00189 Eigen::VectorXd parameters_state_costs_; 00190 Eigen::MatrixXd parameters_control_costs_; 00192 // rollouts 00193 std::vector<Rollout> noisy_rollouts_; 00194 std::vector<Rollout> reused_rollouts_; 00195 int num_active_rollouts_; 00197 // finite difference and optimization matrices 00198 int num_timesteps_padded_; 00199 int start_index_padded_; 00200 Eigen::MatrixXd finite_diff_matrix_A_padded_; 00201 Eigen::MatrixXd control_cost_matrix_R_padded_; 00202 Eigen::MatrixXd control_cost_matrix_R_; 00203 Eigen::MatrixXd inv_control_cost_matrix_R_; 00206 }; 00207 00208 } /* namespace stomp */ 00209 00210 #endif /* INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_H_ */