Class PRMPlanner


  • public class PRMPlanner
    extends QToQPlanner
    Implements a probabilistic roadmap (PRM) planner.

    The PRMPlanner is implemented freely after [1], and has a number of options:

    - Lazy Collision Checking: Using lazy collision checking as in [2], the
    planner can be used for single as well as multiple queries.

    - Nearest Neighbor Search: The algorithm can either use a partial index
    table [3] or a simple brute force method to do the nearest neighbor
    search.

    - Shortest Path Algorithm: Using the Boost Graph Library, both A* and
    Dijkstra's Algorithm may be used for finding the shortest path.

    As default the algorithm runs with lazy collision checking, brute force
    neighbor search and with A* for shortest path search.

    As metric the PRMPlanner uses a WeightedEuclideanMetric for which it
    estimates the weights such that it provides a worst-case estimate of the
    Cartesian motion of the robots given a change in the configuration.

    Example of use
    PRMPlanner* prm = new PRMPlanner(device, workcell, state, collisionDetector, resolution); prm->setCollisionCheckingStrategy(PRMPlanner::LAZY); prm->setNeighSearchStrategy(PRMPlanner::BRUTE_FORCE); prm->setShortestPathSearchStrategy(PRMPlanner::A_STAR); prm->buildRoadmap(1000); Path path; bool pathFound = prm->query(qstart, qgoal, path, maxtime);

    [1]: Probabilistic Roadmaps for Path Planning in High-Dimensional
    Configuration Spaces, L.E. Kavraki, P. Svestka, J-C. Latombe, M.H.
    Overmars. IEEE Transactions on Robotics and Automation, Vol. 12, pages
    566-580, 1996

    [2]: Path Planning using Lazy PRM, R. Bohlin, L.E. Kavraki. Proceedings
    of the IEEE International Conference on Robotics and Automation, Vol. 1,
    pages 521-528, 2000

    [3]: On Delaying Collision Checking in PRM Planning - Application to
    Multi-Robot Coordination, G. Sanchez, J.C. Latombe. The International
    Journal of Robotics Research, Vol. 21, No. 1, pages 5-26, 2002
    • Constructor Detail

      • PRMPlanner

        public PRMPlanner​(long cPtr,
                          boolean cMemoryOwn)
    • Method Detail

      • getCPtr

        public static long getCPtr​(PRMPlanner obj)
      • buildRoadmap

        public void buildRoadmap​(long nodecount)
        Build the roadmap with the setup specified

        Parameters:
        nodecount - [in] Number of nodes to insert
      • setNeighborCount

        public void setNeighborCount​(long n)
        Sets the desired average number of neighbors. Default value is 20.

        Parameters:
        n - [in] Desired average number of neighbors
      • setNeighSearchStrategy

        public void setNeighSearchStrategy​(PRMPlanner.NeighborSearchStrategy neighborSearchStrategy)
        Sets up the nearest neighbor search strategy

        Parameters:
        neighborSearchStrategy - [in] The nearest neighbor search strategy
      • setPartialIndexTableDimensions

        public void setPartialIndexTableDimensions​(long dimensions)
        Sets up the number of dimensions for the partial index table

        This setting only applies when using the PARTIAL_INDEX_TABLE strategy for nearest
        neighbor search.

        dimensions should be within [1; _device->getDOF()]. The optimal
        value of dimensions is a tradeoff between memory usage and time.
        Selecting a value too high compared to the number of nodes in the roadmap
        may introduce an increase in time due to additional bookkeeping.

        The default value is set to 4, which is found suitable for most devices
        with 6 or 7 degrees of freedom.

        Parameters:
        dimensions - [in] Number of dimensions, which should be
      • setCollisionCheckingStrategy

        public void setCollisionCheckingStrategy​(PRMPlanner.CollisionCheckingStrategy collisionCheckingStrategy)
        Sets up the collision checking strategy

        Note: Do not call this after the buildRoadmap as it may result in paths with collisions

        Parameters:
        collisionCheckingStrategy - [in] The collision checking strategy
      • setShortestPathSearchStrategy

        public void setShortestPathSearchStrategy​(PRMPlanner.ShortestPathSearchStrategy shortestPathSearchStrategy)
        Sets up the shortest path search strategy

        Generally A* is the fastest algorithm, but given a small roadmap Dijkstra may
        perform better.

        Parameters:
        shortestPathSearchStrategy - [in] shortestPathSearchStrategy
      • setAStarTimeOutTime

        public void setAStarTimeOutTime​(double timeout)
        Sets the max time of A* before terminating and calling dijkstra

        The A* implementation in the boost graph library has a reported bug, which on
        some platforms in rare occasions may cause it to loop infinitely. If A* uses
        more than this specified time it will break off and call dijkstra instead.

        Default value for this timeout is 1second.

        Parameters:
        timeout - [in] Timeout time.
      • printTimeStats

        public void printTimeStats()
        print timing stats from previous run.