KrisLibrary  1.0.0
SBLTree.h
1 #ifndef ROBOTICS_SBL_TREE_H
2 #define ROBOTICS_SBL_TREE_H
3 
4 #include <KrisLibrary/graph/Tree.h>
5 #include <KrisLibrary/geometry/GridSubdivision.h>
6 #include <KrisLibrary/utils/ArrayMapping.h>
7 #include <list>
8 #include "CSpace.h"
9 #include "EdgePlanner.h"
10 #include "DensityEstimator.h"
11 #include "Path.h"
12 
16 class SBLTree
17 {
18 public:
20  struct EdgeInfo
21  {
22  Node *s,*t;
23  EdgePlannerPtr e;
24  bool reversed;
25  };
26 
27  SBLTree(CSpace*);
28  virtual ~SBLTree();
29  virtual void Cleanup();
30  virtual void Init(const Config& qStart);
31  virtual Node* Extend(Real maxDistance,int maxIters);
32 
33  virtual void AddMilestone(Node* n) {}
34  virtual void RemoveMilestone(Node* n) {}
35  virtual Node* PickExpand();
36 
37  //helpers
38  Node* AddMilestone(const Config& q) { Node* n=new Node(q); AddMilestone(n); return n; }
39  bool HasNode(Node* n) const;
40  Node* AddChild(Node* n,const Config& x);
41  Node* FindClosest(const Config& x);
42  void AdjustMilestone(Node* n,const Config& newConfig);
43  void DeleteSubtree(Node* n);
44 
45  //collision testing along path from ts->ns->ng->tg
46  static bool CheckPath(SBLTree* ts, Node* ns,SBLTree* tg,Node* ng,std::list<EdgeInfo>& outputPath);
47  //collision testing along path from ns -> ng in a single tree
48  static bool CheckPath(SBLTree* t,Node* ns,Node* ng,MilestonePath& outputPath);
49 
50  CSpace* space;
51  Node *root;
52 };
53 
57 class SBLTreeWithIndex : public SBLTree
58 {
59  public:
61  virtual void Cleanup();
62  virtual void AddMilestone(Node* n);
63  virtual void RemoveMilestone(Node* n);
64  virtual Node* PickExpand() { return PickRandom(); }
65  Node* PickRandom() const;
66 
67  std::vector<Node*> index;
68 };
69 
74 class SBLTreeWithGrid : public SBLTree
75 {
76 public:
78  virtual void Init(const Config& qStart);
79  virtual void Cleanup();
82  void InitDefaultGrid(int numDims,Real h);
84  void RandomizeSubset();
85 
86  virtual void AddMilestone(Node* n);
87  virtual void RemoveMilestone(Node* n);
88  virtual Node* PickExpand();
89 
90  Node* FindNearby(const Config& x);
91 
92  Real gridDivision;
94 };
95 
96 
97 #endif
98 
Definition: SBLTree.h:20
An SBLTree with a node index.
Definition: SBLTree.h:57
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:39
A sequence of locally planned paths between milestones.
Definition: planning/Path.h:18
An SBL motion planner that uses a SBLSubdivision to pick the next node to expand, and nodes to connec...
Definition: SBLTree.h:74
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
A grid-based (n-d projected histogram) density estimator.
Definition: DensityEstimator.h:43