39#include <ompl/multilevel/datastructures/Projection.h>
40#include <ompl/util/Exception.h>
45Projection::Projection(ompl::base::StateSpacePtr bundleSpace, ompl::base::StateSpacePtr baseSpace)
46 : bundleSpace_(bundleSpace), baseSpace_(baseSpace)
50bool Projection::isAdmissible()
const
56unsigned int Projection::getBaseDimension()
const
60 return baseSpace_->getDimension();
68unsigned int Projection::getDimension()
const
70 return bundleSpace_->getDimension();
73unsigned int Projection::getCoDimension()
const
75 return getDimension() - getBaseDimension();
78ompl::base::StateSpacePtr Projection::getBundle()
const
83ompl::base::StateSpacePtr Projection::getBase()
const
88bool Projection::isFibered()
const
106 int type = space->getType();
109 int N = space->getDimension();
111 tstr += std::to_string(N);
133 else if (space->isCompound())
136 const std::vector<base::StateSpacePtr> space_decomposed = space_compound->
getSubspaces();
138 for (
unsigned int k = 0; k < space_decomposed.size(); k++)
140 base::StateSpacePtr s0 = space_decomposed.at(k);
142 if (k < space_decomposed.size() - 1)
153std::string Projection::getTypeAsString()
const
157 std::string tstr = getBundleTypeAsString() +
" -> " + getBaseTypeAsString();
170 return getBundleTypeAsString();
174std::string Projection::getBaseTypeAsString()
const
182std::string Projection::getBundleTypeAsString()
const
189 out << getTypeAsString() << std::endl;
196 std::ostream &
operator<<(std::ostream &out,
const Projection &projection)
198 projection.
print(out);
205 const std::vector<ProjectionPtr> &components)
206 :
Projection(bundleSpace, baseSpace), components_(components)
211void CompoundProjection::lift(
const State *xBase, State *xBundle)
const
213 unsigned int M = components_.size();
217 for (
unsigned int m = 0; m < M; m++)
219 const State *xmBase = xBase->
as<CompoundState>()->as<State>(m);
220 State *xmBundle = xBundle->
as<CompoundState>()->as<State>(m);
221 components_.at(m)->lift(xmBase, xmBundle);
226 components_.front()->lift(xBase, xBundle);
230void CompoundProjection::project(
const State *xBundle, State *xBase)
const
232 unsigned int M = components_.size();
236 for (
unsigned int m = 0; m < M; m++)
238 if (components_.at(m)->getBaseDimension() > 0)
240 const State *xmBundle = xBundle->
as<CompoundState>()->as<State>(m);
241 State *xmBase = xBase->
as<CompoundState>()->as<State>(m);
242 components_.at(m)->project(xmBundle, xmBase);
248 components_.front()->project(xBundle, xBase);
254 if (components_.size() > 0)
256 return components_.front()->getDimension();
266 if (components_.size() > 0)
268 return components_.front()->getCoDimension();
277 if (components_.size() > 0)
279 return components_.front()->getBaseDimension();
289 for (
unsigned int k = 0; k < components_.size(); k++)
291 if (!components_.at(k)->isFibered())
299 for (
unsigned int k = 0; k < components_.size(); k++)
301 out << components_.at(k) <<
"|";
The exception type for ompl.
A space to allow the composition of state spaces.
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
A shared pointer wrapper for ompl::base::StateSpace.
const T * as() const
Cast this instance to a desired type.
unsigned int getCoDimension() const
Dimension of Bundle - Dimension of Base.
virtual void print(std::ostream &out) const override
Print to stream.
bool isFibered() const override
Check that every compound has an explicit fiber representation.
unsigned int getBaseDimension() const
Dimension of Base Space.
unsigned int getDimension() const
Dimension of Bundle Space.
std::string stateTypeToString(base::StateSpacePtr) const
Return string representing type of ompl::base::StateSpace.
virtual void print(std::ostream &out) const
Print to stream.
friend std::ostream & operator<<(std::ostream &out, const Projection &)
Print to stream (actual implementation in print(std::ostream &out))
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ STATE_SPACE_SE2
ompl::base::SE2StateSpace
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
@ STATE_SPACE_SE3
ompl::base::SE3StateSpace
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
@ STATE_SPACE_TIME
ompl::base::TimeStateSpace
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_IDENTITY
ompl::multilevel::Projection_Identity
@ PROJECTION_COMPOUND
Compound projection.
@ PROJECTION_CONSTRAINED_RELAXATION
ompl::multilevel::Projection_Relaxation
Main namespace. Contains everything in this library.