Loading...
Searching...
No Matches
ConstrainedPlanningSphere.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2018, Rice University
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: Zachary Kingston */
36
37#include "ConstrainedPlanningCommon.h"
38
39class SphereConstraint : public ob::Constraint
40{
41public:
42 SphereConstraint() : ob::Constraint(3, 1)
43 {
44 }
45
46 void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
47 {
48 out[0] = x.norm() - 1;
49 }
50
51 void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
52 {
53 out = x.transpose().normalized();
54 }
55};
56
57class SphereProjection : public ob::ProjectionEvaluator
58{
59public:
60 SphereProjection(const ob::StateSpacePtr &space) : ob::ProjectionEvaluator(space)
61 {
62 }
63
64 unsigned int getDimension() const override
65 {
66 return 2;
67 }
68
69 void defaultCellSizes() override
70 {
71 cellSizes_.resize(2);
72 cellSizes_[0] = 0.1;
73 cellSizes_[1] = 0.1;
74 }
75
76 void project(const ob::State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
77 {
78 auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
79 projection(0) = atan2(x[1], x[0]);
80 projection(1) = acos(x[2]);
81 }
82};
83
84bool obstacles(const ob::State *state)
85{
86 auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
87
88 if (-0.80 < x[2] && x[2] < -0.6)
89 {
90 if (-0.05 < x[1] && x[1] < 0.05)
91 return x[0] > 0;
92 return false;
93 }
94 else if (-0.1 < x[2] && x[2] < 0.1)
95 {
96 if (-0.05 < x[0] && x[0] < 0.05)
97 return x[1] < 0;
98 return false;
99 }
100 else if (0.6 < x[2] && x[2] < 0.80)
101 {
102 if (-0.05 < x[1] && x[1] < 0.05)
103 return x[0] < 0;
104 return false;
105 }
106
107 return true;
108}
109
110bool spherePlanningOnce(ConstrainedProblem &cp, enum PLANNER_TYPE planner, bool output)
111{
112 cp.setPlanner(planner, "sphere");
113
114 // Solve the problem
115 ob::PlannerStatus stat = cp.solveOnce(output, "sphere");
116
117 if (output)
118 {
119 OMPL_INFORM("Dumping problem information to `sphere_info.txt`.");
120 std::ofstream infofile("sphere_info.txt");
121 infofile << cp.type << std::endl;
122 infofile.close();
123 }
124
125 cp.atlasStats();
126
127 if (output)
128 cp.dumpGraph("sphere");
129
130 return stat;
131}
132
133bool spherePlanningBench(ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
134{
135 cp.setupBenchmark(planners, "sphere");
136 cp.runBenchmark();
137 return false;
138}
139
140bool spherePlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners,
141 struct ConstrainedOptions &c_opt, struct AtlasOptions &a_opt, bool bench)
142{
143 // Create the ambient space state space for the problem.
144 auto rvss = std::make_shared<ob::RealVectorStateSpace>(3);
145
146 ob::RealVectorBounds bounds(3);
147 bounds.setLow(-2);
148 bounds.setHigh(2);
149
150 rvss->setBounds(bounds);
151
152 // Create a shared pointer to our constraint.
153 auto constraint = std::make_shared<SphereConstraint>();
154
155 ConstrainedProblem cp(space, rvss, constraint);
156 cp.setConstrainedOptions(c_opt);
157 cp.setAtlasOptions(a_opt);
158
159 cp.css->registerProjection("sphere", std::make_shared<SphereProjection>(cp.css));
160
161 Eigen::VectorXd start(3), goal(3);
162 start << 0, 0, -1;
163 goal << 0, 0, 1;
164
165 cp.setStartAndGoalStates(start, goal);
166 cp.ss->setStateValidityChecker(obstacles);
167
168 if (!bench)
169 return spherePlanningOnce(cp, planners[0], output);
170 else
171 return spherePlanningBench(cp, planners);
172}
173
174auto help_msg = "Shows this help message.";
175auto output_msg = "Dump found solution path (if one exists) in plain text and planning graph in GraphML to "
176 "`sphere_path.txt` and `sphere_graph.graphml` respectively.";
177auto bench_msg = "Do benchmarking on provided planner list.";
178
179int main(int argc, char **argv)
180{
181 bool output, bench;
182 enum SPACE_TYPE space = PJ;
183 std::vector<enum PLANNER_TYPE> planners = {RRT};
184
185 struct ConstrainedOptions c_opt;
186 struct AtlasOptions a_opt;
187
188 po::options_description desc("Options");
189 desc.add_options()("help,h", help_msg);
190 desc.add_options()("output,o", po::bool_switch(&output)->default_value(false), output_msg);
191 desc.add_options()("bench", po::bool_switch(&bench)->default_value(false), bench_msg);
192
193 addSpaceOption(desc, &space);
194 addPlannerOption(desc, &planners);
195 addConstrainedOptions(desc, &c_opt);
196 addAtlasOptions(desc, &a_opt);
197
198 po::variables_map vm;
199 po::store(po::parse_command_line(argc, argv, desc), vm);
200 po::notify(vm);
201
202 if (vm.count("help") != 0u)
203 {
204 std::cout << desc << std::endl;
205 return 1;
206 }
207
208 return spherePlanning(output, space, planners, c_opt, a_opt, bench);
209}
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition Constraint.h:76
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
The lower and upper bounds for an Rn space.
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
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
A class to store the exit status of Planner::solve()