37 #include <ompl/base/SpaceInformation.h>
38 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
39 #include <ompl/base/objectives/StateCostIntegralObjective.h>
40 #include <ompl/base/objectives/MaximizeMinClearanceObjective.h>
41 #include <ompl/base/spaces/RealVectorStateSpace.h>
42 #include <ompl/geometric/planners/rrt/RRTstar.h>
46 namespace ob = ompl::base;
47 namespace og = ompl::geometric;
57 ob::StateValidityChecker(si) {}
76 double x = state2D->values[0];
77 double y = state2D->values[1];
81 return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25;
97 void plan(
int argc,
char** argv)
130 pdef->setStartAndGoalStates(start, goal);
136 pdef->setOptimizationObjective(getPathLengthObjective(si));
147 optimizingPlanner->setProblemDefinition(pdef);
148 optimizingPlanner->setup();
158 <<
"Found solution of path length "
159 << pdef->getSolutionPath()->length() << std::endl;
165 std::ofstream outFile(argv[1]);
167 printAsMatrix(outFile);
172 std::cout <<
"No solution found." << std::endl;
175 int main(
int argc,
char** argv)
197 obj->setCostThreshold(
ob::Cost(1.51));
217 ob::StateCostIntegralObjective(si, true)
228 return ob::Cost(1 / si_->getStateValidityChecker()->clearance(s));
271 return 10.0*lengthObj + clearObj;
280 obj->setCostToGoHeuristic(&ob::goalRegionCostToGo);
Optimal Rapidly-exploring Random Trees.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
Definition of a scoped state.
A boost shared pointer wrapper for ompl::base::StateSpace.
void addObjective(const OptimizationObjectivePtr &objective, double weight)
Adds a new objective for this multiobjective. A weight must also be specified for specifying importan...
virtual double clearance(const State *) const
Report the distance to the nearest invalid state when starting from state. If the distance is negativ...
State StateType
Define the type of state allocated by this space.
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
const T * as(void) const
Cast this instance to a desired type.
A boost shared pointer wrapper for ompl::base::Planner.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Defines optimization objectives where path cost can be represented as a path integral over a cost fun...
Abstract definition for a class checking the validity of states. The implementation of this class mus...
A class to store the exit status of Planner::solve()
virtual Cost stateCost(const State *s) const
Evaluate a cost map defined on the state space at a state s. Default implementation maps all states t...
A state space representing Rn. The distance function is the L2 norm.
An optimization objective which corresponds to optimizing path length.
Definition of an abstract state.
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().
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
Definition of a geometric path.