Go to the documentation of this file.00001
00027 #ifndef STOMP_PLUGINS_INCLUDE_STOMP_PLUGINS_NOISE_GENERATORS_GOAL_GUIDED_MULTIVARIATE_GAUSSIAN_H_
00028 #define STOMP_PLUGINS_INCLUDE_STOMP_PLUGINS_NOISE_GENERATORS_GOAL_GUIDED_MULTIVARIATE_GAUSSIAN_H_
00029
00030 #include <stomp_moveit/noise_generators/stomp_noise_generator.h>
00031 #include <stomp_moveit/utils/multivariate_gaussian.h>
00032 #include "stomp_moveit/utils/kinematics.h"
00033
00034
00035 namespace stomp_moveit
00036 {
00037 namespace noise_generators
00038 {
00039
00040 typedef boost::mt19937 RGNType;
00041 typedef boost::variate_generator< RGNType, boost::uniform_real<> > RandomGenerator;
00042
00051 class GoalGuidedMultivariateGaussian: public StompNoiseGenerator
00052 {
00053 public:
00054 GoalGuidedMultivariateGaussian();
00055 virtual ~GoalGuidedMultivariateGaussian();
00056
00064 virtual bool initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
00065 const std::string& group_name,const XmlRpc::XmlRpcValue& config) override;
00066
00072 virtual bool configure(const XmlRpc::XmlRpcValue& config) override;
00073
00082 virtual bool setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
00083 const moveit_msgs::MotionPlanRequest &req,
00084 const stomp_core::StompConfiguration &config,
00085 moveit_msgs::MoveItErrorCodes& error_code) override;
00086
00098 virtual bool generateNoise(const Eigen::MatrixXd& parameters,
00099 std::size_t start_timestep,
00100 std::size_t num_timesteps,
00101 int iteration_number,
00102 int rollout_number,
00103 Eigen::MatrixXd& parameters_noise,
00104 Eigen::MatrixXd& noise) override;
00105
00114 virtual void done(bool success,int total_iterations,double final_cost,const Eigen::MatrixXd& parameters){}
00115
00116
00117 virtual std::string getName() const
00118 {
00119 return name_ + "/" + group_;
00120 }
00121
00122
00123 virtual std::string getGroupName() const
00124 {
00125 return group_;
00126 }
00127
00128 protected:
00129
00130 virtual bool setNoiseGeneration(const planning_scene::PlanningSceneConstPtr& planning_scene,
00131 const moveit_msgs::MotionPlanRequest &req,
00132 const stomp_core::StompConfiguration &config,
00133 moveit_msgs::MoveItErrorCodes& error_code);
00134
00135 virtual bool setGoalConstraints(const planning_scene::PlanningSceneConstPtr& planning_scene,
00136 const moveit_msgs::MotionPlanRequest &req,
00137 const stomp_core::StompConfiguration &config,
00138 moveit_msgs::MoveItErrorCodes& error_code);
00139
00140 virtual bool generateRandomGoal(const Eigen::VectorXd& seed,Eigen::VectorXd& goal_joint_pose);
00141
00142 protected:
00143
00144
00145 std::string name_;
00146 std::string group_;
00147
00148
00149 std::string tool_link_;
00150
00151
00152 utils::kinematics::KinematicConfig kc_;
00154
00155 std::vector<utils::MultivariateGaussianPtr> traj_noise_generators_;
00156 Eigen::VectorXd raw_noise_;
00157 std::vector<double> stddev_;
00158 std::vector<double> goal_stddev_;
00160
00161 boost::shared_ptr<RandomGenerator> goal_rand_generator_;
00163
00164 moveit::core::RobotModelConstPtr robot_model_;
00165 moveit::core::RobotStatePtr state_;
00166
00167 };
00168
00169 }
00170 }
00171
00172 #endif