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 
42 static const double pi2 = 2 * boost::math::constants::pi<double>();
43 
45 class TorusConstraint : public ompl::base::Constraint
46 {
47 public:
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 
152 private:
153  const std::string file_;
154  ompl::PPM ppm_;
155 };
156 
157 bool 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 
181 bool torusPlanningBench(ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
182 {
183  cp.setupBenchmark(planners, "torus");
184  cp.runBenchmark();
185  return 0;
186 }
187 
188 bool 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 
220 auto help_msg = "Shows this help message.";
221 auto 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.";
223 auto bench_msg = "Do benchmarking on provided planner list.";
224 auto outer_msg = "Outer radius of torus.";
225 auto inner_msg = "Inner radius of torus.";
226 auto maze_msg = "Filename of maze image (in .ppm format) to use as obstacles on the surface of the torus.";
227 
228 int 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 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:46
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Constraint(const unsigned int ambientDim, const unsigned int coDim, double tolerance=magic::CONSTRAINT_PROJECTION_TOLERANCE)
Constructor. The dimension of the ambient configuration space as well as the dimension of the functio...
Definition: Constraint.h:87
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:75
void jacobian(const State *state, Eigen::Ref< Eigen::MatrixXd > out) const
Compute the Jacobian of the constraint function at state. Result is returned in out, which should be allocated to size coDim by ambientDim. Default implementation performs the differentiation numerically with a seven-point central difference stencil. It is best to provide an analytic formulation.
Definition: Constraint.cpp:45
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
Definition of an abstract state.
Definition: State.h:49
The lower and upper bounds for an Rn space.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68