RigidBodyPlanningWithIK.cpp
78 bool regionSamplingWithGS(const ob::SpaceInformationPtr &si, const ob::ProblemDefinitionPtr &pd, const ob::GoalRegion *region, const ob::GoalLazySamples *gls, ob::State *result)
98 // we continue sampling while we are able to find solutions, we have found not more than 2 previous solutions and we have not yet solved the problem
126 // bind a sampling function that fills its argument with a sampled state and returns true while it can produce new samples
127 // we don't need to check if new samples are different from ones previously computed as this is pefromed automatically by GoalLazySamples
128 ob::GoalSamplingFn samplingFunction = boost::bind(®ionSamplingWithGS, ss.getSpaceInformation(), ss.getProblemDefinition(), ®ion, _1, _2);
133 // we set a goal that is sampleable, but it in fact corresponds to a region that is not sampleable by default
149 // the region variable will now go out of scope. To make sure it is not used in the sampling function any more
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.
Definition: GoalLazySamples.h:72
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
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...
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
virtual unsigned int maxSampleCount() const
Return the maximum number of samples that can be asked for before repeating.
Definition: GoalStates.cpp:90
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:65
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
The lower and upper bounds for an Rn space.
Definition: RealVectorBounds.h:48
void setThreshold(double threshold)
Set the distance to the goal that is allowed for a state to be considered in the goal region...
Definition: GoalRegion.h:81
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...
Definition: GoalLazySamples.h:51
Genetic Algorithm for searching valid states.
Definition: GeneticSearch.h:62
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48