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 
48 namespace ob = ompl::base;
49 namespace og = ompl::geometric;
50 
51 enum PlannerType { VFRRT = 0, TRRT, RRTSTAR };
52 
54 Eigen::VectorXd field(const ob::State *state)
55 {
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 
64 og::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 
127 std::string problemName(PlannerType plannerType)
128 {
129  if (plannerType == VFRRT)
130  return std::string("vfrrt-conservative.path");
131  else 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 
142 int main(int argc, char **argv)
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 }
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.
Definition: ScopedState.h:56
A shared pointer wrapper for ompl::geometric::SimpleSetup.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
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.
Definition: PlannerStatus.h:66
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
Definition of an abstract state.
Definition: State.h:49
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 of a geometric path.
Definition: PathGeometric.h:60
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47