stomp_moveit: stomp_optimization

Go to the documentation of this file.
00001 
00026 #ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_STOMP_OPTIMIZATION_TASK_H_
00027 #define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_STOMP_OPTIMIZATION_TASK_H_
00028 
00029 #include <memory>
00030 #include <moveit_msgs/MotionPlanRequest.h>
00031 #include <moveit/robot_model/robot_model.h>
00032 #include <stomp_core/task.h>
00033 #include <stomp_moveit/cost_functions/stomp_cost_function.h>
00034 #include <XmlRpcValue.h>
00035 #include <pluginlib/class_loader.h>
00036 #include <stomp_moveit/noise_generators/stomp_noise_generator.h>
00037 #include <stomp_moveit/noisy_filters/stomp_noisy_filter.h>
00038 #include <stomp_moveit/update_filters/stomp_update_filter.h>
00039 
00040 
00041 namespace stomp_moveit
00042 {
00043 
00044 typedef pluginlib::ClassLoader<stomp_moveit::cost_functions::StompCostFunction> CostFunctionLoader;
00045 typedef std::shared_ptr<CostFunctionLoader> CostFuctionLoaderPtr;
00046 typedef pluginlib::ClassLoader<stomp_moveit::noisy_filters::StompNoisyFilter> NoisyFilterLoader;
00047 typedef std::shared_ptr<NoisyFilterLoader> NoisyFilterLoaderPtr;
00048 typedef pluginlib::ClassLoader<stomp_moveit::update_filters::StompUpdateFilter> UpdateFilterLoader;
00049 typedef std::shared_ptr<UpdateFilterLoader> UpdateFilterLoaderPtr;
00050 typedef pluginlib::ClassLoader<stomp_moveit::noise_generators::StompNoiseGenerator> NoiseGeneratorLoader;
00051 typedef std::shared_ptr<NoiseGeneratorLoader> NoiseGeneratorLoaderPtr;
00052 
00053 
00062 class StompOptimizationTask: public stomp_core::Task
00063 {
00064 public:
00071   StompOptimizationTask(moveit::core::RobotModelConstPtr robot_model_ptr, std::string group_name,
00072                         const XmlRpc::XmlRpcValue& config);
00073   virtual ~StompOptimizationTask();
00074 
00083   virtual bool setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
00084                    const moveit_msgs::MotionPlanRequest &req,
00085                    const stomp_core::StompConfiguration &config,
00086                    moveit_msgs::MoveItErrorCodes& error_code);
00087 
00099   virtual bool generateNoisyParameters(const Eigen::MatrixXd& parameters,
00100                                        std::size_t start_timestep,
00101                                        std::size_t num_timesteps,
00102                                        int iteration_number,
00103                                        int rollout_number,
00104                                        Eigen::MatrixXd& parameters_noise,
00105                                        Eigen::MatrixXd& noise) override;
00106 
00118   virtual bool computeNoisyCosts(const Eigen::MatrixXd& parameters,
00119                        std::size_t start_timestep,
00120                        std::size_t num_timesteps,
00121                        int iteration_number,
00122                        int rollout_number,
00123                        Eigen::VectorXd& costs,
00124                        bool& validity) override;
00125 
00136   virtual bool computeCosts(const Eigen::MatrixXd& parameters,
00137                        std::size_t start_timestep,
00138                        std::size_t num_timesteps,
00139                        int iteration_number,
00140                        Eigen::VectorXd& costs,
00141                        bool& validity) override;
00142 
00155   virtual bool filterNoisyParameters(std::size_t start_timestep,
00156                                      std::size_t num_timesteps,
00157                                      int iteration_number,
00158                                      int rollout_number,
00159                                      Eigen::MatrixXd& parameters,
00160                                      bool& filtered) override;
00161 
00173   virtual bool filterParameterUpdates(std::size_t start_timestep,
00174                                       std::size_t num_timesteps,
00175                                       int iteration_number,
00176                                       const Eigen::MatrixXd& parameters,
00177                                       Eigen::MatrixXd& updates) override;
00178 
00187   virtual void postIteration(std::size_t start_timestep,
00188                                 std::size_t num_timesteps,int iteration_number,double cost,const Eigen::MatrixXd& parameters);
00189 
00198   virtual void done(bool success,int total_iterations,double final_cost,const Eigen::MatrixXd& parameters) override;
00199 
00200 protected:
00201 
00202   // robot environment
00203   std::string group_name_;
00204   moveit::core::RobotModelConstPtr robot_model_ptr_;
00205   planning_scene::PlanningSceneConstPtr planning_scene_ptr_;
00206 
00208   CostFuctionLoaderPtr cost_function_loader_;
00209   NoisyFilterLoaderPtr noisy_filter_loader_;
00210   UpdateFilterLoaderPtr update_filter_loader_;
00211   NoiseGeneratorLoaderPtr noise_generator_loader_;
00212 
00214   std::vector<cost_functions::StompCostFunctionPtr> cost_functions_;
00215   std::vector<noisy_filters::StompNoisyFilterPtr> noisy_filters_;
00216   std::vector<update_filters::StompUpdateFilterPtr> update_filters_;
00217   std::vector<noise_generators::StompNoiseGeneratorPtr> noise_generators_;
00218 };
00219 
00220 
00221 } /* namespace stomp_moveit */
00222 
00223 #endif /* INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_STOMP_OPTIMIZATION_TASK_H_ */