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.outerouter = outer
56  self.innerinner = inner
57  self.ppmppm = ou.PPM()
59
60  def getStartAndGoalStates(self):
61  h = self.ppmppm.getHeight()
62  w = self.ppmppm.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.ppmppm.getPixel(x, y)
68  if c.red == 255 and c.blue == 0 and c.green == 0:
69  start = self.mazeToAmbientmazeToAmbient(p)
70  elif c.green == 255 and c.blue == 0 and c.red == 0:
71  goal = self.mazeToAmbientmazeToAmbient(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.outerouter * c / nrm) - self.innerinner
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.outerouter)
85  * (xyNorm - self.outerouter))
86  c = (xyNorm - self.outerouter) * (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.ppmppm.getHeight()
93  w = self.ppmppm.getWidth()
94
95  mx = math.atan2(x[2], nrm - self.outerouter) / 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.innerinner
106  b[0] += self.outerouter
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.ppmppm.getHeight()
116  w = self.ppmppm.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.ppmppm.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.mazePixelmazePixel(self.ambientToMazeambientToMaze(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.")
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.")
201  help="Filename of maze image (in .ppm format) to use as obstacles on the "
202  "surface of the torus.")