Loading...
Searching...
No Matches
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
48class ValidityChecker : public ompl::base::StateValidityChecker
49{
50public:
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
67int 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);
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}
A shared pointer wrapper for ompl::base::OptimizationObjective.
double * values
The value of the actual vector in Rn
Definition of a scoped state.
Definition ScopedState.h:57
A shared pointer wrapper for ompl::base::SpaceInformation.
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
StateValidityChecker(SpaceInformation *si)
Constructor.
SpaceInformation * si_
The instance of space information this state validity checker operates on.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
Create the set of classes typically needed to solve a geometric problem.
Definition SimpleSetup.h:63
Benchmark a set of planners on a problem instance.
Definition Benchmark.h:49
Main namespace. Contains everything in this library.
Representation of a benchmark request.
Definition Benchmark.h:157