Loading...
Searching...
No Matches
ConstrainedPlanningTorus.py
1#!/usr/bin/env python
2
3
36
37# Author: Mark Moll
38
39from __future__ import print_function
40import argparse
41import math
42from functools import partial
43import numpy as np
44from ConstrainedPlanningCommon import *
45
46PI2 = 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
128def torusPlanningBench(cp, planners):
129 print(planners)
130 cp.setupBenchmark(planners, "torus")
131 cp.runBenchmark()
132
133
134def 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
153def torusPlanning(options):
154 # Create the ambient space state space for the problem.
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
187if __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:76
The lower and upper bounds for an Rn space.
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
Definition State.h:50
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...