37 #include "KoulesDirectedControlSampler.h"
38 #include "KoulesStateSpace.h"
39 #include <ompl/base/spaces/RealVectorStateSpace.h>
41 namespace ob = ompl::base;
42 namespace oc = ompl::control;
46 const double* dstPos = dest->
as<KoulesStateSpace::StateType>()->values;
51 cs_.steer(control, source, dstPos[0], dstPos[1]);
53 statePropagator_->propagate(source, control, stepSize, dest);
55 if (goal_->isSatisfied(dest))
61 unsigned int r = steps;
62 for (
unsigned int i = 1 ; i < steps ; ++i)
64 statePropagator_->propagate(temp1, control, stepSize, temp2);
65 if (goal_->isSatisfied(dest))
72 std::swap(temp1, temp2);
Definition of an abstract control.
const T * as(void) const
Cast this instance to a desired type.
virtual unsigned int sampleStepCount(unsigned int minSteps, unsigned int maxSteps)
Sample a number of steps to execute a control for.
Definition of an abstract state.
virtual unsigned int sampleTo(ompl::control::Control *control, const ompl::base::State *source, ompl::base::State *dest)
Sample a control given that it will be applied to state state and the intention is to reach state tar...
const SpaceInformation * si_
The space information this sampler operates on.