Klamp't  0.8.1
RealTimeRRTPlanner.h
1 #ifndef REAL_TIME_RRT_PLANNER_H
2 #define REAL_TIME_RRT_PLANNER_H
3 
4 #include "RealTimePlanner.h"
5 #include <KrisLibrary/planning/MotionPlanner.h>
6 
10 {
11 public:
13  virtual ~DynamicRRTPlanner() { }
14  virtual void SetGoal(shared_ptr<PlannerObjectiveBase> newgoal);
15  Vector& MakeState(const Config& q,const Config& dq);
16  Vector& MakeState(const Config& q);
17  RRTPlanner::Node* TryIKExtend(RRTPlanner::Node* node,bool search=true);
18 
19  //perform lazy collision checking of the path up to n
20  //ndelete allows you to check if any nodes are deleted
21  //pass in nodes to check, they will be set to NULL if they lie in the
22  //deleted subtree
23  bool CheckPath(RRTPlanner::Node* n,vector<RRTPlanner::Node*>& ndelete,Timer& timer,Real cutoff);
24  virtual int PlanFrom(ParabolicRamp::DynamicPath& path,Real cutoff);
25  Real EvaluateNodePathCost(RRTPlanner::Node* n);
26 
27  //settings
28  shared_ptr<RampCSpaceAdaptor> stateSpace;
29  Real delta;
30  Real smoothTime;
31  Real ikSolveProbability;
32 
33  //temporary state
34  int iteration;
35  shared_ptr<RRTPlanner> rrt;
36  vector<RRTPlanner::Node*> existingNodes;
37  Vector tempV;
38 };
39 
45 {
46 public:
47  struct NodeData
48  {
49  Real t;
50  Config q,dq;
51 
52  bool reachable;
53  int depth;
54  Real sumPathCost,terminalCost,totalCost;
55  };
56  struct EdgeData
57  {
58  Real cost;
59  shared_ptr<RampEdgeChecker> e;
60  };
61  typedef Graph::TreeNode<NodeData,EdgeData> Node;
62 
64  virtual ~DynamicHybridTreePlanner() { }
65  virtual void SetGoal(shared_ptr<PlannerObjectiveBase> newgoal);
66  Node* AddChild(Node* node,const Config& q);
67  Node* AddChild(Node* node,const ParabolicRamp::ParabolicRampND& ramp);
68  Node* AddChild(Node* node,const ParabolicRamp::DynamicPath& path);
69  Node* AddChild(Node* node,shared_ptr<RampEdgeChecker>& e);
70  //uses a local optimization to extend the tree from the given node.
71  //if search is true, finds a parent node that gives a good fit, otherwise
72  //adds the ik extension as a child of node.
73  Node* TryIKExtend(Node* node,bool search=true);
74  //finds the closest node to q (within the costBranch level set)
75  Node* Closest(const Config& q,Real costBranch=Inf);
76  //extends the tree towards q (within the costBranch level set)
77  Node* ExtendToward(const Config& q,Real costBranch=Inf);
78  //splits an edge p->n in the tree at interpolant u
79  Node* SplitEdge(Node* p,Node* n,Real u);
80 
81  //perform lazy collision checking of the path up to n
82  //split can be set to a pointer to retrieve the last reachable
83  //node.
84  bool CheckPath(Node* n,Timer& timer,Real cutoff,Node** split=NULL);
85  virtual int PlanFrom(ParabolicRamp::DynamicPath& path,Real cutoff);
86 
87  //settings
88  unique_ptr<RampCSpaceAdaptor> stateSpace;
89  Real delta;
90  Real smoothTime;
91  Real ikSolveProbability;
92 
93  //temporary state
94  int iteration;
95  unique_ptr<Node> root;
96  vector<Node*> nodes;
97 };
98 
99 #endif
Dynamic RRT planner – not recently tested.
Definition: RealTimeRRTPlanner.h:9
virtual int PlanFrom(ParabolicRamp::DynamicPath &path, Real cutoff)
Solves for optimal trajectores for a velocity-bounded ND system.
Definition: ParabolicRamp.h:110
A bounded-velocity, bounded-acceleration trajectory consisting of parabolic ramps.
Definition: DynamicPath.h:114
Definition: RealTimeRRTPlanner.h:56
Definition: RealTimeRRTPlanner.h:47
A base class for a motion planner that generates dynamic paths. The output should always respect join...
Definition: RealTimePlanner.h:24
The preferred dynamic sampling-based planner for realtime planning. Will alternate sampling-based pla...
Definition: RealTimeRRTPlanner.h:44