goal_guided_multivariate_gaussian.h

Go to the documentation of this file.

#ifndef STOMP_PLUGINS_INCLUDE_STOMP_PLUGINS_NOISE_GENERATORS_GOAL_GUIDED_MULTIVARIATE_GAUSSIAN_H_
#define STOMP_PLUGINS_INCLUDE_STOMP_PLUGINS_NOISE_GENERATORS_GOAL_GUIDED_MULTIVARIATE_GAUSSIAN_H_

#include <stomp_moveit/noise_generators/stomp_noise_generator.h>
#include <stomp_moveit/utils/multivariate_gaussian.h>
#include "stomp_moveit/utils/kinematics.h"


namespace stomp_moveit
{
namespace noise_generators
{

typedef boost::mt19937 RGNType;
typedef boost::variate_generator< RGNType, boost::uniform_real<> > RandomGenerator;

class GoalGuidedMultivariateGaussian: public StompNoiseGenerator
{
public:
  GoalGuidedMultivariateGaussian();
  virtual ~GoalGuidedMultivariateGaussian();

  virtual bool initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
                          const std::string& group_name,const XmlRpc::XmlRpcValue& config) override;

  virtual bool configure(const XmlRpc::XmlRpcValue& config) override;

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

  virtual bool generateNoise(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 void done(bool success,int total_iterations,double final_cost,const Eigen::MatrixXd& parameters){}


  virtual std::string getName() const
  {
    return name_ + "/" + group_;
  }


  virtual std::string getGroupName() const
  {
    return group_;
  }

protected:

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

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

  virtual bool generateRandomGoal(const Eigen::VectorXd& reference_joint_pose,Eigen::VectorXd& goal_joint_pose);

protected:

  // names
  std::string name_;
  std::string group_;

  // goal constraints
  std::string tool_link_;
  Eigen::VectorXd tool_goal_tolerance_;

  // ros parameters
  std::vector<double> stddev_;                                        
  // noisy trajectory generation
  std::vector<utils::MultivariateGaussianPtr> traj_noise_generators_; 
  Eigen::VectorXd raw_noise_;                                         
  // random goal generation
  boost::shared_ptr<RandomGenerator> goal_rand_generator_;            
  // robot
  moveit::core::RobotModelConstPtr robot_model_;
  moveit::core::RobotStatePtr state_;
  stomp_moveit::utils::kinematics::IKSolverPtr ik_solver_;

};

} /* namespace noise_generators */
} /* namespace stomp_moveit */

#endif /* STOMP_PLUGINS_INCLUDE_STOMP_PLUGINS_NOISE_GENERATORS_GOAL_GUIDED_MULTIVARIATE_GAUSSIAN_H_ */