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]);
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 argc,
char **argv)
145 for (
unsigned int n = 0; n < 3; 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";
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...
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...
Definition of a scoped state.
A shared pointer wrapper for ompl::geometric::SimpleSetup.
const T * as() const
Cast this instance to a desired type.
ompl::base::State StateType
Define the type of state allocated by this space.
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states. States are inserted uniformly (more states on longer segments). Changes are performed only if a path has less than count states.
The planner found an exact solution.
A class to store the exit status of Planner::solve()
Definition of an abstract state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The lower and upper bounds for an Rn space.
Definition of a geometric path.
This namespace contains code that is specific to planning under geometric constraints.