Loading...
Searching...
No Matches
BundleSpaceGraph.cpp
79BundleSpaceGraph::BundleSpaceGraph(const ompl::base::SpaceInformationPtr &si, BundleSpace *parent_) : BaseT(si, parent_)
102 Planner::declareParam<double>("range", this, &BundleSpaceGraph::setRange, &BundleSpaceGraph::getRange, "0.:1.:"
105 Planner::declareParam<double>("goal_bias", this, &BundleSpaceGraph::setGoalBias, &BundleSpaceGraph::getGoalBias,
123 OMPL_DEBUG("Range distance graph sampling: %f (max extent %f)", maxDistance_, getBundle()->getMaximumExtent());
172 base::PathPtr basePath = static_cast<BundleSpaceGraph *>(getChild())->getSolutionPathByReference();
256BundleSpaceGraph::Configuration::Configuration(const ompl::base::SpaceInformationPtr &si) : state(si->allocState())
420void BundleSpaceGraph::interpolate(const Configuration *a, const Configuration *b, Configuration *dest) const
425Configuration *BundleSpaceGraph::steerTowards(const Configuration *from, const Configuration *to)
437Configuration *BundleSpaceGraph::steerTowards_Range(const Configuration *from, Configuration *to)
453Configuration *BundleSpaceGraph::extendGraphTowards_Range(const Configuration *from, Configuration *to)
567const std::pair<BundleSpaceGraph::Edge, bool> BundleSpaceGraph::addEdge(const Vertex a, const Vertex b)
569 base::Cost weight = getOptimizationObjectivePtr()->motionCost(graph_[a]->state, graph_[b]->state);
685ompl::base::PathPtr BundleSpaceGraph::getPath(const Vertex &start, const Vertex &goal, Graph &graph)
688 auto weight = boost::make_transform_value_property_map(std::mem_fn(&EdgeInternalState::getCost),
693 boost::astar_search(graph, start, [this, goal](const Vertex v) { return costHeuristic(v, goal); },
788void BundleSpaceGraph::getPlannerDataGraph(ompl::base::PlannerData &data, const Graph &graph, const Vertex vStart) const
846 OMPL_DEBUG("Graph (level %d) has %d/%d vertices/edges", getLevel(), boost::num_vertices(graph_),
A nearest neighbors datastructure that uses linear search. The linear search is done over sqrt(n) ele...
Definition NearestNeighborsSqrtApprox.h:58
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
A shared pointer wrapper for ompl::base::Path.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition PlannerData.h:175
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition PlannerData.cpp:413
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition PlannerData.cpp:422
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Definition PlannerData.cpp:432
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
Definition PlannerData.cpp:391
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition Planner.cpp:228
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again.
Definition Planner.cpp:180
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition Planner.cpp:265
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::control::SpaceInformation.
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
Definition PathGeometric.h:242
A configuration in Bundle-space.
Definition BundleSpaceGraph.h:96
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
Definition BundleSpaceGraph.h:140
An edge in Bundle-space.
Definition BundleSpaceGraph.h:161
bool connect(const Configuration *from, const Configuration *to)
Try to connect configuration a to configuration b using the current metric.
Definition BundleSpaceGraph.cpp:475
BundleSpaceGraphSamplerPtr graphSampler_
Pointer to strategy to sample from graph.
Definition BundleSpaceGraph.h:412
bool getSolution(ompl::base::PathPtr &solution) override
Return best solution.
Definition BundleSpaceGraph.cpp:618
BundleSpaceImportancePtr importanceCalculator_
Pointer to strategy to compute importance of this bundle space (which is used to decide which bundle ...
Definition BundleSpaceGraph.h:409
PathRestrictionPtr pathRestriction_
Pointer to current path restriction (the set of points which project onto the best cost path on the b...
Definition BundleSpaceGraph.h:418
Configuration * extendGraphTowards_Range(const Configuration *from, Configuration *to)
Steer system at Configuration *from to Configuration *to while system is valid, stopping if maxDistan...
Definition BundleSpaceGraph.cpp:453
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition BundleSpaceGraph.cpp:187
virtual Vertex addConfiguration(Configuration *q)
Add configuration to graph. Return its vertex in boost graph.
Definition BundleSpaceGraph.cpp:354
RoadmapNeighborsPtr nearestDatastructure_
Nearest neighbor structure for Bundle space configurations.
Definition BundleSpaceGraph.h:386
void writeToGraphviz(std::string filename) const
Write class to graphviz.
Definition BundleSpaceGraph.cpp:760
double getImportance() const override
Importance of Bundle-space depending on number of vertices in Bundle-graph.
Definition BundleSpaceGraph.cpp:294
Configuration * steerTowards_Range(const Configuration *from, Configuration *to)
Steer system at Configuration *from to Configuration *to, stopping if maxdistance is reached.
Definition BundleSpaceGraph.cpp:437
virtual const std::pair< Edge, bool > addEdge(const Vertex a, const Vertex b)
Add edge between Vertex a and Vertex b to graph.
Definition BundleSpaceGraph.cpp:567
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal)
Shortest path on Bundle-graph.
Definition BundleSpaceGraph.cpp:680
virtual void setStartIndex(Vertex)
Set vertex representing the start.
Definition BundleSpaceGraph.cpp:608
Configuration * steerTowards(const Configuration *from, const Configuration *to)
Steer system at Configuration *from to Configuration *to.
Definition BundleSpaceGraph.cpp:425
std::vector< Configuration * > goalConfigurations_
List of configurations that satisfy the goal condition.
Definition BundleSpaceGraph.h:433
virtual void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
Definition BundleSpaceGraph.cpp:299
std::vector< Configuration * > startConfigurations_
List of configurations that satisfy the start condition.
Definition BundleSpaceGraph.h:430
virtual Vertex getStartIndex() const
Get vertex representing the start.
Definition BundleSpaceGraph.cpp:576
virtual void interpolate(const Configuration *a, const Configuration *b, Configuration *dest) const
Interpolate from configuration a to configuration b and store results in dest.
Definition BundleSpaceGraph.cpp:420
virtual Configuration * addBundleConfiguration(base::State *)
Add ompl::base::State to graph. Return its configuration.
Definition BundleSpaceGraph.cpp:342
virtual void printConfiguration(const Configuration *) const
Print configuration to std::cout.
Definition BundleSpaceGraph.cpp:783
virtual Graph & getGraphNonConst()
Get underlying boost graph representation (non const)
Definition BundleSpaceGraph.cpp:382
virtual void addBundleEdge(const Configuration *a, const Configuration *b)
Add edge between configuration a and configuration b to graph.
Definition BundleSpaceGraph.cpp:349
virtual double distance(const Configuration *a, const Configuration *b) const
Distance between two configurations using the current metric.
Definition BundleSpaceGraph.cpp:410
virtual bool checkMotion(const Configuration *a, const Configuration *b) const
Check if we can move from configuration a to configuration b using the current metric.
Definition BundleSpaceGraph.cpp:415
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, Configuration *, EdgeInternalState, GraphMetaData > Graph
A Bundle-graph structure using boost::adjacency_list bundles.
Definition BundleSpaceGraph.h:196
virtual const Graph & getGraph() const
Get underlying boost graph representation.
Definition BundleSpaceGraph.cpp:377
void addGoalConfiguration(Configuration *x)
Add configuration to graph as goal vertex.
Definition BundleSpaceGraph.cpp:581
void print(std::ostream &out) const override
Print class to ostream.
Definition BundleSpaceGraph.cpp:775
bool findSection() override
Call algorithm to solve the find section problem.
Definition BundleSpaceGraph.cpp:168
void getPlannerData(ompl::base::PlannerData &data) const override
Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (...
Definition BundleSpaceGraph.cpp:844
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition BundleSpaceGraph.cpp:116
double graphLength_
Length of graph (useful for determing importance of Bundle-space.
Definition BundleSpaceGraph.h:395
virtual Vertex getGoalIndex() const
Get vertex representing the goal.
Definition BundleSpaceGraph.cpp:590
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition BundleSpace.cpp:164
ProjectionPtr getProjection() const
Get ProjectionPtr from Bundle to Base.
Definition BundleSpace.cpp:226
BundleSpacePropagatorPtr propagator_
Propagator (steering or interpolation) on bundle space. Note: currently just a stub for base::StatePr...
Definition BundleSpace.h:275
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition BundleSpace.cpp:134
BundleSpace * getChild() const
Return k-1 th bundle space (locally the base space)
Definition BundleSpace.cpp:389
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
Definition BundleSpace.cpp:323
virtual void print(std::ostream &out) const
Internal function implementing actual printing to stream.
Definition BundleSpace.cpp:469
An annotated vertex, adding information about its level in the multilevel hierarchy....
Definition PlannerDataVertexAnnotated.h:56
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition SelfConfig.cpp:225
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Definition MultiLevelPlanarManipulatorDemo.cpp:68