Diagonal.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2016, Georgia Institute of Technology
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 Rice University 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 /* Author: Florian Hauer */
36 
37 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
38 #include <ompl/base/spaces/RealVectorStateSpace.h>
39 #include <ompl/geometric/planners/rrt/RRTXstatic.h>
40 #include <ompl/geometric/planners/rrt/RRTsharp.h>
41 #include <ompl/geometric/planners/rrt/RRTstar.h>
42 #include <ompl/tools/benchmark/Benchmark.h>
43 
44 #include <boost/format.hpp>
45 #include <boost/math/constants/constants.hpp>
46 #include <fstream>
47 
48 class ValidityChecker : public ompl::base::StateValidityChecker
49 {
50 public:
51  ValidityChecker(const ompl::base::SpaceInformationPtr &si) : ompl::base::StateValidityChecker(si)
52  {
53  }
54 
55  bool isValid(const ompl::base::State *state) const override
56  {
57  const auto *state2D = state->as<ompl::base::RealVectorStateSpace::StateType>();
58 
59  double sum = 0;
60  for (unsigned i = 0; i < si_->getStateSpace()->getDimension(); ++i)
61  sum += state2D->values[i] * state2D->values[i];
62 
63  return sqrt(sum) > 0.1;
64  }
65 };
66 
67 int main(int argc, char **argv)
68 {
69  if (argc != 2)
70  {
71  std::cout << "Usage: " << argv[0] << " dimensionOfTheProblem" << std::endl;
72  exit(0);
73  }
74  int dim = atoi(argv[1]);
75 
76  auto space(std::make_shared<ompl::base::RealVectorStateSpace>(dim));
78  const ompl::base::SpaceInformationPtr &si = ss.getSpaceInformation();
79  space->setBounds(-1, 1);
80 
81  ss.setStateValidityChecker(std::make_shared<ValidityChecker>(si));
82 
83  ompl::base::ScopedState<> start(space), goal(space);
84  for (int i = 0; i < dim; ++i)
85  {
86  start[i] = -1;
87  goal[i] = 1;
88  }
89 
90  ss.setStartAndGoalStates(start, goal);
91 
92  // by default, use the Benchmark class
93  double runtime_limit = 5, memory_limit = 1024;
94  int run_count = 100;
95  ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.05, true, true, false, false);
96  ompl::tools::Benchmark b(ss, "Diagonal");
97 
98  double range = 0.1 * sqrt(dim);
99 
100  auto lengthObj(std::make_shared<ompl::base::PathLengthOptimizationObjective>(si));
101  ompl::base::OptimizationObjectivePtr oop((0.5 / sqrt(dim)) * lengthObj);
102 
103  ss.setOptimizationObjective(oop);
104 
105  bool knn = true;
106 
107  auto rrtstar(std::make_shared<ompl::geometric::RRTstar>(si));
108  rrtstar->setName("RRT*");
109  rrtstar->setDelayCC(true);
110  // rrtstar->setFocusSearch(true);
111  rrtstar->setRange(range);
112  rrtstar->setKNearest(knn);
113  b.addPlanner(rrtstar);
114  auto rrtsh(std::make_shared<ompl::geometric::RRTsharp>(si));
115  rrtsh->setRange(range);
116  rrtsh->setKNearest(knn);
117  b.addPlanner(rrtsh);
118  /*auto rrtsh3(std::make_shared<ompl::geometric::RRTsharp>(si));
119  rrtsh3->setName("RRT#v3");
120  rrtsh3->setRange(range);
121  rrtsh3->setKNearest(knn);
122  rrtsh3->setVariant(3);
123  b.addPlanner(rrtsh3);
124  auto rrtsh2(std::make_shared<ompl::geometric::RRTsharp>(si));
125  rrtsh2->setName("RRT#v2");
126  rrtsh2->setRange(range);
127  rrtsh2->setKNearest(knn);
128  rrtsh2->setVariant(2);
129  b.addPlanner(rrtsh2);*/
130  auto rrtX1(std::make_shared<ompl::geometric::RRTXstatic>(si));
131  rrtX1->setName("RRTX0.1");
132  rrtX1->setEpsilon(0.1);
133  rrtX1->setRange(range);
134  // rrtX1->setVariant(3);
135  rrtX1->setKNearest(knn);
136  b.addPlanner(rrtX1);
137  auto rrtX2(std::make_shared<ompl::geometric::RRTXstatic>(si));
138  rrtX2->setName("RRTX0.01");
139  rrtX2->setEpsilon(0.01);
140  rrtX2->setRange(range);
141  // rrtX2->setVariant(3);
142  rrtX2->setKNearest(knn);
143  b.addPlanner(rrtX2);
144  auto rrtX3(std::make_shared<ompl::geometric::RRTXstatic>(si));
145  rrtX3->setName("RRTX0.001");
146  rrtX3->setEpsilon(0.001);
147  rrtX3->setRange(range);
148  // rrtX3->setVariant(3);
149  rrtX3->setKNearest(knn);
150  b.addPlanner(rrtX3);
151  b.benchmark(request);
152  b.saveResultsToFile(boost::str(boost::format("Diagonal.log")).c_str());
153 
154  exit(0);
155 }
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
Definition of a scoped state.
Definition: ScopedState.h:56
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
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
Abstract definition for a class checking the validity of states. The implementation of this class mus...
StateValidityChecker(SpaceInformation *si)
Constructor.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking. If it is possible that ompl::base::StateSpace::interpolate() or ompl::control::ControlSpace::propagate() return states that are outside of bounds, this function should also make a call to ompl::base::SpaceInformation::satisfiesBounds().
A shared pointer wrapper for ompl::base::OptimizationObjective.
Representation of a benchmark request.
Definition: Benchmark.h:156
double * values
The value of the actual vector in Rn
SpaceInformation * si_
The instance of space information this state validity checker operates on.