37 #include <ompl/base/spaces/SE3StateSpace.h>
38 #include <ompl/geometric/SimpleSetup.h>
39 #include <ompl/base/goals/GoalLazySamples.h>
40 #include <ompl/geometric/GeneticSearch.h>
42 #include <ompl/config.h>
45 namespace ob = ompl::base;
46 namespace og = ompl::geometric;
85 for (
int i = 0 ; i < 100 ; ++i)
86 if (g.solve(0.05, *region, result))
94 std::cout <<
"Found goal state: " << std::endl;
95 si->printState(result);
102 void planWithIK(
void)
119 start->setXYZ(0, 0, 0);
120 start->rotation().setIdentity();
121 ss.addStartState(start);
124 MyGoalRegion region(ss.getSpaceInformation());
128 ob::GoalSamplingFn samplingFunction = boost::bind(®ionSamplingWithGS, ss.getSpaceInformation(), ss.getProblemDefinition(), ®ion, _1, _2);
141 std::cout <<
"Found solution:" << std::endl;
143 ss.simplifySolution();
144 ss.getSolutionPath().print(std::cout);
147 std::cout <<
"No solution found" << std::endl;
155 int main(
int,
char **)
157 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
Definition of a goal region that can be sampled, but the sampling process can be slow. This class allows sampling the happen in a separate thread, and the number of goals may increase, as the planner is running, in a thread-safe manner.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a scoped state.
A boost shared pointer wrapper for ompl::base::StateSpace.
virtual double distanceGoal(const State *st) const =0
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
virtual unsigned int maxSampleCount(void) const
Return the maximum number of samples that can be asked for before repeating.
CompoundState StateType
Define the type of state allocated by this state space.
const T * as(void) const
Cast this instance to a desired type.
Create the set of classes typically needed to solve a geometric problem.
A class to store the exit status of Planner::solve()
Definition of an abstract state.
The lower and upper bounds for an Rn space.
void setThreshold(double threshold)
Set the distance to the goal that is allowed for a state to be considered in the goal region...
Definition of a goal region.
A boost shared pointer wrapper for ompl::base::Goal.
boost::function< bool(const GoalLazySamples *, State *)> GoalSamplingFn
Goal sampling function. Returns false when no further calls should be made to it. Fills its second ar...
Genetic Algorithm for searching valid states.
A state space representing SE(3)