Loading...
Searching...
No Matches
VectorFieldConservative.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2015, Caleb Voss and Wilson Beebe
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Willow Garage nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Authors: Caleb Voss, Wilson Beebe */
36
37#include <fstream>
38
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>
47
48namespace ob = ompl::base;
49namespace og = ompl::geometric;
50
51enum PlannerType { VFRRT = 0, TRRT, RRTSTAR };
52
54Eigen::VectorXd field(const ob::State *state)
55{
56 const ob::RealVectorStateSpace::StateType &x = *state->as<ob::RealVectorStateSpace::StateType>();
57 Eigen::VectorXd v(2);
58 v[0] = std::cos(x[0]) * std::sin(x[1]);
59 v[1] = std::sin(x[0]) * std::cos(x[1]);
60 return -v;
61}
62
64og::SimpleSetupPtr setupProblem(PlannerType plannerType)
65{
66 // construct the state space we are planning in
67 auto space(std::make_shared<ob::RealVectorStateSpace>(2));
68 auto si(std::make_shared<ob::SpaceInformation>(space));
69
70 ob::RealVectorBounds bounds(2);
71 bounds.setLow(-10);
72 bounds.setHigh(10);
73
74 space->setBounds(bounds);
75
76 // define a simple setup class
77 auto ss(std::make_shared<og::SimpleSetup>(space));
78
79 // set state validity checking for this space
80 ss->setStateValidityChecker(std::make_shared<ob::AllValidStateValidityChecker>(si));
81
82 // create a start state
83 ob::ScopedState<> start(space);
84 start[0] = -5;
85 start[1] = -2;
86
87 // create a goal state
88 ob::ScopedState<> goal(space);
89 goal[0] = 5;
90 goal[1] = 3;
91
92 // set the start and goal states
93 ss->setStartAndGoalStates(start, goal, 0.1);
94
95 // make the optimization objectives for TRRT and RRT*, and set the planner
96 if (plannerType == TRRT)
97 {
98 ss->setOptimizationObjective(
99 std::make_shared<ob::VFMechanicalWorkOptimizationObjective>(si, field));
100 ss->setPlanner(std::make_shared<og::TRRT>(ss->getSpaceInformation()));
101 }
102 else if (plannerType == RRTSTAR)
103 {
104 ss->setOptimizationObjective(
105 std::make_shared<ob::VFUpstreamCriterionOptimizationObjective>(si, field));
106 ss->setPlanner(std::make_shared<og::RRTstar>(ss->getSpaceInformation()));
107 }
108 else if (plannerType == VFRRT)
109 {
110 double explorationSetting = 0.7;
111 double lambda = 1;
112 unsigned int update_freq = 100;
113 ss->setPlanner(std::make_shared<og::VFRRT>(ss->getSpaceInformation(), field, explorationSetting, lambda, update_freq));
114 }
115 else
116 {
117 std::cout << "Bad problem number.\n";
118 exit(-1);
119 }
120
121 ss->setup();
122
123 return ss;
124}
125
127std::string problemName(PlannerType plannerType)
128{
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");
135 else
136 {
137 std::cout << "Bad problem number.\n";
138 exit(-1);
139 }
140}
141
142int main(int, char **)
143{
144 // Run all three problems
145 for (unsigned int n = 0; n < 3; n++)
146 {
147 // initialize the planner
148 og::SimpleSetupPtr ss = setupProblem(PlannerType(n));
149
150 // attempt to solve the problem
151 ob::PlannerStatus solved = ss->solve(10.0);
152
153 if (solved)
154 {
155 if (solved == ob::PlannerStatus::EXACT_SOLUTION)
156 std::cout << "Found solution.\n";
157 else
158 std::cout << "Found approximate solution.\n";
159
160 // Set up to write the path
161 std::ofstream f(problemName(PlannerType(n)).c_str());
162 ompl::geometric::PathGeometric p = ss->getSolutionPath();
163 p.interpolate();
164 auto upstream(std::make_shared<ob::VFUpstreamCriterionOptimizationObjective>(
165 ss->getSpaceInformation(), field));
166 p.printAsMatrix(f);
167 std::cout << "Total upstream cost: " << p.cost(upstream) << "\n";
168 }
169 else
170 std::cout << "No solution found.\n";
171 }
172
173 return 0;
174}
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition: ScopedState.h:57
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
Definition of a geometric path.
Definition: PathGeometric.h:66
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.
Definition: GeneticSearch.h:48
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49