PerturbationRoadmap.hpp

//
//  PerturbationRoadmap.hpp
//  libmove3d-motion
//
//  Created by Jim Mainprice on 01/02/12.
//  Copyright (c) 2012 LAAS-CNRS. All rights reserved.
//

#ifndef PERTURBATION_ROADMAP_hpp
#define PERTURBATION_ROADMAP_hpp

#include "planner/PRM/PRM.hpp"
#include "planner/TrajectoryOptim/Classic/costOptimization.hpp"

#include "API/Trajectory/trajectory.hpp"

class PerturbationRoadmap : public PRM
{
public:
        PerturbationRoadmap(Robot* R, Graph* G);
        
        ~PerturbationRoadmap();
        
        virtual unsigned init();
  
protected:
  
  void getTranslationBounds();
        
  void addTrajectory(const API::Trajectory& T);
  
  bool testPerturb( confPtr_t q_new, std::vector<Node*>& vect_conf );
  
  bool findBestCycle( confPtr_t q_new, double dist, std::vector<Node*>& vect_conf );
  
  bool addPerturbation( confPtr_t q_rand );
  
  bool expandPerturbation( confPtr_t q_rand );
  
  
  confPtr_t trajShootConfiguration();
  
  confPtr_t getExpansionConfiguration(bool sample_on_traj);
  
  Node* getClosestOnTraj( confPtr_t q_rand );
  
  double distToTraj( confPtr_t q_rand );
  
        virtual void expandOneStep();
        
private:
  Node* m_main_compco;
  double m_delta;
  bool m_descent;
  bool m_sampled_on_traj;
  double m_max_dist_to_traj;
  int m_K_Nearest;
  
  bool m_use_rejection;
  double m_std_dev_trans;
  double m_std_dev_rot;
  double m_radian_steps;
  double m_transl_steps;
  double m_transl_max;
  
  confPtr_t m_qi;
  confPtr_t m_qf;
  
  API::CostOptimization* m_traj;
};

#endif