4 #include "MotionPlanner.h" 6 #include <KrisLibrary/utils/Subset.h> 44 std::vector<int> roadmapNodes;
45 std::vector<Subset> pathCovers;
49 std::vector<std::pair<int,int> > connections;
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);
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);
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);
92 bool CanImproveConnectivity(
const Mode& ma,
const Mode& mb,Real maxExplanationCost);
94 void UpdateMinCost(
Mode& m);
97 bool ExceedsCostLimit(
const Config& q,Real limit,
Subset& violations);
105 Real
GetLength(
const std::vector<int>& path)
const;
113 std::vector<Real> obstacleWeights;
117 Real connectThreshold,expandDistance,goalConnectThreshold;
118 Real goalBiasProbability;
135 int numExpands,numRefinementAttempts,numRefinementSuccesses,numExplorationAttempts,
136 numEdgeChecks,numConfigChecks,
137 numUpdatePaths,numUpdatePathsIterations;
138 double timeNearestNeighbors,timeRefine,timeExplore,timeUpdatePaths,timeOverhead;
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