6 #include <KrisLibrary/graph/UndirectedGraph.h> 7 #include <KrisLibrary/graph/ConnectedComponents.h> 31 virtual void Cleanup();
32 virtual void Init(
const Config& qStart,
const Config& qGoal);
33 virtual bool Extend();
34 virtual Node* PickConnection(
SBLTree* t,
const Config& x) {
return t->FindClosest(x); }
36 bool IsDone()
const {
return !outputPath.empty(); }
40 bool CheckPath(Node* nStart,Node* nGoal);
43 Real maxExtendDistance;
45 Real edgeConnectionThreshold;
49 std::list<EdgeInfo> outputPath;
61 virtual void Cleanup();
62 virtual void Init(
const Config& qStart,
const Config& qGoal);
63 virtual bool Extend();
65 void RandomizeSubset();
67 int numItersPerRandomize;
88 virtual void Cleanup();
89 int AddSeed(
const Config& q);
90 std::pair<int,int> Expand();
91 int ExpandTree(
int t);
93 void AddRoadmapEdgesIfBelowThreshold(Real distanceThreshold);
94 void AddRoadmapEdge(
int i,
int j) { roadmap.AddEdge(i,j); }
95 bool IsEdgeConnected(
int i,
int j)
const;
96 bool IsSeedFullyConnected(
int i)
const;
97 bool AreSeedsConnected(
int i,
int j)
const {
return ccs.GetComponent(i) == ccs.GetComponent(j); }
100 virtual std::pair<int,Node*> PickConnection(
int t,Node* n);
102 int PickRandomAdjacentTree(
int t);
103 int PickClosestAdjacentTree(
int t,
const Config& x);
104 Node* GetClosestNode(
int t,
const Config& x) {
return roadmap.nodes[t]->FindClosest(x); }
105 Node* PickNode(
int t) {
return roadmap.nodes[t]->PickExpand(); }
108 Real maxExtendDistance;
110 Real defaultPPickClosestTree,defaultPPickClosestNode;
Definition: ConnectedComponents.h:9
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
The SBL motion planner.
Definition: SBL.h:23
A tree graph structure, represented directly at the node level.
Definition: Tree.h:25
A tree of configurations to be used in the SBL motion planner.
Definition: SBLTree.h:16
An SBL planner whose trees use grids for point location.
Definition: SBL.h:57
A probabilistic roadmap of trees, where SBL is used for local planning.
Definition: SBL.h:79