void ompl::base::DiscreteStateSpace::registerProjections()
{
class DiscreteDefaultProjection : public ProjectionEvaluator
{
public:
DiscreteDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
{
}
virtual unsigned int getDimension() const
{
return 1;
}
virtual void defaultCellSizes()
{
bounds_.resize(1);
bounds_.low[0] = space_->as<DiscreteStateSpace>()->lowerBound_;
bounds_.high[0] = space_->as<DiscreteStateSpace>()->upperBound_;
cellSizes_.resize(1);
cellSizes_[0] = 1.0;
}
virtual void project(const State *state, EuclideanProjection &projection) const
{
projection(0) = state->as<DiscreteStateSpace::StateType>()->value;
}
};
registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new DiscreteDefaultProjection(this))));
}
void ompl::base::SE2StateSpace::registerProjections(void)
{
class SE2DefaultProjection : public ProjectionEvaluator
{
public:
SE2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
{
}
virtual unsigned int getDimension(void) const
{
return 2;
}
virtual void defaultCellSizes(void)
{
cellSizes_.resize(2);
const RealVectorBounds &b = space_->as<SE2StateSpace>()->getBounds();
cellSizes_[0] = (b.high[0] - b.low[0]) / magic::PROJECTION_DIMENSION_SPLITS;
cellSizes_[1] = (b.high[1] - b.low[1]) / magic::PROJECTION_DIMENSION_SPLITS;
}
virtual void project(const State *state, EuclideanProjection &projection) const
{
memcpy(&projection(0), state->as<SE2StateSpace::StateType>()->as<RealVectorStateSpace::StateType>(0)->values, 2 * sizeof(double));
}
};
registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SE2DefaultProjection(this))));
}
void ompl::base::SE3StateSpace::registerProjections()
{
class SE3DefaultProjection : public ProjectionEvaluator
{
public:
SE3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
{
}
unsigned int getDimension() const override
{
return 3;
}
void defaultCellSizes() override
{
cellSizes_.resize(3);
bounds_ = space_->as<SE3StateSpace>()->getBounds();
cellSizes_[0] = (bounds_.high[0] - bounds_.low[0]) / magic::PROJECTION_DIMENSION_SPLITS;
cellSizes_[1] = (bounds_.high[1] - bounds_.low[1]) / magic::PROJECTION_DIMENSION_SPLITS;
cellSizes_[2] = (bounds_.high[2] - bounds_.low[2]) / magic::PROJECTION_DIMENSION_SPLITS;
}
void project(const State *state, EuclideanProjection &projection) const override
{
memcpy(&projection(0), state->as<SE3StateSpace::StateType>()->as<RealVectorStateSpace::StateType>(0)->values, 3 * sizeof(double));
}
};
registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SE3DefaultProjection(this))));
}
void ompl::base::DiscreteStateSpace::registerProjections()
{
class DiscreteDefaultProjection : public ProjectionEvaluator
{
public:
DiscreteDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
{
}
unsigned int getDimension() const override
{
return 1;
}
void defaultCellSizes() override
{
bounds_.resize(1);
bounds_.low[0] = space_->as<DiscreteStateSpace>()->lowerBound_;
bounds_.high[0] = space_->as<DiscreteStateSpace>()->upperBound_;
cellSizes_.resize(1);
cellSizes_[0] = 1.0;
}
void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
{
projection(0) = state->as<DiscreteStateSpace::StateType>()->value;
}
};
registerDefaultProjection(std::make_shared<DiscreteDefaultProjection>(this));
}
void ompl::base::SO3StateSpace::registerProjections(void)
{
class SO3DefaultProjection : public ProjectionEvaluator
{
public:
SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
{
}
virtual unsigned int getDimension(void) const
{
return 3;
}
virtual void defaultCellSizes(void)
{
cellSizes_.resize(3);
cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
cellSizes_[1] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
cellSizes_[2] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
}
virtual void project(const State *state, EuclideanProjection &projection) const
{
projection(0) = state->as<SO3StateSpace::StateType>()->x;
projection(1) = state->as<SO3StateSpace::StateType>()->y;
projection(2) = state->as<SO3StateSpace::StateType>()->z;
}
};
registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO3DefaultProjection(this))));
}
void ompl::base::TimeStateSpace::registerProjections(void)
{
class TimeDefaultProjection : public ProjectionEvaluator
{
public:
TimeDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
{
}
virtual unsigned int getDimension(void) const
{
return 1;
}
virtual void defaultCellSizes(void)
{
cellSizes_.resize(1);
if (space_->as<TimeStateSpace>()->isBounded())
cellSizes_[0] = (space_->as<TimeStateSpace>()->getMaxTimeBound() - space_->as<TimeStateSpace>()->getMinTimeBound()) / magic::PROJECTION_DIMENSION_SPLITS;
else
cellSizes_[0] = 1.0;
}
virtual void project(const State *state, EuclideanProjection &projection) const
{
projection(0) = state->as<TimeStateSpace::StateType>()->position;
}
};
registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new TimeDefaultProjection(this))));
}
void ompl::base::SO2StateSpace::registerProjections()
{
class SO2DefaultProjection : public ProjectionEvaluator
{
public:
SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
{
}
unsigned int getDimension() const override
{
return 1;
}
void defaultCellSizes() override
{
cellSizes_.resize(1);
cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
bounds_.resize(1);
bounds_.low[0] = -boost::math::constants::pi<double>();
bounds_.high[0] = boost::math::constants::pi<double>();
}
void project(const State *state, EuclideanProjection &projection) const override
{
projection(0) = state->as<SO2StateSpace::StateType>()->value;
}
};
registerDefaultProjection(std::make_shared<SO2DefaultProjection>(this));
}
void ompl::base::SO3StateSpace::registerProjections()
{
class SO3DefaultProjection : public ProjectionEvaluator
{
public:
SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
{
}
unsigned int getDimension() const override
{
return 3;
}
void defaultCellSizes() override
{
cellSizes_.resize(3);
cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
cellSizes_[1] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
cellSizes_[2] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
bounds_.resize(3);
bounds_.setLow(-1.0);
bounds_.setHigh(1.0);
}
void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
{
projection(0) = state->as<SO3StateSpace::StateType>()->x;
projection(1) = state->as<SO3StateSpace::StateType>()->y;
projection(2) = state->as<SO3StateSpace::StateType>()->z;
}
};
registerDefaultProjection(std::make_shared<SO3DefaultProjection>(this));
}
virtual void registerProjections(void)
{
registerDefaultProjection(ob::ProjectionEvaluatorPtr(new RigidBodyStateProjectionEvaluator(this)));
}
void KoulesStateSpace::registerProjections(void)
{
registerDefaultProjection(ob::ProjectionEvaluatorPtr(new KoulesProjection(this, (getDimension() - 1) / 4 + 1)));
registerProjection("PDSTProjection", ob::ProjectionEvaluatorPtr(
new KoulesProjection(this, (getDimension() - 1) / 2 + 1)));
}
/*! Register the projections by setting the pointer to the twoDRobotStateProjectionEvaluator
*/
void twoDRobotStateSpace::registerProjections(void)
{
registerDefaultProjection(ob::ProjectionEvaluatorPtr(new twoDRobotStateProjectionEvaluator(this)));
}
void registerProjections() override
{
registerDefaultProjection(ompl::base::ProjectionEvaluatorPtr(
new KinematicChainProjector(this)));
}
