37 #include "KoulesConfig.h"
38 #include "KoulesStateSpace.h"
39 #include "KoulesControlSpace.h"
40 #include <ompl/base/spaces/RealVectorStateSpace.h>
42 namespace ob = ompl::base;
43 namespace oc = ompl::control;
48 oc::RealVectorControlSpace::ControlType *rcontrol =
49 control->
as<oc::RealVectorControlSpace::ControlType>();
51 double theta = rng_.
uniformReal(0., 2. * boost::math::constants::pi<double>());
52 rcontrol->values[0] = r * cos(theta);
53 rcontrol->values[1] = r * sin(theta);
61 void KoulesControlSampler::steer(
oc::Control *control,
const ob::State *state,
double x,
double y)
63 const double* r = state->
as<KoulesStateSpace::StateType>()->values;
66 double xNrm2 = dx * dx + dy * dy;
67 if (xNrm2 > std::numeric_limits<float>::epsilon())
71 oc::RealVectorControlSpace::ControlType *rcontrol =
72 control->
as<oc::RealVectorControlSpace::ControlType>();
73 rcontrol->values[0] = v * dx;
74 rcontrol->values[1] = v * dy;
80 KoulesControlSpace::KoulesControlSpace(
unsigned int numKoules)
81 : ompl::control::RealVectorControlSpace(
82 ompl::base::StateSpacePtr(new KoulesStateSpace(numKoules)), 2)
84 bounds_.setLow(shipVmin);
85 bounds_.setHigh(shipVmax);
std::vector< double > low
Lower bound.
Definition of an abstract control.
const ControlSpace * space_
The control space this sampler operates on.
const T * as(void) const
Cast this instance to a desired type.
A control space representing Rn.
std::vector< double > high
Upper bound.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition of an abstract state.
The lower and upper bounds for an Rn space.
const T * as(void) const
Cast this instance to a desired type.
T * as(void)
Cast this instance to a desired type.
virtual void sample(ompl::control::Control *control)
Sample a control. All other control sampling functions default to this one, unless a user-specified i...