KrisLibrary  1.0.0
SBL.h
1 #ifndef ROBOTICS_SBL_H
2 #define ROBOTICS_SBL_H
3 
4 #include "SBLTree.h"
5 #include "Path.h"
6 #include <KrisLibrary/graph/UndirectedGraph.h>
7 #include <KrisLibrary/graph/ConnectedComponents.h>
8 
24 {
25 public:
26  typedef SBLTree::Node Node;
28 
30  virtual ~SBLPlanner();
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); }
35 
36  bool IsDone() const { return !outputPath.empty(); }
37  void CreatePath(MilestonePath& path) const;
38 
39  //helper, no need to call this
40  bool CheckPath(Node* nStart,Node* nGoal); //path from start->ns->ng->goal
41 
42  CSpace* space;
43  Real maxExtendDistance;
44  int maxExtendIters;
45  Real edgeConnectionThreshold;
46 
47  int numIters;
48  SBLTree *tStart, *tGoal;
49  std::list<EdgeInfo> outputPath;
50 };
51 
58 {
59  public:
61  virtual void Cleanup();
62  virtual void Init(const Config& qStart,const Config& qGoal);
63  virtual bool Extend();
64  virtual Node* PickConnection(SBLTree* t,const Config& x);
65  void RandomizeSubset();
66 
67  int numItersPerRandomize;
68  Real gridDivision;
69 };
70 
79 class SBLPRT
80 {
81  public:
82  typedef SBLTree::Node Node;
85 
86  SBLPRT(CSpace* s);
87  virtual ~SBLPRT();
88  virtual void Cleanup();
89  int AddSeed(const Config& q);
90  std::pair<int,int> Expand(); //picks a random tree, returns connection if made
91  int ExpandTree(int t); //expands tree t, calls PickConnection to attempt a connection, and returns the connected tree if successful, or -1 on failure
92 
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); }
98  void CreatePath(int i,int j,MilestonePath& path);
99 
100  virtual std::pair<int,Node*> PickConnection(int t,Node* n);
101  //helpers for picking a tree/node to connect
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(); }
106 
107  CSpace* space;
108  Real maxExtendDistance;
109  int maxExtendIters;
110  Real defaultPPickClosestTree,defaultPPickClosestNode;
111 
112  int numIters;
113  Roadmap roadmap; //the roadmap nodes
114  Graph::ConnectedComponents ccs; //connected components of the roadmap
115 };
116 
117 #endif
118 
Definition: SBLTree.h:20
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