VectorFieldNonconservative.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/VFUpstreamCriterionOptimizationObjective.h>
41 #include <ompl/base/spaces/RealVectorStateSpace.h>
42 #include <ompl/geometric/planners/rrt/RRTstar.h>
43 #include <ompl/geometric/planners/rrt/VFRRT.h>
44 #include <ompl/geometric/SimpleSetup.h>
45 
46 namespace ob = ompl::base;
47 namespace og = ompl::geometric;
48 
49 
51 Eigen::VectorXd field(const ob::State *state)
52 {
54  Eigen::VectorXd v(2);
55  v[0] = x[1];
56  v[1] = -x[0];
57  v.normalize();
58  return v;
59 }
60 
61 int main(int argc, char **argv)
62 {
63  // construct the state space we are planning in
64  auto space(std::make_shared<ob::RealVectorStateSpace>(2));
65  auto si(std::make_shared<ob::SpaceInformation>(space));
66 
67  ob::RealVectorBounds bounds(2);
68  bounds.setLow(-10);
69  bounds.setHigh(10);
70 
71  space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
72 
73  // define a simple setup class
74  og::SimpleSetup ss(space);
75 
76  // set state validity checking for this space
77  ss.setStateValidityChecker(
78  std::make_shared<ob::AllValidStateValidityChecker>(si));
79 
80  // create a start state
81  ob::ScopedState<> start(space);
82  start[0] = -5;
83  start[1] = -2;
84 
85  // create a goal state
86  ob::ScopedState<> goal(space);
87  goal[0] = 5;
88  goal[1] = 2;
89 
90  // set the start and goal states
91  ss.setStartAndGoalStates(start, goal, 0.1);
92 
93  // initialize the planner
94  double explorationSetting = 0.7;
95  double lambda = 1;
96  unsigned int update_freq = 100;
97  ss.setPlanner(std::make_shared<og::VFRRT>(
98  ss.getSpaceInformation(), field, explorationSetting, lambda, update_freq));
99  ss.setup();
100 
101  // attempt to solve the problem
102  ob::PlannerStatus solved = ss.solve(10.0);
103 
104  if (solved)
105  {
106  if (solved == ob::PlannerStatus::EXACT_SOLUTION)
107  std::cout << "Found solution.\n";
108  else
109  std::cout << "Found approximate solution.\n";
110 
111  // Set up to write the path
112  std::ofstream f("vfrrt-nonconservative.path");
113  ompl::geometric::PathGeometric p = ss.getSolutionPath();
114  p.interpolate();
115  auto upstream(std::make_shared<ob::VFUpstreamCriterionOptimizationObjective>(
116  ss.getSpaceInformation(), field));
117  p.printAsMatrix(f);
118  std::cout << "Total upstream cost: " << p.cost(upstream) << "\n";
119  }
120  else
121  std::cout << "No solution found.\n";
122 
123  return 0;
124 }
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
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
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
A state space representing Rn. The distance function is the L2 norm.
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