Toggle Planning: Coordinated Mapping of C-free and C-obst
Related Projects:      

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 the following sampling-based motion planning techniques.

  • Toggle PRM which performs a coordinated and simultaneous a mapping of both C-free and C-obst.
  • Toggle Local Planner extends local planning to a two dimensional subspace of C-space, namely a triangle in C-space.
  • Lazy Toggle PRM is a lazy version of Toggle PRM that is well suited for single-query scenarios.



Toggle PRM

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 provided in pseudo code. 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.

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.

Movies
  • Passage 1
  • Passage 2
  • Passage 3

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

    Movies
  • Obstacle 1
  • Obstacle 2
  • Obstacle 3

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

    Movies
  • 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.

    Movies
  • Zig2D
  • Maze2D

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

    Movies
  • Free Map
  • Block Map
  • Path

    • Toggle Local Planner

    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

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


    Related Publications

    Toggle PRM: A Coordinated Mapping of C-Free and C-Obstacle in Arbitrary Dimension, Jory Denny and Nancy M. Amato, Algorithmic Foundations of Robotics X. Springer Tracts in Advanced Robotics (STAR). Presented at the 2012 Workshop on the Algorithmic Foundations of Robotics (WAFR)., Vol: 86, pp. 297-312, Dec 2013. DOI: 10.1007/978-3-642-36279-8_18
    Keywords: Robotics, Sampling-Based Motion Planning
    Links : [Published]

    BibTex

    @article{denny-wafr-2012
    , author = "Jory Denny and Nancy M. Amato"
    , title = "Toggle PRM: A Coordinated Mapping of C-Free and C-Obstacle in Arbitrary Dimension"
    , journal = "Algorithmic Foundations of Robotics X. Springer Tracts in Advanced Robotics (STAR)"
    , volume = "86"
    , publisher = "Springer, Berlin, Heidelberg."
    , pages = "297-312"
    , year = "2013"
    , doi = "doi.org/10.1007/978-3-642-36279-8_18"
    , note = "Presented at the 2012 Workshop on the Algorithmic Foundations of Robotics (WAFR)."
    }


    Abstract

    Motion planning has received much attention over the past 40 years. More than 15 years have passed since the introduction of the successful sampling-based approach known as the Probabilistic RoadMap Method (PRM). PRM and its many variants have demonstrated great success for some high-dimensional problems, but they all have some level of difficulty in the presence of narrow passages. Recently, an approach called Toggle PRM has been introduced whose performance does not degrade for 2-dimensional problems with narrow passages. In Toggle PRM, a simultaneous, coordinated mapping of both C-free and C-obst is performed and every connection attempt augments one of the maps – either validating an edge in the current space or adding a configuration ’witnessing’ the connection failure to the other space. In this paper, we generalize Toggle PRM to d-dimensions and show that the benefits of mapping both C-free and C-obst continue to hold in higher dimensions. In particular, we introduce a new narrow passage characterization, alpha-epsilon-separable narrow passages, which describes the types of passages that can be successfully mapped by Toggle PRM. Intuitively,alpha-epsilson-separable narrow passages are arbitrarily narrow regions of C-free that separate regions of C-obst , at least locally, such as hallways in an office building. We experimentally compare Toggle PRM with other methods in a variety of scenarios with different types of narrow passages and robots with up to 16 dof.


    Lazy Toggle PRM: A Single-Query Approach to Motion Planning, Jory Denny, Kensen Shi, and Nancy M. Amato, Proc. IEEE Int. Conf. on Robotics and Automation (ICRA), pp. 2407-2414, Karlsruhe, Germany, May 2013. DOI: 10.1109/ICRA.2013.6630904
    Keywords: Robotics, Sampling-Based Motion Planning
    Links : [Published]

    BibTex

    @inproceedings{denny-icra-2013
    , author = "Jory Denny and Kensen Shi and Nancy M. Amato"
    , title = "Lazy Toggle PRM: A single-query approach to motion planning"
    , booktitle = "IEEE International Conference on Robotics and Automation"
    , location = "Karlsruhe, Germany"
    , month = "May"
    , year - "2013"
    , pages = "2407-2414"
    , doi = "10.1109/ICRA.2013.6630904"
    }


    Abstract

    Probabilistic 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. We present a strategy, Lazy Toggle PRM, 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. We demonstrate the effectiveness of Lazy Toggle PRM in a wide range of scenarios, including those with narrow passages and high descriptive complexity (e.g., those described by many triangles), concluding that it is more effective than existing methods in solving difficult queries.


    The Toggle Local Planner for sampling-based motion planning, Jory Denny and Nancy M. Amato, Proc. IEEE Int. Conf. Robot. Autom. (ICRA), pp. 1779-1786, St. Paul, MN, USA, May 2012. DOI: 10.1109/ICRA.2012.6225212
    Keywords: Robotics, Sampling-Based Motion Planning
    Links : [Published]

    BibTex

    @inproceedings{denny-icra-2012
    , author = "Jory Denny and Nancy M. Amato"
    , title = "The Toggle Local Planner for sampling-based motion planning"
    , booktitle = "IEEE International Conference on Robotics and Automation (ICRA)"
    , location = "Saint Paul, MN, USA"
    , month = "May"
    , year - "2012"
    , pages = "1779-1786"
    , doi = "10.1109/ICRA.2012.6225212"
    }


    Abstract

    Sampling-based solutions to the motion planning problem, such as the probabilistic roadmap method (PRM), have become commonplace in robotics applications. These solutions are the norm as the dimensionality of the planning space grows, i.e., d >; 5. An important primitive of these methods is the local planner, which is used for validation of simple paths between two configurations. The most common is the straight-line local planner which interpolates along the straight line between the two configurations. In this paper, we introduce a new local planner, Toggle Local Planner (Toggle LP), which extends local planning to a two-dimensional subspace of the overall planning space. If no path exists between the two configurations in the subspace, then Toggle LP is guaranteed to correctly return false. Intuitively, more connections could be found by Toggle LP than by the straight-line planner, resulting in better connected roadmaps. As shown in our results, this is the case, and additionally, the extra cost, in terms of time or storage, for Toggle LP is minimal. Additionally, our experimental analysis of the planner shows the benefit for a wide array of robots, with DOF as high as 70.


    Toggle PRM: Simultaneous mapping of C-free and C-obstacle - A study in 2D, Jory Denny and Nancy M. Amato, Proc. IEEE/RSJ Int. Conf. Intel. Rob. Syst. (IROS), pp. 2632-2639, Sep 2011. DOI: 10.1109/IROS.2011.6095102
    Keywords: Robotics, Sampling-Based Motion Planning
    Links :

    BibTex

    @proceedings{denny-iros-2011
    , author = "Jory Denny and Nancy Amato"
    , title = "Toggle PRM: Simultaneous mapping of C-free and C-obstacle - A study in 2D"
    , booktitle = "Proc. IEEE/RSJ Int. Conf. Intel. Rob. Syst. (IROS)"
    , location = "San Francisco, CA, USA"
    , month = "September"
    , year = "2011"
    , pages = "2632-2639"
    , doi = "10.1109/IROS.2011.6095102"
    }


    Abstract

    Motion planning is known to be difficult. Probabilistic planners have made great advances, but still have difficulty for problems that require planning in narrow passages or on surfaces in C space . This work proposes Toggle PRM, a new methodology for PRMs that simultaneously maps both free and obstacle space. In this paper, we focus on 2 DOF problems and show that mapping both spaces leads to increased sampling density in narrow passages and to improved overall efficiency as compared to previous sampling based approaches.