KrisLibrary  1.0.0
MCRPlanner.h
1 #ifndef MCR_PLANNER_H
2 #define MCR_PLANNER_H
3 
4 #include "MotionPlanner.h"
5 #include "CSpace.h"
6 #include <KrisLibrary/utils/Subset.h>
7 
30 {
31  public:
32  struct Milestone {
33  Config q;
34  int mode;
35  };
36  struct Edge {
37  EdgePlannerPtr e;
38  int mode;
39  };
41 
42  struct Mode {
43  Subset subset; //subset covered by this mode
44  std::vector<int> roadmapNodes;
45  std::vector<Subset> pathCovers; //minimal covers leading from the start to this mode
46  Real minCost;
47  };
48  struct Transition {
49  std::vector<std::pair<int,int> > connections;
50  };
52 
53  MCRPlanner(CSpace* space);
54  void Init(const Config& start,const Config& goal);
56  void Expand(Real maxExplanationCost,std::vector<int>& newNodes);
57  void Expand2(Real maxExplanationCost,std::vector<int>& newNodes);
59  void Plan(int initialLimit,const std::vector<int>& expansionSchedule,std::vector<int>& bestPath,Subset& cover);
61  void BuildRoadmap(Real maxExplanationCost,RoadmapPlanner& prm);
66  bool CoveragePath(int s,int t,const Subset& cover,std::vector<int>& path,Subset& pathCover);
68  bool GreedyPath(int s,int t,std::vector<int>& path,Subset& pathCover);
70  bool OptimalPath(int s,int t,std::vector<int>& path,Subset& pathCover);
74  void Completion(int s,int node,int t,Subset& pathCover);
75 
76  //helpers
77  Real Cost(const Subset& s) const;
78  int AddNode(const Config& q,int parent=-1);
79  int AddNode(const Config& q,const Subset& subset,int parent=-1);
80  bool AddEdge(int i,int j,int depth=0);
81  int AddEdge(int i,const Config& q,Real maxExplanationCost); //returns index of q
82  void AddEdgeRaw(int i,int j);
83  int ExtendToward(int i,const Config& qdest,Real maxExplanationCost);
84  void KNN(const Config& q,int k,std::vector<int>& neighbors,std::vector<Real>& distances);
85  void KNN(const Config& q,Real maxExplanationCost,int k,std::vector<int>& neighbors,std::vector<Real>& distances);
86  void UpdatePathsGreedy();
87  void UpdatePathsComplete();
88  void UpdatePathsGreedy2(int nstart=-1);
89  void UpdatePathsComplete2(int nstart=-1);
90  //returns true if a path from mode a to mode b can improve the explanation
91  //at b
92  bool CanImproveConnectivity(const Mode& ma,const Mode& mb,Real maxExplanationCost);
93  //updates the minCost member of m
94  void UpdateMinCost(Mode& m);
95  //fast checking of whether the cost of the local constraints at q exceed the
96  //given limit
97  bool ExceedsCostLimit(const Config& q,Real limit,Subset& violations);
98  //fast checking of whether the cost of the local constraints violated on
99  //the edge ab exceed the given limit
100  bool ExceedsCostLimit(const Config& a,const Config& b,Real limit,Subset& violations);
101 
103  void GetCover(const std::vector<int>& path,Subset& cover) const;
105  Real GetLength(const std::vector<int>& path) const;
107  void GetMilestonePath(const std::vector<int>& path,MilestonePath& mpath) const;
108 
109  Config start,goal;
110  CSpace* space;
111 
112  //weighted explanation
113  std::vector<Real> obstacleWeights;
114 
115  //settings for RRT* like expansion
116  int numConnections;
117  Real connectThreshold,expandDistance,goalConnectThreshold;
118  Real goalBiasProbability; //probability of expanding toward the goal
119  bool bidirectional; //not functional yet
120 
121  //settings for path update
130 
131  Roadmap roadmap;
132  ModeGraph modeGraph;
133 
134  //planning statistics
135  int numExpands,numRefinementAttempts,numRefinementSuccesses,numExplorationAttempts,
136  numEdgeChecks,numConfigChecks,
137  numUpdatePaths,numUpdatePathsIterations;
138  double timeNearestNeighbors,timeRefine,timeExplore,timeUpdatePaths,timeOverhead;
139 };
140 
141 #endif
void GetCover(const std::vector< int > &path, Subset &cover) const
Computes the cover of the path.
bool CoveragePath(int s, int t, const Subset &cover, std::vector< int > &path, Subset &pathCover)
A search that finds a path subject to a coverage constraint.
Definition: MCRPlanner.cpp:1448
A planner that minimizes the the number of violated constraints using a RRT-like strategy.
Definition: MCRPlanner.h:29
void Expand(Real maxExplanationCost, std::vector< int > &newNodes)
Performs one iteration of planning given a limit on the explanation size.
Definition: MCRPlanner.cpp:975
Vector Config
an alias for Vector
Definition: RobotKinematics3D.h:14
Motion planning configuration space base class. The configuration space implements an interpolation s...
Definition: CSpace.h:34
A sequence of locally planned paths between milestones.
Definition: planning/Path.h:15
void BuildCCGraph(Graph::UndirectedGraph< Subset, int > &G)
Definition: MCRPlanner.cpp:1395
void Plan(int initialLimit, const std::vector< int > &expansionSchedule, std::vector< int > &bestPath, Subset &cover)
Performs bottom-up planning according to a given limit expansion schedule.
Definition: MCRPlanner.cpp:1271
Definition: MCRPlanner.h:36
bool GreedyPath(int s, int t, std::vector< int > &path, Subset &pathCover)
A greedy heuristic that performs smallest cover given predecessor.
Definition: MCRPlanner.cpp:1646
void GetMilestonePath(const std::vector< int > &path, MilestonePath &mpath) const
Returns the MilestonePath.
Definition: MCRPlanner.cpp:1698
A base roadmap planner class.
Definition: MotionPlanner.h:23
A finite subset of numbered items with convenient operators for union, intersection, difference, etc.
Definition: Subset.h:13
void Completion(int s, int node, int t, Subset &pathCover)
Definition: MCRPlanner.cpp:1687
bool updatePathsDynamic
Definition: MCRPlanner.h:127
Real GetLength(const std::vector< int > &path) const
Computes the length of the path.
Definition: MCRPlanner.h:32
bool OptimalPath(int s, int t, std::vector< int > &path, Subset &pathCover)
An optimal search.
Definition: MCRPlanner.cpp:1667
Definition: MCRPlanner.h:48
Definition: MCRPlanner.h:42
void BuildRoadmap(Real maxExplanationCost, RoadmapPlanner &prm)
Outputs the graph with the given explanation limit.
Definition: MCRPlanner.cpp:1360
bool updatePathsComplete
Definition: MCRPlanner.h:124
int updatePathsMax
For complete planning, keep at most this number of covers.
Definition: MCRPlanner.h:129