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 "KoulesDecomposition.h" 44 #include <ompl/base/spaces/RealVectorStateSpace.h> 45 #include <ompl/control/planners/rrt/RRT.h> 46 #include <ompl/control/planners/kpiece/KPIECE1.h> 47 #include <ompl/control/planners/est/EST.h> 48 #include <ompl/control/planners/pdst/PDST.h> 49 #include <ompl/control/planners/sst/SST.h> 50 #include <ompl/control/planners/syclop/SyclopRRT.h> 51 #include <ompl/control/planners/syclop/SyclopEST.h> 67 return si_->satisfiesBounds(state);
75 return std::make_shared<KoulesDirectedControlSampler>(si, goal, propagateMax);
79 KoulesSetup::KoulesSetup(
unsigned int numKoules,
const std::string& plannerName,
80 const std::vector<double>& stateVec)
83 initialize(numKoules, plannerName, stateVec);
86 KoulesSetup::KoulesSetup(
unsigned int numKoules,
const std::string& plannerName,
double kouleVel)
89 initialize(numKoules, plannerName);
90 double* state = getProblemDefinition()->getStartState(0)->as<KoulesStateSpace::StateType>()->values;
93 for (
unsigned int i = 0; i < numKoules; ++i)
95 theta = rng.
uniformReal(0., 2. * boost::math::constants::pi<double>());
96 state[4 * i + 7] = kouleVel * cos(theta);
97 state[4 * i + 8] = kouleVel * sin(theta);
101 void KoulesSetup::initialize(
unsigned int numKoules,
const std::string& plannerName,
102 const std::vector<double>& stateVec)
108 if (stateVec.size() == space->getDimension())
109 space->copyFromReals(start.get(), stateVec);
115 std::vector<double> startVec(space->getDimension(), 0.);
116 double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules;
117 startVec[0] = .5 * sideLength;
118 startVec[1] = .5 * sideLength;
119 startVec[4] = .5 * delta;
120 for (
unsigned int i = 0; i < numKoules; ++i, theta += delta)
122 r = .1 + i * .1 / numKoules;
123 startVec[4 * i + 5] = .5 * sideLength + r * cos(theta);
124 startVec[4 * i + 6] = .5 * sideLength + r * sin(theta);
126 space->copyFromReals(start.get(), startVec);
128 setStartState(start);
130 setGoal(std::make_shared<KoulesGoal>(si_));
132 si_->setPropagationStepSize(propagationStepSize);
134 si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
136 bool isPDST = plannerName ==
"pdst";
138 si_->setDirectedControlSamplerAllocator(
141 return KoulesDirectedControlSamplerAllocator(si, goal, isPDST);
144 setPlanner(getConfiguredPlannerInstance(plannerName));
146 setStateValidityChecker(std::make_shared<KoulesStateValidityChecker>(si_));
148 setStatePropagator(std::make_shared<KoulesStatePropagator>(si_));
151 ob::PlannerPtr KoulesSetup::getConfiguredPlannerInstance(
const std::string& plannerName)
153 if (plannerName ==
"rrt")
155 auto rrtplanner(std::make_shared<oc::RRT>(si_));
156 rrtplanner->setIntermediateStates(
true);
159 if (plannerName ==
"est")
160 return std::make_shared<oc::EST>(si_);
161 if (plannerName ==
"kpiece")
162 return std::make_shared<oc::KPIECE1>(si_);
163 if (plannerName ==
"sst")
165 auto sstplanner(std::make_shared<oc::SST>(si_));
166 sstplanner->setSelectionRadius(0.05);
167 sstplanner->setPruningRadius(0.001);
170 if (plannerName ==
"sycloprrt")
171 return std::make_shared<oc::SyclopRRT>(si_, std::make_shared<KoulesDecomposition>(getStateSpace()));
172 if (plannerName ==
"syclopest")
173 return std::make_shared<oc::SyclopEST>(si_, std::make_shared<KoulesDecomposition>(getStateSpace()));
175 auto pdstplanner(std::make_shared<oc::PDST>(si_));
176 pdstplanner->setProjectionEvaluator(
177 si_->getStateSpace()->getProjection(
"PDSTProjection"));
Definition of a scoped state.
This namespace contains sampling based planning routines used by planning under differential constrai...
A shared pointer wrapper for ompl::base::StateSpace.
Main namespace. Contains everything in this library.
A 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 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.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
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().
A shared pointer wrapper for ompl::base::Goal.