QuotientSpace.h
90 ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override final;
157 void mergeStates(const ompl::base::State *qQ0, const ompl::base::State *qX1, ompl::base::State *qQ1) const;
const ompl::base::StateSpacePtr computeQuotientSpace(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Compute the quotient Q1 / Q0 between two given spaces.
Definition: QuotientSpace.cpp:195
A shared pointer wrapper for ompl::base::Path.
A shared pointer wrapper for ompl::base::SpaceInformation.
void checkSpaceHasFiniteMeasure(const ompl::base::StateSpacePtr space) const
Check if quotient-space is unbounded.
Definition: QuotientSpace.cpp:153
void setChild(QuotientSpace *child_)
Set pointer to less simplified quotient-space.
Definition: QuotientSpace.cpp:1370
void projectQ0(const ompl::base::State *q, ompl::base::State *qQ0) const
Quotient Space Projection Operator onto first component ProjectQ0: Q0 \times X1 \rightarrow Q0.
Definition: QuotientSpace.cpp:1113
unsigned int getLevel() const
Level in abstraction hierarchy of quotient-spaces.
Definition: QuotientSpace.cpp:1380
QuotientSpace * getParent() const
Parent is a more simplified quotient-space (higher in abstraction hierarchy)
Definition: QuotientSpace.cpp:1360
unsigned int id_
Identity of space (to keep track of number of quotient-spaces created)
Definition: QuotientSpace.h:204
A shared pointer wrapper for ompl::base::OptimizationObjective.
void projectX1(const ompl::base::State *q, ompl::base::State *qX1) const
Quotient Space Projection Operator onto second component ProjectX1: Q0 \times X1 \rightarrow X1.
Definition: QuotientSpace.cpp:982
const ompl::base::SpaceInformationPtr & getX1() const
Get SpaceInformationPtr for X1 (Note: X1 is the second component of Q1 = Q0 x X1)
Definition: QuotientSpace.cpp:1295
unsigned int getTotalNumberOfFeasibleSamples() const
Number of feasible samples drawn on space Q1.
Definition: QuotientSpace.cpp:1355
QuotientSpace * getChild() const
Child is a less simplified quotient-space (lower in abstraction hierarchy)
Definition: QuotientSpace.cpp:1365
unsigned int getTotalNumberOfSamples() const
Number of samples drawn on space Q1.
Definition: QuotientSpace.cpp:1350
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
const ompl::base::SpaceInformationPtr & getQ0() const
Get SpaceInformationPtr for Q0 (Note: Q0 is the first component of Q1 = Q0 x X1)
Definition: QuotientSpace.cpp:1305
friend std::ostream & operator<<(std::ostream &out, const QuotientSpace &qtnt)
Write class to stream (use as std::cout << *this << std::endl) Actual implementation is in void print...
Definition: QuotientSpace.cpp:1571
QuotientSpaceType identifyQuotientSpaceType(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Identify the type of the quotient Q1 / Q0.
Definition: QuotientSpace.cpp:348
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: QuotientSpace.cpp:139
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
virtual void setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planner. The problem needs to be set before calling solve()....
Definition: QuotientSpace.cpp:176
A shared pointer wrapper for ompl::base::ProblemDefinition.
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override final
solve disabled (use MultiQuotient::solve) final prevents subclasses to override
Definition: QuotientSpace.cpp:170
void mergeStates(const ompl::base::State *qQ0, const ompl::base::State *qX1, ompl::base::State *qQ1) const
Merge a state from Q0 and X1 into a state on Q1 (concatenate)
Definition: QuotientSpace.cpp:698
const ompl::base::SpaceInformationPtr & getQ1() const
Get SpaceInformationPtr for Q1 (Note: Q1 is the product space Q1 = Q0 x X1)
Definition: QuotientSpace.cpp:1300
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::ValidStateSampler.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: QuotientSpace.cpp:132
QuotientSpace(const ompl::base::SpaceInformationPtr &si, QuotientSpace *parent_=nullptr)
Quotient Space contains three OMPL spaces, which we call Q1, Q0 and X1.
Definition: QuotientSpace.cpp:50
virtual void print(std::ostream &out) const
Internal function implementing actual printing to stream.
Definition: QuotientSpace.cpp:1445
void setParent(QuotientSpace *parent_)
Set pointer to more simplified quotient-space.
Definition: QuotientSpace.cpp:1375