Loading...
Searching...
No Matches
ConstrainedPlanningTorus.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 <ompl/util/PPM.h>
38#include <boost/filesystem.hpp>
39
40#include "ConstrainedPlanningCommon.h"
41
42static const double pi2 = 2 * boost::math::constants::pi<double>();
43
45class TorusConstraint : public ompl::base::Constraint
46{
47public:
48 const double outer;
49 const double inner;
50
51 TorusConstraint(const double outer, const double inner, const std::string &maze)
52 : ompl::base::Constraint(3, 1), outer(outer), inner(inner), file_(maze)
53 {
54 ppm_.loadFile(maze.c_str());
55 }
56
57 void getStartAndGoalStates(Eigen::Ref<Eigen::VectorXd> start, Eigen::Ref<Eigen::VectorXd> goal) const
58 {
59 const double h = ppm_.getHeight() - 1;
60 const double w = ppm_.getWidth() - 1;
61
62 for (unsigned int x = 0; x <= w; ++x)
63 for (unsigned int y = 0; y <= h; ++y)
64 {
65 Eigen::Vector2d p = {x / w, y / h};
66
67 auto &c = ppm_.getPixel(x, y);
68 if (c.red == 255 && c.blue == 0 && c.green == 0)
69 mazeToAmbient(p, start);
70
71 else if (c.green == 255 && c.blue == 0 && c.red == 0)
72 mazeToAmbient(p, goal);
73 }
74 }
75
76 void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
77 {
78 Eigen::Vector3d c = {x[0], x[1], 0};
79 out[0] = (x - outer * c.normalized()).norm() - inner;
80 }
81
82 void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
83 {
84 const double xySquaredNorm = x[0] * x[0] + x[1] * x[1];
85 const double xyNorm = std::sqrt(xySquaredNorm);
86 const double denom = std::sqrt(x[2] * x[2] + (xyNorm - outer) * (xyNorm - outer));
87 const double c = (xyNorm - outer) * (xyNorm * xySquaredNorm) / (xySquaredNorm * xySquaredNorm * denom);
88 out(0, 0) = x[0] * c;
89 out(0, 1) = x[1] * c;
90 out(0, 2) = x[2] / denom;
91 }
92
93 void ambientToMaze(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const
94 {
95 Eigen::Vector3d c = {x[0], x[1], 0};
96
97 const double h = ppm_.getHeight();
98 const double w = ppm_.getWidth();
99
100 out[0] = std::atan2(x[2], c.norm() - outer) / pi2;
101 out[0] += (out[0] < 0);
102 out[0] *= h;
103 out[1] = std::atan2(x[1], x[0]) / pi2;
104 out[1] += (out[1] < 0);
105 out[1] *= w;
106 }
107
108 void mazeToAmbient(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const
109 {
110 Eigen::Vector2d a = x * pi2;
111
112 Eigen::Vector3d b = {std::cos(a[0]), 0, std::sin(a[0])};
113 b *= inner;
114 b[0] += outer;
115
116 double norm = std::sqrt(b[0] * b[0] + b[1] * b[1]);
117 out << std::cos(a[1]), std::sin(a[1]), 0;
118 out *= norm;
119 out[2] = b[2];
120 }
121
122 bool mazePixel(const Eigen::Ref<const Eigen::VectorXd> &x) const
123 {
124 const double h = ppm_.getHeight();
125 const double w = ppm_.getWidth();
126
127 if (x[0] < 0 || x[0] >= w || x[1] < 0 || x[1] >= h)
128 return false;
129
130 const ompl::PPM::Color &c = ppm_.getPixel(x[0], x[1]);
131 return !(c.red == 0 && c.blue == 0 && c.green == 0);
132 }
133
134 bool isValid(const ompl::base::State *state) const
135 {
136 auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
137 Eigen::Vector2d coords;
138 ambientToMaze(x, coords);
139
140 return mazePixel(coords);
141 }
142
143 void dump(std::ofstream &file) const
144 {
145 file << outer << std::endl;
146 file << inner << std::endl;
147
148 boost::filesystem::path path(file_);
149 file << boost::filesystem::canonical(path).string() << std::endl;
150 }
151
152private:
153 const std::string file_;
154 ompl::PPM ppm_;
155};
156
157bool torusPlanningOnce(ConstrainedProblem &cp, enum PLANNER_TYPE planner, bool output)
158{
159 cp.setPlanner(planner);
160
161 // Solve the problem
162 ob::PlannerStatus stat = cp.solveOnce(output, "torus");
163
164 if (output)
165 {
166 OMPL_INFORM("Dumping problem information to `torus_info.txt`.");
167 std::ofstream infofile("torus_info.txt");
168 infofile << cp.type << std::endl;
169 dynamic_cast<TorusConstraint *>(cp.constraint.get())->dump(infofile);
170 infofile.close();
171 }
172
173 cp.atlasStats();
174
175 if (output)
176 cp.dumpGraph("torus");
177
178 return stat;
179}
180
181bool torusPlanningBench(ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
182{
183 cp.setupBenchmark(planners, "torus");
184 cp.runBenchmark();
185 return false;
186}
187
188bool torusPlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners,
189 struct ConstrainedOptions &c_opt, struct AtlasOptions &a_opt, bool bench, double outer, double inner,
190 const std::string &maze)
191{
192 // Create the ambient space state space for the problem.
193 auto rvss = std::make_shared<ob::RealVectorStateSpace>(3);
194
195 ob::RealVectorBounds bounds(3);
196 bounds.setLow(-(outer + inner));
197 bounds.setHigh(outer + inner);
198
199 rvss->setBounds(bounds);
200
201 // Create a shared pointer to our constraint.
202 auto constraint = std::make_shared<TorusConstraint>(outer, inner, maze);
203
204 ConstrainedProblem cp(space, rvss, constraint);
205 cp.setConstrainedOptions(c_opt);
206 cp.setAtlasOptions(a_opt);
207
208 Eigen::Vector3d start, goal;
209 constraint->getStartAndGoalStates(start, goal);
210
211 cp.setStartAndGoalStates(start, goal);
212 cp.ss->setStateValidityChecker(std::bind(&TorusConstraint::isValid, constraint, std::placeholders::_1));
213
214 if (!bench)
215 return torusPlanningOnce(cp, planners[0], output);
216 else
217 return torusPlanningBench(cp, planners);
218}
219
220auto help_msg = "Shows this help message.";
221auto output_msg = "Dump found solution path (if one exists) in plain text and planning graph in GraphML to "
222 "`torus_path.txt` and `torus_graph.graphml` respectively.";
223auto bench_msg = "Do benchmarking on provided planner list.";
224auto outer_msg = "Outer radius of torus.";
225auto inner_msg = "Inner radius of torus.";
226auto maze_msg = "Filename of maze image (in .ppm format) to use as obstacles on the surface of the torus.";
227
228int main(int argc, char **argv)
229{
230 bool output, bench;
231 enum SPACE_TYPE space = PJ;
232 std::vector<enum PLANNER_TYPE> planners = {RRT};
233
234 struct ConstrainedOptions c_opt;
235 struct AtlasOptions a_opt;
236
237 double outer, inner;
238 boost::filesystem::path path(__FILE__);
239 std::string maze = (path.parent_path() / "mazes/thick.ppm").string();
240
241 po::options_description desc("Options");
242 desc.add_options()("help,h", help_msg);
243 desc.add_options()("output,o", po::bool_switch(&output)->default_value(false), output_msg);
244 desc.add_options()("bench", po::bool_switch(&bench)->default_value(false), bench_msg);
245 desc.add_options()("outer", po::value<double>(&outer)->default_value(2), outer_msg);
246 desc.add_options()("inner", po::value<double>(&inner)->default_value(1), inner_msg);
247 desc.add_options()("maze,m", po::value<std::string>(&maze), maze_msg);
248
249 addSpaceOption(desc, &space);
250 addPlannerOption(desc, &planners);
251 addConstrainedOptions(desc, &c_opt);
252 addAtlasOptions(desc, &a_opt);
253
254 po::variables_map vm;
255 po::store(po::parse_command_line(argc, argv, desc), vm);
256 po::notify(vm);
257
258 if (vm.count("help"))
259 {
260 std::cout << desc << std::endl;
261 return 1;
262 }
263
264 if (maze == "")
265 {
266 OMPL_ERROR("--maze is a required.");
267 return 1;
268 }
269
270 return static_cast<int>(torusPlanning(output, space, planners, c_opt, a_opt, bench, outer, inner, maze));
271}
Load and save .ppm files - "portable pixmap format" an image file formats designed to be easily excha...
Definition: PPM.h:47
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:76
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
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49