In the Toggle Methodology for solving motion planning problems, there are
two main concepts which have led to some theoretical benefits in
sampling-based motion planning. The first is mapping obstacle space, or C-obstacle.
By doing this, we create an approximate model of C-obstacle to better guide sampling
within free space, or C-free. The second is retaining witnesses from failed connection
attempts. When a connection is attempted between two nodes, either it reveals connectivity
information of their own space, or reveals important information of the opposite space.
For example, if a connection between two nodes in C-obstacle fails, that local plan entered
C-free at some point. We conjecture and prove with one of these methods, Toggle PRM, that
this point is more than likely within a narrow passage, and over time yields higher density
sampling within the narrow passage.

Through this methodology we have developed two sampling-based motion planning techniques.
The first, Toggle PRM, coordinates a mapping for both C-free and C-obst. The second, Toggle
Local Planner, extends local planning to a two dimensional subspace of C-space, namely a
triangle in C-space.

Probabilistic RoadMap (PRM) planners use randomization to
construct a graph (roadmap) in C-space in which nodes correspond to
collision-free robot configurations. An edge is formed between two nodes when they can be connected by
a simple local planner. Queries are then
processed by connecting the start and goal to the roadmap. This method has known weaknesses when it
comes to planning through narrow passages because the probability of sampling within a narrow
passage is proportional to its volume; when the passage gets more narrow, it is more
difficult to generate samples within it. Toggle PRM tries to address this issue.

There are two main novelties in Toggle PRM. Firstly, we map both C-free and C-obst space, instead of
only keeping collision-free configurations. The method "Toggles" between planning in either space.
Secondly, when a local planning attempt between two nodes of one space fails (i.e., the simple path
crosses the opposite space), we retain a witness to
the failure, and ultimately add it to the opposite space's roadmap (e.g., When a connection between
two free nodes in the roadmap fails a witness to the failure is saved in the obstacle map).
These two novelties allows Toggle PRM to be provably more efficient than uniform random sampling
when sampling in the presence of a narrow passage.

The left image shows an example of a mapping of C-free
with Toggle PRM in a simple zig-zag environment. The
middle image shows the corresponding mapping of C-obst.
The right image shows an example of a witness from a local
planning attempt. The local plan (shown in dotted line) is
between two nodes of C-obst (black) and the witness (red)
is located in C-free.

Algorithm

An overview of Toggle PRM is shown below. In
this algorithm, the major difference from traditional PRM
methods is roadmaps of both C-free and C-obst are constructed.
Additionally, the local planning method returns a
witness configuration when a connection attempt fails. Both of
these allow important information gain to occur that Toggle
PRM utilizes to map both spaces in a coordinated manner.
The algorithm takes in an environment describing a motion
planning problem, a sampler to generate nodes (e.g., uniform
random sampling), and a connector, which is a combination
of a local planner (e.g., straight line) and a nearest neighbor
finder (e.g., k-closest). We start by initializing two empty
roadmaps. While the map construction is not finished (e.g.,
until the map is a certain size, a query is solved, maximum
attempts made, etc.), then samples
are generated in both C-free and C-obst, and connections are
attempted in both maps. Witnesses returned by failed connections
are added to the opposite space’s roadmap. When
connections are attempted in C-obst, toggling the meaning of
validity is necessary for the local planner to be accurate for
C-obst. The algorithm repeats until the stopping criterion is
reached, or it is shown to be unreachable.

In the algorithm, a queue is used to incrementally add
nodes in a coordinated manner, and connect them to the roadmap. Some slight experimental speedup can
be gained by using a priority queue. When free nodes have priority over block nodes they will
be added and connected first, allowing queries to be solved more quickly.

Lastly, the algorithm needs a certain sense of balance.
If too many connections are tried, the queue will be flooded
with witnesses, and if too few connections are attempted, the algorithm will function like
traditional PRM. A good balance is achieved a node connect to its nearest neighbor, then to one node
in a separate connected component of its space's roadmap. This allows for limited connected
components and the algorithm to "learn" about the opposite space.

Separable Narrow Passages

Toggle PRM is proven to do well in two dimensions. However, one of the main questions
is where Toggle PRM performs well in higher dimensions. We have discovered and classified
a new type of narrow passage, alpha-epsilon-separable narrow passages, where Toggle PRM
can be guaranteed to outperform uniform random sampling. Here, epsilon is a parameter
describing how "separated" a narrow passage is. For example,an epsilon value of zero implies
that a narrow passage is completely of dimension n-1 where n is the full dimensionality of my
C-space, where as an epsilon of one implies it is not a narrow space at all. Alpha is a measure
of an approximated separability. For example, an alpha value of one means there is a clear division
in C-obstacle surrounding the narrow passage, whereas lower alpha values imply the degree of "local"
separability for a given point in the narrow passage. In the figure to the right, Passages (a) and (b)
are examples of two dimensional narrow passages which are alpha-epsilon-separable. However Passage (c)
is not separable because there is an n-2 dimensional narrow passage through the middle of a
cylindrical obstacle. Essentially, we say that Toggle PRM can efficiently solve narrow passages
of dimension n-1 which "locally" separate parts of C-obstacle.

Examples

The examples and experiments of Toggle PRM break down into three sections. First, we show a simple
comparison of Toggle PRM to uniform sampling in traditional PRM. Next, we show controlled narrow
passage types whereby we vary the volume of the narrow passage and surrounding obstacles. Last,
example queries are solved in both 2D and 6D example environments.

The third set of experiments mixes the various volumes. We see that Toggle PRM clearly
produces more configurations inside the narrow passage even when other methods are unable to produce
1 or 2 configurations within the narrow passages.

Probilistic RoadMaps (PRMs) are quite successful in solving complex and
high-dimensional motion planning problems. While particularly suited for
multiple-query scenarios and expansive spaces, they lack efficiency in both
solving single-query scenarios and mapping narrow spaces. Two PRM variants
separately tackle these gaps. Lazy PRM reduces the computational cost of
roadmap construction for single-query scenarios by delaying roadmap validation
until query time. Toggle PRM is well suited for mapping narrow spaces by mapping
both C-free and C-obst, which gives certain theoretical benefits. However, fully
validating the two resulting roadmaps can be costly.

Lazy Toggle PRM is a strategy for integrating these two approaches into a method
which is both suited for narrow passages and efficient single-query
calculations. This simultaneously addresses two challenges of PRMs. Like Lazy
PRM, Lazy Toggle PRM delays validation of roadmaps until query time, but if no
path is found, the algorithm augments the roadmap using the Toggle PRM
methodology. The effectiveness of Lazy Toggle PRM is demonstrated in a wide
range of scenarios, including those with narrow passages and high descriptive
complexity (e.g., those described by many triangles).

Algorithm

Lazy Toggle PRM deviates from other methods by combining
two strategies: lazy evaluation and mapping both C-free and C-obst by
utilizing witnesses from failed path validations or edge connections.
Lazy Toggle PRM iterates over three phases: lazy roadmap
construction, path validation, and witness
processing. The first phase uses lazy evaluation during standard
roadmap construction until at least one path exists in the roadmap. At this
time, the second phase will iteratively extract and validate portions of the
graph until either a path is found or no path exists. If no path exists, the
third phase employs techniques of Toggle PRM to effectively augment the
roadmap. These three phases are repeated until a successful path is found or a
maximum number of iterations is reached (implying no path is found).

Lazy roadmap construction
This phase samples configurations and lazily adds them
to without checking their validity until the start and goal are in the same
connected component of the graph (i.e., connected by a path).
Note that any level of laziness can be requested here, e.g., the sampling could
validate configurations before adding them to the roadmap and only use lazy edge
validation. Figure 2 shows a lazy roadmap in gray which my overlap C-free or
C-obst in any way.

Path Validation
The path validation phase begins when the start and goal
are in the same connected component. While a path exists from the start to goal, this phase
finds the shortest path and evaluates its
validity. Figure 3 shows an example path extracted in bold gray.
Vertices are validated before edges beginning with
portions closest to the start and goal and working towards the middle of the
path. This ordering is used because vertices and edges closer to the start or goal
are more likely to appear repeatedly in extracted paths,
and thus their validity becomes more important.
Since invalid edges are often detected quickly by coarse validations,
edges are validated in a multi-resolution fashion, i.e., a fast but coarse
validation is done for the entire path, followed by incrementally slower and
finer validations. As soon as the algorithm finds an invalid vertex or edge,
it is deleted from the free roadmap, its witness (the vertex itself or an
invalid configuration along the edge) is added to a queue used in the witness
processing phase (red node in Figure 3), and the path validation phase is repeated.
If all vertices and edges are valid, then the entire path is valid, so the
algorithm returns the path and terminates. When there are no more paths
from the start to goal, the witness processing phase begins. Figure 4 shows no
more paths existing, red shows the obstacle roadmap while blue and gray comprise
the free roadmap. Figure 6 shows a successful path extraction after a few
iterations of Lazy Toggle PRM.

Witness Processing
This phase handles witnesses yielded from the Path Validation phase.
The algorithm considers two cases, depending on the validity of the witness
being processed. In the first case where the witness is invalid,
the witness is added and connected to the obstacle roadmap just like in
Toggle PRM. This connection phase might yield additional witnesses from
failed edge connections and will also be processed in this phase.
In the second case where the witness is valid, the witness is added
to the free roadmap and connected lazily, i.e., not validated. Figure 5 shows
examples of both valid and invalid witnesses augmenting their respective
roadmaps. When all witnesses are processed or a potential path is created,
the algorithm returns to the lazy roadmap construction phase.

Examples

Lazy Toggle PRM was analyzed in many environments. The most notable examples
shown here, both the building and cityscape (Helico) environment. In the building
problem a 3DOF robot must find its way out of the building. In the cityscape, a
6DOF helicopter robot must traverse through a complex cityscape. Results show
significant speedup compared to PRM, Toggle PRM, and Lazy PRM. The below graphs
collectively show recuded collision detection calls over all methods, reduced
graph searches compared to Lazy PRM, and overall improved efficiency compared
with other approaches.

The Toggle Local Planner extends the toggle methodology of utilizing important connection information from obstacle
space in a novel local planner. In the Toggle Local Planner, a simple local plan through a two dimensional subspace of
the overall planning space is attempted. When attempting connection between two nodes s and g, the Toggle Local Planner
generates a third node n. Then the triangle sgn is the two dimensional search space. Through recursive analysis of both
free and obstacle space either a path is generated or a disconnection in this triangle is proved. Experimental analysis
of the algorithm shows improved connectivity for spaces as high as 70 DOF.