OptimalMotionPlanner.h

#ifndef OPTIMAL_MOTION_PLANNER_H
#define OPTIMAL_MOTION_PLANNER_H

#include "MotionPlanner.h"
#include <KrisLibrary/graph/ShortestPaths.h>

class PRMStarPlanner : public RoadmapPlanner
{
 public:
  PRMStarPlanner(CSpace* space);
  void Init(const Config& start,const Config& goal);
  void SetMaxCost(Real cmax);
  virtual void Cleanup();
  void PlanMore();
  void KNN(const Config& x,int k,vector<int>& nn);
  void Neighbors(const Config& x,Real r,vector<int>& neighbors);
  bool HasPath() const;
  bool GetPath(MilestonePath& path);
  bool GetPath(int a,int b,vector<int>& nodes,MilestonePath& path);
  Real OptimizePath(ObjectiveFunctionalBase* cost,MilestonePath& path);
  Real OptimizePath(int a,const vector<int>& goals,ObjectiveFunctionalBase* cost,MilestonePath& path);
  bool CheckPath(int a,int b);
  virtual int AddMilestone(const Config& x);
  virtual void ConnectEdge(int i,int j,const EdgePlannerPtr& e);
  void ConnectEdgeLazy(int i,int j,const EdgePlannerPtr& e);

  //configuration variables
  bool lazy;
  bool rrg;
  bool bidirectional;
  bool connectByRadius;
  Real connectRadiusConstant;
  Real connectNeighborsConstant;
  Real connectionThreshold;
  Real lazyCheckThreshold;
  Real suboptimalityFactor;

  int start,goal;
  typedef Graph::ShortestPathProblem<Config,EdgePlannerPtr> ShortestPathProblem;
  ShortestPathProblem spp,sppGoal,sppLB,sppLBGoal;
  Roadmap LBroadmap;

  //statistics
  int numPlanSteps;
  Real tCheck, tKnn, tConnect, tLazy, tLazyCheck, tShortestPaths;
  int numEdgeChecks;
  int numEdgePrechecks;
};


#endif