QuotientSpace.cpp
50 ompl::geometric::QuotientSpace::QuotientSpace(const base::SpaceInformationPtr &si, QuotientSpace *parent_)
65 OMPL_DEVMSG1("ATOMIC dimension: %d measure: %f", Q1_space->getDimension(), Q1_space->getMeasure());
86 if ((Q0_space->getMeasure() <= 0) || (X1_space->getMeasure() <= 0) || (Q1_space->getMeasure() <= 0))
153 void ompl::geometric::QuotientSpace::checkSpaceHasFiniteMeasure(const base::StateSpacePtr space) const
170 ompl::base::PlannerStatus ompl::geometric::QuotientSpace::solve(const base::PlannerTerminationCondition &ptc)
173 throw ompl::Exception("A Quotient-Space cannot be solved alone. Use class MultiQuotient to solve Quotient-Spaces.");
176 void ompl::geometric::QuotientSpace::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
195 const ompl::base::StateSpacePtr ompl::geometric::QuotientSpace::computeQuotientSpace(const base::StateSpacePtr Q1,
229 base::RealVectorBounds Q1_bounds = std::static_pointer_cast<base::RealVectorStateSpace>(Q1)->getBounds();
278 const base::RealVectorStateSpace *Q1_RN = Q1_decomposed.at(1)->as<base::RealVectorStateSpace>();
298 const base::RealVectorStateSpace *Q1_RN = Q1_decomposed.at(1)->as<base::RealVectorStateSpace>();
348 ompl::geometric::QuotientSpace::identifyQuotientSpaceType(const base::StateSpacePtr Q1, const base::StateSpacePtr Q0)
698 void ompl::geometric::QuotientSpace::mergeStates(const base::State *qQ0, const base::State *qX1, base::State *qQ1) const
720 const base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
721 const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
736 const base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
749 const base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
769 const base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
796 const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
816 const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
838 const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
863 const base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
891 const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
922 const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
951 const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
988 const base::RealVectorStateSpace::StateType *sQ1 = q->as<base::RealVectorStateSpace::StateType>();
1067 const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
1096 const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
1130 const base::RealVectorStateSpace::StateType *sQ1 = q->as<base::RealVectorStateSpace::StateType>();
1390 ompl::geometric::QuotientSpace::QuotientSpaceType ompl::geometric::QuotientSpace::getType() const
1395 ompl::base::OptimizationObjectivePtr ompl::geometric::QuotientSpace::getOptimizationObjectivePtr() const
1508 out << "R^" << Q0_dimension_ << " | Q" << level_ + 1 << ": R^" << Q1_dimension_ << " | X" << level_ + 1
1524 out << "SE(2) | Q" << level_ + 1 << ": SE(2)xR^" << X1_dimension_ << " | X" << level_ + 1 << ": R^"
1530 out << "SO(2) | Q" << level_ + 1 << ": SO(2)xR^" << X1_dimension_ << " | X" << level_ + 1 << ": R^"
1536 out << "SE(3) | Q" << level_ + 1 << ": SE(3)xR^" << X1_dimension_ << " | X" << level_ + 1 << ": R^"
1542 out << "SE(2)xR^" << Q0_dimension_ - 3 << " | Q" << level_ + 1 << ": SE(2)xR^" << Q1_dimension_ - 3
1548 out << "SO(2)xR^" << Q0_dimension_ - 1 << " | Q" << level_ + 1 << ": SO(2)xR^" << Q1_dimension_ - 1
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
void setXYZ(double x, double y, double z)
Set the X, Y and Z components of the state.
Definition: SE3StateSpace.h:107
A state space representing SO(2). The distance function and interpolation take into account angle wra...
Definition: SO2StateSpace.h:64
A shared pointer wrapper for ompl::base::SpaceInformation.
double getYaw() const
Get the yaw component of the state. This is the rotation in plane, with respect to the Z axis.
Definition: SE2StateSpace.h:73
A space to allow the composition of state spaces.
Definition: StateSpace.h:574
void setYaw(double yaw)
Set the yaw component of the state. This is the rotation in plane, with respect to the Z axis.
Definition: SE2StateSpace.h:100
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
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
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
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
Definition: RealVectorBounds.cpp:47
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:91
A state space representing SO(3). The internal representation is done with quaternions....
Definition: SO3StateSpace.h:83
A shared pointer wrapper for ompl::base::OptimizationObjective.
void setLow(double value)
Set the lower bound in each dimension to a specific value.
Definition: RealVectorBounds.cpp:42
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 RealVectorBounds & getBounds() const
Get the bounds for this state space.
Definition: RealVectorStateSpace.h:130
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
A state space representing Rn. The distance function is the L2 norm.
Definition: RealVectorStateSpace.h:74
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
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
Definition: SE3StateSpace.h:54
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
QuotientSpaceType identifyQuotientSpaceType(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Identify the type of the quotient Q1 / Q0.
Definition: QuotientSpace.cpp:348
const SO3StateSpace::StateType & rotation() const
Get the rotation component of the state.
Definition: SE3StateSpace.h:77
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: QuotientSpace.cpp:139
The definition of a state in Rn
Definition: RealVectorStateSpace.h:78
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
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
Definition: RealVectorStateSpace.cpp:138
The definition of a state in SO(2)
Definition: SO2StateSpace.h:68
void setXY(double x, double y)
Set the X and Y components of the state.
Definition: SE2StateSpace.h:91
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
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: StateSpace.h:590
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.
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
Definition: StateSpace.cpp:978
A shared pointer wrapper for ompl::base::StateSampler.
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
The lower and upper bounds for an Rn space.
Definition: RealVectorBounds.h:48