Loading...
Searching...
No Matches
MultiLevelPlanningHyperCube.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Stuttgart
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 University of Stuttgart nor the names
18 * of its contributors may be used to endorse or promote products
19 * derived from this software without specific prior written
20 * permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Andreas Orthey */
37
38#include "MultiLevelPlanningCommon.h"
39#include "MultiLevelPlanningHyperCubeCommon.h"
40#include <ompl/base/spaces/RealVectorStateSpace.h>
41
42#include <ompl/multilevel/planners/qrrt/QRRT.h>
43
44#include <ompl/tools/benchmark/Benchmark.h>
45#include <ompl/util/String.h>
46
47#include <boost/math/constants/constants.hpp>
48#include <boost/format.hpp>
49#include <fstream>
50
51const unsigned int ndim = 100;
52
53int main()
54{
55 auto space(std::make_shared<ompl::base::RealVectorStateSpace>(ndim));
58 ompl::base::ScopedState<> start(space), goal(space);
59
60 ompl::base::SpaceInformationPtr si = ss.getSpaceInformation();
61
62 bounds.setLow(0.);
63 bounds.setHigh(1.);
64 space->setBounds(bounds);
65 ss.setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(si, ndim));
66 si->setStateValidityCheckingResolution(0.001);
67 for (unsigned int i = 0; i < ndim; ++i)
68 {
69 start[i] = 0.;
70 goal[i] = 1.;
71 }
72 ss.setStartAndGoalStates(start, goal);
73
74 std::vector<int> admissibleProjection = getHypercubeAdmissibleProjection(ndim);
75 ompl::base::PlannerPtr planner = GetMultiLevelPlanner<ompl::multilevel::QRRT>(admissibleProjection, si, "QRRT");
76 ss.setPlanner(planner);
77
78 bool solved = ss.solve(1.0);
79
80 double timeToCompute = ss.getLastPlanComputationTime();
81
82 if (solved)
83 {
84 const ompl::base::ProblemDefinitionPtr pdef = planner->getProblemDefinition();
85 std::cout << std::string(80, '-') << std::endl;
86 pdef->getSolutionPath()->print(std::cout);
87 std::cout << std::string(80, '-') << std::endl;
88 OMPL_INFORM("Solved hypercube with %d dimensions after %f seconds.", ndim, timeToCompute);
89 }
90 else
91 {
92 OMPL_ERROR("Failed finding solution after %f seconds.", timeToCompute);
93 }
94
95 return 0;
96}
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition ScopedState.h:57
Create the set of classes typically needed to solve a geometric problem.
Definition SimpleSetup.h:63
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64