37 #include "KoulesSetup.h"
38 #include "KoulesGoal.h"
39 #include "KoulesStateSpace.h"
40 #include "KoulesControlSpace.h"
41 #include "KoulesStatePropagator.h"
42 #include "KoulesDirectedControlSampler.h"
43 #include <ompl/base/spaces/RealVectorStateSpace.h>
44 #include <ompl/control/planners/rrt/RRT.h>
45 #include <ompl/control/planners/kpiece/KPIECE1.h>
46 #include <ompl/control/planners/est/EST.h>
47 #include <ompl/control/planners/pdst/PDST.h>
49 namespace ob = ompl::base;
50 namespace oc = ompl::control;
62 return si_->satisfiesBounds(state);
74 KoulesSetup::KoulesSetup(
unsigned int numKoules,
const std::string& plannerName,
75 const std::vector<double>& stateVec)
76 : ompl::control::SimpleSetup(ompl::control::ControlSpacePtr(new
KoulesControlSpace(numKoules)))
78 initialize(numKoules, plannerName, stateVec);
81 KoulesSetup::KoulesSetup(
unsigned int numKoules,
const std::string& plannerName,
double kouleVel)
82 : ompl::control::SimpleSetup(ompl::control::ControlSpacePtr(new
KoulesControlSpace(numKoules)))
84 initialize(numKoules, plannerName);
85 double* state = getProblemDefinition()->getStartState(0)->as<KoulesStateSpace::StateType>()->values;
88 for (
unsigned int i = 0; i < numKoules; ++i)
90 theta = rng.
uniformReal(0., 2. * boost::math::constants::pi<double>());
91 state[4 * i + 7] = kouleVel * cos(theta);
92 state[4 * i + 8] = kouleVel * sin(theta);
96 void KoulesSetup::initialize(
unsigned int numKoules,
const std::string& plannerName,
97 const std::vector<double>& stateVec)
103 if (stateVec.size() == space->getDimension())
104 space->copyFromReals(start.get(), stateVec);
110 std::vector<double> startVec(space->getDimension(), 0.);
111 double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules;
112 startVec[0] = .5 * sideLength;
113 startVec[1] = .5 * sideLength;
114 startVec[4] = .5 * delta;
115 for (
unsigned int i = 0; i < numKoules; ++i, theta += delta)
117 r = .1 + i * .1 / numKoules;
118 startVec[4 * i + 5] = .5 * sideLength + r * cos(theta);
119 startVec[4 * i + 6] = .5 * sideLength + r * sin(theta);
121 space->copyFromReals(start.get(), startVec);
123 setStartState(start);
127 si_->setPropagationStepSize(propagationStepSize);
129 si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
131 si_->setDirectedControlSamplerAllocator(
132 boost::bind(&KoulesDirectedControlSamplerAllocator, _1, getGoal(), plannerName ==
"pdst"));
134 setPlanner(getConfiguredPlannerInstance(plannerName));
141 ob::PlannerPtr KoulesSetup::getConfiguredPlannerInstance(
const std::string& plannerName)
143 if (plannerName ==
"rrt")
146 rrtplanner->as<
oc::RRT>()->setIntermediateStates(
true);
149 else if (plannerName ==
"est")
151 else if (plannerName ==
"kpiece")
156 pdstplanner->as<
oc::PDST>()->setProjectionEvaluator(
157 si_->getStateSpace()->getProjection(
"PDSTProjection"));
Definition of a scoped state.
A boost shared pointer wrapper for ompl::base::StateSpace.
Path-Directed Subdivision Tree.
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
A boost shared pointer wrapper for ompl::base::Planner.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
A boost shared pointer wrapper for ompl::control::DirectedControlSampler.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition of an abstract state.
Kinodynamic Planning by Interior-Exterior Cell Exploration.
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking. If it is possible that ompl::base::StateSpace::interpolate() or ompl::control::ControlSpace::propagate() return states that are outside of bounds, this function should also make a call to ompl::base::SpaceInformation::satisfiesBounds().
Rapidly-exploring Random Tree.
A boost shared pointer wrapper for ompl::base::Goal.