ConstrainedPlanningTorus.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Mark Moll
38 
39 from __future__ import print_function
40 import argparse
41 import math
42 from functools import partial
43 import numpy as np
44 from ConstrainedPlanningCommon import *
45 
46 PI2 = 2 * math.pi
47 
48 # Torus manifold.
49 
50 
52 
53  def __init__(self, outer, inner, maze):
54  super(TorusConstraint, self).__init__(3, 1)
55  self.outer = outer
56  self.inner = inner
57  self.ppm = ou.PPM()
58  self.ppm.loadFile(maze)
59 
60  def getStartAndGoalStates(self):
61  h = self.ppm.getHeight()
62  w = self.ppm.getWidth()
63 
64  for x in range(w):
65  for y in range(h):
66  p = np.array([x / (w - 1.), y / (h - 1.)], dtype=np.float64)
67  c = self.ppm.getPixel(x, y)
68  if c.red == 255 and c.blue == 0 and c.green == 0:
69  start = self.mazeToAmbient(p)
70  elif c.green == 255 and c.blue == 0 and c.red == 0:
71  goal = self.mazeToAmbient(p)
72  return start, goal
73 
74  def function(self, x, out):
75  c = np.array([x[0], x[1], 0])
76  nrm = math.sqrt(x[0] * x[0] + x[1] * x[1])
77  if not np.isfinite(nrm) or nrm == 0:
78  nrm = 1
79  out[0] = np.linalg.norm(x - self.outer * c / nrm) - self.inner
80 
81  def jacobian(self, x, out):
82  xySquaredNorm = x[0] * x[0] + x[1] * x[1]
83  xyNorm = math.sqrt(xySquaredNorm)
84  denom = math.sqrt(x[2] * x[2] + (xyNorm - self.outer)
85  * (xyNorm - self.outer))
86  c = (xyNorm - self.outer) * (xyNorm * xySquaredNorm) / \
87  (xySquaredNorm * xySquaredNorm * denom)
88  out[0, :] = [x[0] * c, x[1] * c, x[2] / denom]
89 
90  def ambientToMaze(self, x):
91  nrm = math.sqrt(x[0] * x[0] + x[1] * x[1])
92  h = self.ppm.getHeight()
93  w = self.ppm.getWidth()
94 
95  mx = math.atan2(x[2], nrm - self.outer) / PI2
96  mx += (mx < 0)
97  mx *= h
98  my = math.atan2(x[1], x[0]) / PI2
99  my += (my < 0)
100  my *= w
101  return mx, my
102 
103  def mazeToAmbient(self, x):
104  a = x * PI2
105  b = [math.cos(a[0]), 0, math.sin(a[0])] * self.inner
106  b[0] += self.outer
107 
108  norm = math.sqrt(b[0] * b[0] + b[1] * b[1])
109  out = np.array([math.cos(a[1]), math.sin(a[1]), 0], dtype=np.float64)
110  out *= norm
111  out[2] = b[2]
112  return out
113 
114  def mazePixel(self, x):
115  h = self.ppm.getHeight()
116  w = self.ppm.getWidth()
117 
118  if x[0] < 0 or x[0] >= w or x[1] < 0 or x[1] >= h:
119  return False
120 
121  c = self.ppm.getPixel(int(x[0]), int(x[1]))
122  return not (c.red == 0 and c.blue == 0 and c.green == 0)
123 
124  def isValid(self, state):
125  return self.mazePixel(self.ambientToMaze(state))
126 
127 
128 def torusPlanningBench(cp, planners):
129  print(planners)
130  cp.setupBenchmark(planners, "torus")
131  cp.runBenchmark()
132 
133 
134 def torusPlanningOnce(cp, planner, output):
135  cp.setPlanner(planner)
136 
137  # Solve the problem
138  stat = cp.solveOnce(output, "torus")
139 
140  if output:
141  ou.OMPL_INFORM("Dumping problem information to `torus_info.txt`.")
142  with open("torus_info.txt", "w") as infofile:
143  print(cp.spaceType, file=infofile)
144 
145  cp.atlasStats()
146 
147  if output:
148  cp.dumpGraph("torus")
149 
150  return stat
151 
152 
153 def torusPlanning(options):
154  # Create the ambient space state space for the problem.
155  rvss = ob.RealVectorStateSpace(3)
156 
157  bounds = ob.RealVectorBounds(3)
158  bounds.setLow(-(options.outer + options.inner))
159  bounds.setHigh(options.outer + options.inner)
160 
161  rvss.setBounds(bounds)
162 
163  # Create our constraint.
164  constraint = TorusConstraint(options.outer, options.inner, options.maze)
165 
166  cp = ConstrainedProblem(options.space, rvss, constraint, options)
167 
168  start, goal = constraint.getStartAndGoalStates()
169  print("Start = ", start)
170  print("Goal = ", goal)
171 
172  sstart = ob.State(cp.css)
173  sgoal = ob.State(cp.css)
174  for i in range(3):
175  sstart[i] = start[i]
176  sgoal[i] = goal[i]
177  cp.setStartAndGoalStates(sstart, sgoal)
178  cp.ss.setStateValidityChecker(ob.StateValidityCheckerFn(partial(
179  TorusConstraint.isValid, constraint)))
180 
181  planners = options.planner.split(",")
182  if not options.bench:
183  torusPlanningOnce(cp, planners[0], options.output)
184  else:
185  torusPlanningBench(cp, planners)
186 
187 if __name__ == "__main__":
188  defaultMaze = join(join(dirname(__file__), "mazes"), "normal.ppm")
189  parser = argparse.ArgumentParser()
190  parser.add_argument("-o", "--output", action="store_true",
191  help="Dump found solution path (if one exists) in plain text and planning "
192  "graph in GraphML to `torus_path.txt` and `torus_graph.graphml` "
193  "respectively.")
194  parser.add_argument("--bench", action="store_true",
195  help="Do benchmarking on provided planner list.")
196  parser.add_argument("--outer", type=float, default=2,
197  help="Outer radius of torus.")
198  parser.add_argument("--inner", type=float, default=1,
199  help="Inner radius of torus.")
200  parser.add_argument("--maze", default=defaultMaze,
201  help="Filename of maze image (in .ppm format) to use as obstacles on the "
202  "surface of the torus.")
203  addSpaceOption(parser)
204  addPlannerOption(parser)
205  addConstrainedOptions(parser)
206  addAtlasOptions(parser)
207 
208  torusPlanning(parser.parse_args())
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:107
Definition of an abstract state.
Definition: State.h:113
A state space representing Rn. The distance function is the L2 norm.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
The lower and upper bounds for an Rn space.