KrisLibrary
1.0.0
|
Data structure for a kinodynamic planning tree. More...
#include <KinodynamicMotionPlanner.h>
Classes | |
struct | EdgeData |
Public Types | |
typedef Graph::TreeNode< State, EdgeData > | Node |
Public Member Functions | |
KinodynamicTree (KinodynamicSpace *s) | |
void | Init (const State &initialState) |
void | EnablePointLocation (const char *type=NULL) |
void | Clear () |
Node * | AddMilestone (Node *parent, const ControlInput &u) |
Node * | AddMilestone (Node *parent, const ControlInput &u, const InterpolatorPtr &path, const EdgePlannerPtr &e) |
Node * | AddMilestone (Node *parent, KinodynamicMilestonePath &path, const EdgePlannerPtr &e=NULL) |
void | AddPath (Node *n0, const KinodynamicMilestonePath &path, std::vector< Node * > &res) |
void | Reroot (Node *n) |
Node * | PickRandom () |
Node * | FindClosest (const State &x) |
void | DeleteSubTree (Node *n, bool rebuild=true) |
void | RebuildPointLocation () |
Static Public Member Functions | |
static void | GetPath (Node *start, Node *goal, KinodynamicMilestonePath &path) |
Public Attributes | |
KinodynamicSpace * | space |
Node * | root |
std::shared_ptr< PointLocationBase > | pointLocation |
If point location is enabled, this will contain a point location data structure. | |
std::vector< Node * > | index |
std::vector< Vector > | pointRefs |
Data structure for a kinodynamic planning tree.
The planning tree has nodes that are states and edges that store control inputs, traces, and local planners. Specifically, the edge x1->x2 stores the input u s.t. x2=f(x1,u), the trace from x1->x2, and the edge planner for that trace. This data can be retrieved using x2->getEdgeFromParent().
Potential improvement can speed up point location using Kd trees.
void KinodynamicTree::DeleteSubTree | ( | Node * | n, |
bool | rebuild = true |
||
) |
Deletes n and its subtree. If point location is not enabled or rebuild=true, cost is O(k) where k is the size of the subtree. Otherwise, cost is O(N) where N is the number of nodes! If you plan to make several deletions, delete the subtrees with rebuild=false, then call RebuildPointLocation before calling FindClosest() or PickRandom() again.
References KinodynamicMilestonePath::MakeEdges(), Math::RandBool(), and Math::WeightedSample().