MCRPlannerGoalSet.h

#ifndef MCR_PLANNER_GOAL_SET_H
#define MCR_PLANNER_GOAL_SET_H

#include "MotionPlanner.h"
#include "CSpace.h"
#include <KrisLibrary/utils/Subset.h>


class MCRPlannerGoalSet
{
 public:
  struct Milestone {
    Config q;
    int mode;
  };
  struct Edge {
    EdgePlannerPtr e;
    int mode;
  };
  typedef Graph::UndirectedGraph<Milestone,Edge> Roadmap;

  struct Mode {
    Subset subset;      //subset covered by this mode
    std::vector<int> roadmapNodes;
    std::vector<Subset> pathCovers;   //minimal covers leading from the start to this mode
    Real minCost;
  };
  struct Transition {
    std::vector<std::pair<int,int> > connections;
  };
  typedef Graph::UndirectedGraph<Mode,Transition> ModeGraph;

  MCRPlannerGoalSet(CSpace* space);
  void Init(const Config& start,CSet* goal);
  void Expand(Real maxExplanationCost,std::vector<int>& newNodes);
  void Expand2(Real maxExplanationCost,std::vector<int>& newNodes);
  void Plan(int initialLimit,const std::vector<int>& expansionSchedule,std::vector<int>& bestPath,Subset& cover);
  void BuildRoadmap(Real maxExplanationCost,RoadmapPlanner& prm);
  void BuildCCGraph(Graph::UndirectedGraph<Subset,int>& G);
  bool CoveragePath(int s,int t,const Subset& cover,std::vector<int>& path,Subset& pathCover);
  bool GreedyPath(int s,int t,std::vector<int>& path,Subset& pathCover);
  bool OptimalPath(int s,int t,std::vector<int>& path,Subset& pathCover);
  bool GreedyPath(std::vector<int>& path,Subset& pathCover);
  bool OptimalPath(std::vector<int>& path,Subset& pathCover);

  //helpers
  Real Cost(const Subset& s) const;
  int AddNode(const Config& q,int parent=-1);
  int AddNode(const Config& q,const Subset& subset,int parent=-1);
  bool AddEdge(int i,int j,int depth=0);
  int AddEdge(int i,const Config& q,Real maxExplanationCost);  //returns index of q
  void AddEdgeRaw(int i,int j);
  int ExtendToward(int i,const Config& qdest,Real maxExplanationCost);
  void KNN(const Config& q,int k,std::vector<int>& neighbors,std::vector<Real>& distances);
  void KNN(const Config& q,Real maxExplanationCost,int k,std::vector<int>& neighbors,std::vector<Real>& distances);
  void UpdatePathsGreedy();
  void UpdatePathsComplete();
  void UpdatePathsGreedy2(int nstart=-1);
  void UpdatePathsComplete2(int nstart=-1);
  //returns true if a path from mode a to mode b can improve the explanation
  //at b
  bool CanImproveConnectivity(const Mode& ma,const Mode& mb,Real maxExplanationCost);
  //updates the minCost member of m
  void UpdateMinCost(Mode& m);
  //fast checking of whether the cost of the local constraints at q exceed the
  //given limit
  bool ExceedsCostLimit(const Config& q,Real limit,Subset& violations);
  //fast checking of whether the cost of the local constraints violated on 
  //the edge ab exceed the given limit
  bool ExceedsCostLimit(const Config& a,const Config& b,Real limit,Subset& violations);

  void GetCover(const std::vector<int>& path,Subset& cover) const;
  Real GetLength(const std::vector<int>& path) const;
  void GetMilestonePath(const std::vector<int>& path,MilestonePath& mpath) const;
 
  Config start;
  CSpace* space;
  CSet* goalSet;
  Optimization::NonlinearProgram* goalNumeric;
  std::vector<int> goalNodes;   //a list of goal nodes in roadmap

  //weighted explanation
  std::vector<Real> obstacleWeights;
 
  //settings for RRT* like expansion
  int numConnections;
  Real connectThreshold,expandDistance,goalConnectThreshold;
  Real goalBiasProbability;  //probability of expanding toward the goal
  bool bidirectional;        //not functional yet
  
  //settings for path update
  bool updatePathsComplete;
  bool updatePathsDynamic;
  int updatePathsMax;

  Roadmap roadmap;
  ModeGraph modeGraph;

  //planning statistics
  int numExpands,numRefinementAttempts,numRefinementSuccesses,numExplorationAttempts,
    numEdgeChecks,numConfigChecks,
    numUpdatePaths,numUpdatePathsIterations;
  double timeNearestNeighbors,timeRefine,timeExplore,timeUpdatePaths,timeOverhead;
};

#endif