1 #ifndef MULTI_MODAL_PLANNER_H 2 #define MULTI_MODAL_PLANNER_H 4 #include "MultiModalCSpace.h" 5 #include "AnyMotionPlanner.h" 7 #include <KrisLibrary/graph/ShortestPaths.h> 8 #include <KrisLibrary/structs/IndexedPriorityQueue.h> 16 void SetStart(
const Config& q,
int mode);
17 void SetGoal(
const Config& q,
int mode);
18 void SetStartMode(
int mode);
19 void SetGoalMode(
int mode);
21 void ExpandMode(
int mode,
int numSamples=-1);
22 void ExpandTrans(
int m1,
int m2,
int numSamples=-1);
23 void AddTransition(
int m1,
int m2,
const Config& q);
24 bool IsStartAndGoalConnected()
const;
28 std::shared_ptr<MotionPlannerInterface> planner;
35 std::vector<Config> transitions;
37 std::vector<int> prevRoadmapIndices;
39 std::vector<int> nextRoadmapIndices;
45 if(m1 < t.m1)
return true;
46 else if(m1 > t.m1)
return false;
47 if(m2 < t.m2)
return true;
48 else if(m2 > t.m2)
return false;
49 return count < t.count;
57 void ConnectTransitions(
int mode,
int t1,
int t2);
59 void GetTransitionNodes(
int mode,std::vector<TransitionIndex>& transitions,std::vector<int>& roadmapIndices)
const;
63 PlanningGraph planningGraph;
64 int startMode,startRoadmapIndex;
65 int goalMode,goalRoadmapIndex;
66 int numExpandModeSamples,numExpandTransSamples;
72 std::map<TransitionIndex,int> ccIndex;
83 virtual void OnDistanceUpdate(
int n);
92 virtual double EdgeLength(
int s,
int t);
94 virtual double Heuristic(
int mode);
101 void SampleMore(
int n);
102 void ExpandAdjacent(
int n);
104 void PushFringe(
int i);
105 void UpdateFringe(
int i);
107 void RefreshFringe();
108 double Priority(
int n);
109 bool GoalConnected()
const;
110 double PathDistance(
int n)
const;
112 void UpdateEdgeLength(
int s,
int t);
115 void UpdatePriority(
int n);
122 ShortestPathProblem spp;
123 SearchGraph searchGraph;
124 std::vector<bool> reachedModes;
125 std::vector<bool> outputModes;
127 double transSampleWeight;
140 virtual int PickRefineCount();
145 int numRefineSamplesPerMode;
146 int numRefineSamplesPerOldMode;
147 int numRefineSamplesConstant;
151 std::vector<bool> lastRefineSet;
152 int remainingRefineSamples;
153 int expandPhaseCount,expandStepCount;
154 int refinePhaseCount,refineStepCount;
Definition: MultiModalPlanner.h:130
Definition: ConnectedComponents.h:9
Vector Config
an alias for Vector
Definition: RobotKinematics3D.h:14
Definition: MultiModalPlanner.h:32
A motion planner creator.
Definition: AnyMotionPlanner.h:234
Definition: MultiModalPlanner.h:11
Definition: MultiModalPlanner.h:42
Single-source shortest paths (Dijkstra's algorithm)
Definition: ShortestPaths.h:39
Definition: MultiModalPlanner.h:26
Definition: MultiModalCSpace.h:37
Definition: MultiModalPlanner.h:80
Definition: MultiModalPlanner.h:89