stomp_optimization_task.h

Go to the documentation of this file.

#ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_STOMP_OPTIMIZATION_TASK_H_
#define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_STOMP_OPTIMIZATION_TASK_H_

#include <memory>
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit/robot_model/robot_model.h>
#include <stomp_core/task.h>
#include <stomp_moveit/cost_functions/stomp_cost_function.h>
#include <XmlRpcValue.h>
#include <pluginlib/class_loader.h>
#include <stomp_moveit/noise_generators/stomp_noise_generator.h>
#include <stomp_moveit/noisy_filters/stomp_noisy_filter.h>
#include <stomp_moveit/update_filters/stomp_update_filter.h>


namespace stomp_moveit
{

typedef pluginlib::ClassLoader<stomp_moveit::cost_functions::StompCostFunction> CostFunctionLoader;
typedef std::shared_ptr<CostFunctionLoader> CostFuctionLoaderPtr;
typedef pluginlib::ClassLoader<stomp_moveit::noisy_filters::StompNoisyFilter> NoisyFilterLoader;
typedef std::shared_ptr<NoisyFilterLoader> NoisyFilterLoaderPtr;
typedef pluginlib::ClassLoader<stomp_moveit::update_filters::StompUpdateFilter> UpdateFilterLoader;
typedef std::shared_ptr<UpdateFilterLoader> UpdateFilterLoaderPtr;
typedef pluginlib::ClassLoader<stomp_moveit::noise_generators::StompNoiseGenerator> NoiseGeneratorLoader;
typedef std::shared_ptr<NoiseGeneratorLoader> NoiseGeneratorLoaderPtr;


class StompOptimizationTask: public stomp_core::Task
{
public:
  StompOptimizationTask(moveit::core::RobotModelConstPtr robot_model_ptr, std::string group_name,
                        const XmlRpc::XmlRpcValue& config);
  virtual ~StompOptimizationTask();

  virtual bool setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
                   const moveit_msgs::MotionPlanRequest &req,
                   const stomp_core::StompConfiguration &config,
                   moveit_msgs::MoveItErrorCodes& error_code);

  virtual bool generateNoisyParameters(const Eigen::MatrixXd& parameters,
                                       std::size_t start_timestep,
                                       std::size_t num_timesteps,
                                       int iteration_number,
                                       int rollout_number,
                                       Eigen::MatrixXd& parameters_noise,
                                       Eigen::MatrixXd& noise) override;

  virtual bool computeNoisyCosts(const Eigen::MatrixXd& parameters,
                       std::size_t start_timestep,
                       std::size_t num_timesteps,
                       int iteration_number,
                       int rollout_number,
                       Eigen::VectorXd& costs,
                       bool& validity) override;

  virtual bool computeCosts(const Eigen::MatrixXd& parameters,
                       std::size_t start_timestep,
                       std::size_t num_timesteps,
                       int iteration_number,
                       Eigen::VectorXd& costs,
                       bool& validity) override;

  virtual bool filterNoisyParameters(std::size_t start_timestep,
                                     std::size_t num_timesteps,
                                     int iteration_number,
                                     int rollout_number,
                                     Eigen::MatrixXd& parameters,
                                     bool& filtered) override;

  virtual bool filterParameterUpdates(std::size_t start_timestep,
                                      std::size_t num_timesteps,
                                      int iteration_number,
                                      const Eigen::MatrixXd& parameters,
                                      Eigen::MatrixXd& updates) override;

  virtual void postIteration(std::size_t start_timestep,
                                std::size_t num_timesteps,int iteration_number,double cost,const Eigen::MatrixXd& parameters);

  virtual void done(bool success,int total_iterations,double final_cost,const Eigen::MatrixXd& parameters) override;

protected:

  // robot environment
  std::string group_name_;
  moveit::core::RobotModelConstPtr robot_model_ptr_;
  planning_scene::PlanningSceneConstPtr planning_scene_ptr_;

  CostFuctionLoaderPtr cost_function_loader_;
  NoisyFilterLoaderPtr noisy_filter_loader_;
  UpdateFilterLoaderPtr update_filter_loader_;
  NoiseGeneratorLoaderPtr noise_generator_loader_;

  std::vector<cost_functions::StompCostFunctionPtr> cost_functions_;
  std::vector<noisy_filters::StompNoisyFilterPtr> noisy_filters_;
  std::vector<update_filters::StompUpdateFilterPtr> update_filters_;
  std::vector<noise_generators::StompNoiseGeneratorPtr> noise_generators_;
};


} /* namespace stomp_moveit */

#endif /* INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_STOMP_OPTIMIZATION_TASK_H_ */