39 #include <ompl/base/StateSpace.h>
40 #include <ompl/base/objectives/VFMechanicalWorkOptimizationObjective.h>
41 #include <ompl/base/objectives/VFUpstreamCriterionOptimizationObjective.h>
42 #include <ompl/base/spaces/RealVectorStateSpace.h>
43 #include <ompl/geometric/planners/rrt/RRTstar.h>
44 #include <ompl/geometric/planners/rrt/TRRT.h>
45 #include <ompl/geometric/planners/rrt/VFRRT.h>
46 #include <ompl/geometric/SimpleSetup.h>
51 enum PlannerType { VFRRT = 0, TRRT, RRTSTAR };
54 Eigen::VectorXd field(
const ob::State *state)
58 v[0] = std::cos(x[0]) * std::sin(x[1]);
59 v[1] = std::sin(x[0]) * std::cos(x[1]);
64 og::SimpleSetupPtr setupProblem(PlannerType plannerType)
67 auto space(std::make_shared<ob::RealVectorStateSpace>(2));
68 auto si(std::make_shared<ob::SpaceInformation>(space));
74 space->setBounds(bounds);
77 auto ss(std::make_shared<og::SimpleSetup>(space));
80 ss->setStateValidityChecker(std::make_shared<ob::AllValidStateValidityChecker>(si));
93 ss->setStartAndGoalStates(start, goal, 0.1);
96 if (plannerType == TRRT)
98 ss->setOptimizationObjective(
99 std::make_shared<ob::VFMechanicalWorkOptimizationObjective>(si, field));
100 ss->setPlanner(std::make_shared<og::TRRT>(ss->getSpaceInformation()));
102 else if (plannerType == RRTSTAR)
104 ss->setOptimizationObjective(
105 std::make_shared<ob::VFUpstreamCriterionOptimizationObjective>(si, field));
106 ss->setPlanner(std::make_shared<og::RRTstar>(ss->getSpaceInformation()));
108 else if (plannerType == VFRRT)
110 double explorationSetting = 0.7;
112 unsigned int update_freq = 100;
113 ss->setPlanner(std::make_shared<og::VFRRT>(ss->getSpaceInformation(), field, explorationSetting, lambda, update_freq));
117 std::cout <<
"Bad problem number.\n";
127 std::string problemName(PlannerType plannerType)
129 if (plannerType == VFRRT)
130 return std::string(
"vfrrt-conservative.path");
131 if (plannerType == TRRT)
132 return std::string(
"trrt-conservative.path");
133 else if (plannerType == RRTSTAR)
134 return std::string(
"rrtstar-conservative.path");
137 std::cout <<
"Bad problem number.\n";
142 int main(
int,
char **)
145 for (
unsigned int n = 0; n < 3; n++)
148 og::SimpleSetupPtr ss = setupProblem(PlannerType(n));
156 std::cout <<
"Found solution.\n";
158 std::cout <<
"Found approximate solution.\n";
161 std::ofstream f(problemName(PlannerType(n)).c_str());
164 auto upstream(std::make_shared<ob::VFUpstreamCriterionOptimizationObjective>(
165 ss->getSpaceInformation(), field));
167 std::cout <<
"Total upstream cost: " << p.
cost(upstream) <<
"\n";
170 std::cout <<
"No solution found.\n";
The lower and upper bounds for an Rn space.
Definition of a scoped state.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
Definition of a geometric path.
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states....
virtual void printAsMatrix(std::ostream &out) const
Print the path as a real-valued matrix where the i-th row represents the i-th state along the path....
base::Cost cost(const base::OptimizationObjectivePtr &obj) const override
The sum of the costs for the sequence of segments that make up the path, computed using OptimizationO...
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
A class to store the exit status of Planner::solve()
@ EXACT_SOLUTION
The planner found an exact solution.