KrisLibrary  1.0.0
AOMetaPlanner.h
1 #ifndef PLANNING_AO_META_PLANNER_H
2 #define PLANNING_AO_META_PLANNER_H
3 
4 #include "KinodynamicMotionPlanner.h"
5 #include "Objective.h"
6 
16 class CostSpaceRRTPlanner : public LazyRRTKinodynamicPlanner
17 {
18 public:
19  typedef Real (*HeuristicFn) (const Config& x);
20 
21  CostSpaceRRTPlanner(const std::shared_ptr<KinodynamicSpace>& baseSpace,const std::shared_ptr<ObjectiveFunctionalBase>& objective,Real costMax=Inf);
22  virtual ~CostSpaceRRTPlanner() {}
23  void EnableCostSpaceBias(bool enabled);
24  void SetCostMax(Real cmax);
25  void SetCostDistanceWeight(Real weight);
28  void SetHeuristic(HeuristicFn f);
29  virtual void Init(const State& xinit,CSet* goalSet);
30  virtual bool Plan(int maxIters);
31  virtual bool Done() const;
32  virtual bool GetPath(KinodynamicMilestonePath& path);
33  virtual void PickDestination(State& xdest);
34  virtual bool FilterExtension(Node* n,const KinodynamicMilestonePath& path);
35  virtual RRTKinodynamicPlanner::Node* ExtendToward(const State& xdest);
36  virtual void OnNewBestPath();
37  virtual void GetStats(PropertyMap& stats) const;
38  void PruneTree();
39  Real PathCost() const;
40  Real TerminalCost(const State& xc);
41 
42  std::shared_ptr<KinodynamicSpace> baseSpace;
43  std::shared_ptr<KinodynamicSpace> costSpace;
44  std::shared_ptr<ObjectiveFunctionalBase> objective;
45  std::shared_ptr<CSet> costGoalSet;
46  HeuristicFn heuristic;
47  Real costSpaceDistanceWeight;
48  bool lazy;
49 
50  //keeps track of the best path and cost
51  KinodynamicMilestonePath bestPath;
52  Real bestPathCost;
53 
54  //temp: keeps track of how much of the tree is suboptimal
55  int prunableNodeSampleCount;
56  int nodeSampleCount;
57 
58  //keeps track of some statistics
59  int numGoalsSampled,numPrunedNodes;
60 };
61 
71 class CostSpaceESTPlanner : public ESTKinodynamicPlanner
72 {
73 public:
74  typedef Real (*HeuristicFn) (const Config& x);
75 
76  CostSpaceESTPlanner(const std::shared_ptr<KinodynamicSpace>& baseSpace,const std::shared_ptr<ObjectiveFunctionalBase>& objective,Real costMax=Inf);
77  virtual ~CostSpaceESTPlanner() {}
78  void EnableCostSpaceBias(bool enabled);
79  void SetCostMax(Real cmax);
80  void SetDensityEstimatorResolution(Real res);
81  void SetDensityEstimatorResolution(const Vector& res);
84  void SetHeuristic(HeuristicFn f);
85  virtual void Init(const State& xinit,CSet* goalSet);
86  virtual bool Plan(int maxIters);
87  virtual bool GetPath(KinodynamicMilestonePath& path);
88  virtual bool FilterExtension(Node* n,const KinodynamicMilestonePath& path);
89  virtual void OnNewBestPath();
90  virtual void GetStats(PropertyMap& stats) const;
91  void PruneTree();
92  Real PathCost() const;
93  Real TerminalCost(const State& xc);
94 
95  std::shared_ptr<KinodynamicSpace> baseSpace;
96  std::shared_ptr<KinodynamicSpace> costSpace;
97  std::shared_ptr<ObjectiveFunctionalBase> objective;
98  std::shared_ptr<CSet> costGoalSet;
99  HeuristicFn heuristic;
100  Real costSpaceResolution;
101 
102  //keeps track of the best path + cost
103  KinodynamicMilestonePath bestPath;
104  Real bestPathCost;
105 
106  //temp: keeps track of how much of the tree is suboptimal
107  int prunableNodeSampleCount;
108  int nodeSampleCount;
109 
110  //keeps track of some statistics
111  int numGoalsSampled,numPrunedNodes;
112 };
113 
114 
115 
116 #endif
CostSpaceESTPlanner
An optimizing planner that progressively produces better paths using state-cost space. Plan() returns true only if a better path was found in the last maxIters iterations. Done() and GetPath() return true if there ANY solution has been found.
Definition: AOMetaPlanner.h:71
CostSpaceRRTPlanner::SetHeuristic
void SetHeuristic(HeuristicFn f)
Definition: AOMetaPlanner.cpp:117
Config
Vector Config
an alias for Vector
Definition: RobotKinematics3D.h:14
KinodynamicMilestonePath
Stores a kinodynamic path with piecewise constant controls.
Definition: KinodynamicPath.h:23
LazyRRTKinodynamicPlanner
A lazy version of kinodynamic RRT.
Definition: KinodynamicMotionPlanner.h:167
CostSpaceRRTPlanner::FilterExtension
virtual bool FilterExtension(Node *n, const KinodynamicMilestonePath &path)
Subclasses can overload this to eliminate certain extensions of the tree.
Definition: AOMetaPlanner.cpp:353
Graph::TreeNode
A tree graph structure, represented directly at the node level.
Definition: Tree.h:25
CostSpaceRRTPlanner
An optimizing planner that progressively produces better paths using state-cost space. Plan() returns true only if a better path was found in the last maxIters iterations. Done() and GetPath() return true if there ANY solution has been found.
Definition: AOMetaPlanner.h:16
ESTKinodynamicPlanner
The EST planner for kinodynamic systems.
Definition: KinodynamicMotionPlanner.h:129
PropertyMap
A simple map from keys to values.
Definition: PropertyMap.h:27
CSet
A subset of a CSpace, which establishes a constraint for a configuration to be feasible.
Definition: CSet.h:20
Generated by   doxygen 1.8.11