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)
104 const ob::StateSpacePtr& space = getStateSpace();
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"));
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
A shared pointer wrapper for ompl::base::Goal.
Definition of a scoped state.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking....
Definition of an abstract state.
A shared pointer wrapper for ompl::control::DirectedControlSampler.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Main namespace. Contains everything in this library.