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  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, 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
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
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
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66