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}
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...
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
virtual void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const =0
Compute the projection as an array of double values.
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.
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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49