37 #include "KoulesConfig.h"
38 #include "KoulesStateSpace.h"
39 #include "KoulesProjection.h"
41 namespace ob = ompl::base;
43 KoulesStateSpace::KoulesStateSpace(
unsigned int numKoules)
44 : RealVectorStateSpace(4 * numKoules + 5), mass_(numKoules + 1, kouleMass),
45 radius_(numKoules + 1, kouleRadius)
48 radius_[0] = shipRadius;
49 setName(
"Koules" + boost::lexical_cast<std::string>(numKoules) + getName());
58 bounds_.setLow(j, shipRadius);
59 bounds_.setHigh(j++, sideLength - shipRadius);
60 bounds_.setLow(j, shipRadius);
61 bounds_.setHigh(j++, sideLength - shipRadius);
63 bounds_.setLow(j, -10.);
64 bounds_.setHigh(j++, 10.);
65 bounds_.setLow(j, -10.);
66 bounds_.setHigh(j++, 10.);
68 bounds_.setLow(j, -boost::math::constants::pi<double>());
69 bounds_.setHigh(j++, boost::math::constants::pi<double>());
70 for (
unsigned int i = 0; i < numKoules; ++i)
73 bounds_.setLow(j, -2. * kouleRadius);
74 bounds_.setHigh(j++, sideLength + 2. * kouleRadius);
75 bounds_.setLow(j, -2. * kouleRadius);
76 bounds_.setHigh(j++, sideLength + 2. * kouleRadius);
78 bounds_.setLow(j, -10);
79 bounds_.setHigh(j++, 10.);
80 bounds_.setLow(j, -10.);
81 bounds_.setHigh(j++, 10.);
85 void KoulesStateSpace::registerProjections(
void)
92 bool KoulesStateSpace::isDead(
const ompl::base::State* state,
unsigned int i)
const
94 const StateType* s =
static_cast<const StateType*
>(state);
95 return s->values[i ? 4 * i + 1 : 0] == -2. * kouleRadius;
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
Definition of an abstract state.