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 false;
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 static_cast<int>(torusPlanning(output, space, planners, c_opt, a_opt, bench, outer, inner, maze));
271 }
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:108
Definition of an abstract state.
Definition: State.h:114
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:119
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
Load and save .ppm files - "portable pixmap format" an image file formats designed to be easily excha...
Definition: PPM.h:79
A class to store the exit status of Planner::solve()
virtual 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,...
Definition: Constraint.cpp:45
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
The lower and upper bounds for an Rn space.