MotionPlanner.h

#ifndef ROBOTICS_MOTION_PLANNER_H
#define ROBOTICS_MOTION_PLANNER_H

#include <KrisLibrary/graph/Tree.h>
#include <KrisLibrary/graph/UndirectedGraph.h>
#include <KrisLibrary/graph/ConnectedComponents.h>
#include <vector>
#include <list>
#include "CSpace.h"
#include "EdgePlanner.h"
#include "Path.h"

class PointLocationBase;
class ObjectiveFunctionalBase;


class RoadmapPlanner
{
public:
  typedef Graph::UndirectedGraph<Config,EdgePlannerPtr> Roadmap;

  RoadmapPlanner(CSpace*);
  virtual ~RoadmapPlanner();
  
  virtual void Cleanup();
  virtual void GenerateConfig(Config& x);
  virtual int AddMilestone(const Config& x);
  virtual int TestAndAddMilestone(const Config& x);
  virtual void ConnectEdge(int i,int j,const EdgePlannerPtr& e);
  virtual EdgePlannerPtr TestAndConnectEdge(int i,int j);
  virtual bool HasEdge(int i,int j) { return roadmap.FindEdge(i,j)!=NULL; }
  virtual EdgePlannerPtr GetEdge(int i,int j) { return *roadmap.FindEdge(i,j); }
  virtual bool AreConnected(int i,int j) { return ccs.SameComponent(i,j); }
  virtual bool AreConnected(int i,int j) const { return ccs.SameComponent(i,j); }
  virtual void ConnectToNeighbors(int i,Real connectionThreshold,bool ccReject=true);
  virtual void ConnectToNearestNeighbors(int i,int k,bool ccReject=true);
  virtual void Generate(int numSamples,Real connectionThreshold); 
  virtual void CreatePath(int i,int j,MilestonePath& path);
  virtual Real OptimizePath(int i,const std::vector<int>& goals,ObjectiveFunctionalBase* cost,MilestonePath& path);

  CSpace* space;
  Roadmap roadmap;
  Graph::ConnectedComponents ccs;
  std::shared_ptr<PointLocationBase> pointLocator;
};


class TreeRoadmapPlanner
{
public:
  struct Milestone
  {
    Config x; 
    int id;
    int connectedComponent;
  };
  
  typedef Graph::TreeNode<Milestone,EdgePlannerPtr > Node;
  
  TreeRoadmapPlanner(CSpace*);
  virtual ~TreeRoadmapPlanner();
  
  virtual void GenerateConfig(Config& x);
  virtual Node* AddMilestone(const Config& x);
  virtual Node* TestAndAddMilestone(const Config& x);
  virtual Node* AddInfeasibleMilestone(const Config& x) { return NULL; }
  virtual Node* Extend(); 
  virtual void Cleanup();
  virtual void ConnectToNeighbors(Node*);
  virtual EdgePlannerPtr TryConnect(Node*,Node*);
  virtual void DeleteSubtree(Node* n);
  //helpers
  //default implementation uses pointLocator
  virtual Node* ClosestMilestone(const Config& x);
  virtual int ClosestMilestoneIndex(const Config& x);
  //default implementation uses O(n) search
  virtual Node* ClosestMilestoneInComponent(int component,const Config& x);
  virtual Node* ClosestMilestoneInSubtree(Node* node,const Config& x);
  Node* Extend(Node* n,const Config& x);
  Node* TryExtend(Node* n,const Config& x);
  void AttachChild(Node* p, Node* c, const EdgePlannerPtr& e);   //c will become a child of p
  Node* SplitEdge(Node* p,Node* n,Real u);
  void CreatePath(Node* a, Node* b, MilestonePath& path);
  virtual Real OptimizePath(Node* a,const std::vector<Node*>& goals,ObjectiveFunctionalBase* cost,MilestonePath& path);
  
  CSpace* space;
  std::vector<Node*> connectedComponents;
  Real connectionThreshold;
  
  //temporary
  std::vector<Vector> milestoneConfigs;
  std::vector<Node*> milestones;
  std::shared_ptr<PointLocationBase> pointLocator;
  Config x;
};


class PerturbationTreePlanner : public TreeRoadmapPlanner
{
public:
  PerturbationTreePlanner(CSpace*s);
  virtual void GenerateConfig(Config& x);
  virtual Node* AddMilestone(const Config& x); 
  virtual void Cleanup();

  //overrideable 
  virtual Node* SelectMilestone(const std::vector<Node*>& milestones);

  Real delta;
  std::vector<Real> weights;
};

class RRTPlanner : public TreeRoadmapPlanner
{
public:
  RRTPlanner(CSpace*s);
  virtual Node* Extend();
  
  Real delta;
};

class BidirectionalRRTPlanner : public RRTPlanner
{
public:
  BidirectionalRRTPlanner(CSpace*s);
  void Init(const Config& start, const Config& goal);
  bool Plan();
  void CreatePath(MilestonePath&) const;
};

/*
class VisibilityPRM : public RandomizedPlanner
{
public:
  VisibilityPRM(CSpace*s);
  //may return NULL for rejected config
  virtual Node* AddMilestone(const Config& x);
  virtual Node* AddInfeasibleMilestone(const Config& x) { return NULL; }
  virtual Node* Extend();
  virtual Node* CanConnectComponent(int i,const Config& x);
};
*/

#endif