Home research People General Info Seminars Resources Intranet
| Algorithms & Applcations Group | Home | Research | Publications | People | Resources | News
Algorithms & Applications Group
Toggle Methodology for Motion Planning

Project Personnel:Jory Denny,Nancy Amato

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.


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.


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.

Comparison between Toggle PRM and Uniform Random Sampling Movies
Controlled narrow passages

The first set of experiments varies the narrow passage width. We see that Toggle PRM clearly produces more configurations inside the narrow passage.

The second set of experiments varies the surrounding obstacle width. We see that Toggle PRM clearly produces more configurations inside the narrow passage.

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.

  • Mix
  • Tiny
  • Example Queries

    The first set of experiments is on two narrow passages with 2DOF robots. Toggle PRM typically solves the problem with both fewer nodes, and more importantly fewer CD calls.

  • Zig2D
  • Maze2D
  • The second set of experiments is with a 6DOF robot. Again, Toggle PRM is able to outperform other methods.

    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).


    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.


    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.

    Lazy Toggle PRM: A Single-Query Approach to Motion Planning, Jory Denny, Kensen Shi, Nancy M. Amato, In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), pp. 2407 - 2414, Karlsruhe, Germany, May 2013.
    Proceedings(ps, pdf, abstract)

    Toggle PRM: A Coordinated Mapping of C-free and C-obstacle in Arbitrary Dimension, Jory Denny, Nancy M. Amato, In Proc. Int. Wkshp. on Alg. Found. of Rob. (WAFR), pp. 297-312, Boston, MA, USA, Jun 2012.
    Proceedings(ps, pdf, abstract)

    The Toggle Local Planner for Sampling-Based Motion Planning, Jory Denny, Nancy M. Amato, In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), pp. 1779-1786, St. Paul, Minnesota, USA, May 2012.
    Proceedings(ps, pdf, abstract)

    Toggle PRM: Simultaneous Mapping of C-free and C-obstacle - A Study in 2D -, Jory Denny, Nancy M. Amato, In Proc. IEEE Int. Conf. Intel. Rob. Syst. (IROS), pp. 2632 - 2639, San Francisco, California, U.S.A., Sep 2011.
    Proceedings(ps, pdf, abstract)

    Supported by NSF

    Project Alumni:Kensen Shi