How to load constructed roadmap by PRM? #631

mamoll · 2019-02-11T14:11:27Z

Original report by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

I am using PRM planner to construct the roadmap and I want to use it afterwards to be able to query for a path in real time. But I cannot see any API for that. Could you please tell me how to do it?

mamoll · 2019-02-11T15:00:15Z

Original comment by Mark Moll (Bitbucket: mamoll, GitHub: mamoll).

Please look at the Thunder tool. There is a ThunderLightning.cpp demo in the ompl/demos folder that shows how to use it.

mamoll · 2019-02-11T15:07:58Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

Could you please post a little working example?
What is thunder.db and how do I create it?

mamoll · 2019-02-12T09:36:27Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

I am not able to successfully reuse the roadmap (no solution is found and I am not using any state validity checking for now.)

mamoll · 2019-02-12T09:40:50Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

I was able to run PRM with thunder tool. But I cannot find a solution. The logs indicate that a graph is being constructed. I am pasting the logs below:

**Info: Initializing Thunder Framework
Info: Thunder Framework initialized.
Properties of the state space 'RealVectorSpace0'

signature: 2 1 2
dimension: 2
extent: 225.568
Call setup() before to get more information
Settings for the state space 'RealVectorSpace0'
state validity check resolution: 1%
valid segment count factor: 1
state space:
Real vector state space 'RealVectorSpace0' of dimension 2 with bounds:
min: 0 0
max: 160 159

Declared parameters:
longest_valid_segment_fraction = 0.01
valid_segment_count_factor = 1
Valid state sampler named uniform with parameters:
nr_attempts = 100
Planner PRM specs:
Multithreaded: Yes
Reports approximate solutions: Yes
Can optimize solutions: Yes
Aware of the following parameters: max_nearest_neighbors
Declared parameters for planner PRM:
max_nearest_neighbors =
Planner Thunder_Retrieve_Repair specs:
Multithreaded: No
Reports approximate solutions: Yes
Can optimize solutions: No
Aware of the following parameters:
Declared parameters for planner Thunder_Retrieve_Repair:
Start states:
RealVectorState [40 70]
Goal state, threshold = 2.22045e-16, memory address = 0x1eb0da0, state =
RealVectorState [100 100]
OptimizationObjective = nullptr
There are 0 solutions
Info: Thunder Framework: Starting solve()
Warning: State validity checker not set! No collision checking is performed
at line 63 in /home/ankur/Flytbase/scaircrow/ompl/src/ompl/base/src/SpaceInformation.cpp
Info: Calling setup() for SPARSdb
SPARSdb Debug Output:
Settings:
Max Failures: 5000
Dense Delta Fraction: 0.001
Sparse Delta Fraction: 0.05
Sparse Delta: 11.2784
Stretch Factor: 1.2
Maximum Extent: 225.568
Status:
Vertices Count: 0
Edges Count: 0
Iterations: 0
Consecutive Failures: 0
Number of guards: 0

Info: Loading database from file: /home/ankur/Flytbase/scaircrow/project/src/i-o_files/thunder.db
Info: ThunderDB: Loaded planner data with
11 vertices
10 edges
0 start states
0 goal states
Info: Adding plannerData to SPARSdb:
Info: Loading PlannerData into SPARSdb
Info: Loading vertices:
Info: Loading edges:
Info: 1 connected components
Info: Loaded database from file in 0.000135 sec
Debug: ParallelPlan.solveOne starting planner Thunder_Retrieve_Repair
Debug: ParallelPlan.solveOne starting planner PRM
Info: Looking for a node near the problem start
Info: Looking for a node near the problem goal
Info: PRM: Starting planning with 2 states already in datastructure
Debug: Starting lazy collision checking
Info: Done lazy collision checking
Info: spars::getSimilarPaths() returned true - found a solution of size 13
Debug: ParallelPlan.solveOne: Solution found by Thunder_Retrieve_Repair in 0.000138 seconds
Info: PRM: Created 1 states
Debug: ParallelPlan.solveOne: Solution found by PRM in 0.000131 seconds
Info: ParallelPlan::solve(): Solution found by one or more threads in 0.000249 seconds
Info: Thunder Solve: Possible solution found in 0.000258 seconds
Info: SimpleSetup: Path simplification took 0.000000 seconds and changed from 2 to 2 states
Info: Solution path has 2 states and was generated from planner PRM
Info: THUNDER RESULTS: From Scratch
Info: Adding path to database because best solution was not from database
Info: Performing post-processing
Debug: Expected number of necessary coverage guards is calculated to be 5 from the original path state count 2
Debug: ompl::geometric::SPARSdb: Benchmark logging enabled (slower)
Debug: Results of attempting to make insertion in SPARSdb
Debug: -------------------------------------------------------
Debug: Original length: 67.082039
Debug: New length: 67.082039
Debug: Percent increase: 0.000000 %
Info: SPARSdb now has 12 states
Info: Finished inserting experience path in 0.000126 seconds
SOLVED = 1
terminate called after throwing an instance of 'ompl::Exception'
what(): No solution path
Aborted (core dumped)
**

I am not able to understand where I went wrong. I am also pasting my code below:

#!c++

void plan(std::string base_location)
{
    std::vector<double> grid_size = {159,160};
    auto space(std::make_shared<ob::RealVectorStateSpace>(2));

    ob::RealVectorBounds bounds(2);
    bounds.setLow(0,0);
    bounds.setLow(1,0);
    bounds.setHigh(0,grid_size[1]);
    bounds.setHigh(1,grid_size[0]);

    space->setBounds(bounds);

    double start_x = 40.0;
    double start_y = 70.0;
    double goal_x = 100.0;
    double goal_y = 100.0;

    ob::ScopedState<ob::RealVectorStateSpace> start(space);
    start->values[0] = start_x;
    start->values[1] = start_y;

    ob::ScopedState<ob::RealVectorStateSpace> goal(space);
    goal->values[0] = goal_x;
    goal->values[1] = goal_y;

    ob::SpaceInformationPtr si(new ob::SpaceInformation(space));

    // ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));

    // LengthMotionValidator *motion_validator = new LengthMotionValidator(si,base_location,pdef,start_x,start_y,goal_x,goal_y);
    // ob::MotionValidatorPtr mv(motion_validator);
    // si->setMotionValidator(mv);
    // si->setup();

    // pdef->setStartAndGoalStates(start,goal);

    // auto path_planner = new og::PRM(si);
    // ob::PlannerPtr path_planner_ptr = *(new ob::PlannerPtr(path_planner));
    // path_planner->setProblemDefinition(pdef);
    // path_planner->setup();

    // std::cout<<"Path planner setup done!"<<std::endl;

    // std::cout<<"Growing the RoadMap ..."<<std::endl;
    // path_planner->growRoadmap(40.0);
    // std::cout<<"Roadmap growth complete."<<std::endl;

    // std::cout<<"Expanding Roadmap ..."<<std::endl;
    // path_planner->expandRoadmap(10.0);
    // std::cout<<"Roadmap expansion complete."<<std::endl;

    //Making a thunder tool to reuse the generated environment.
    ot::ExperienceSetupPtr expPlanner_;
    expPlanner_ = std::make_shared<ot::Thunder>(space);
    expPlanner_->setFilePath("/home/ankur/Flytbase/scaircrow/project/src/i-o_files/thunder.db");
    space->setup();
    og::SimpleSetup ss(si);

    expPlanner_->setStartAndGoalStates(start,goal);
    expPlanner_->setRepairPlanner(std::make_shared<og::PRM>(expPlanner_->getSpaceInformation()));
    expPlanner_->setPlanner(std::make_shared<og::PRM>(expPlanner_->getSpaceInformation()));
    // expPlanner_->setPlannerAllocator(ss.getPlannerAllocator());
    expPlanner_->print(std::cout);
    bool solved = expPlanner_->solve(10.);
    expPlanner_->doPostProcessing();
    std::cout<<"SOLVED = "<<solved<<std::endl;
    // expPlanner_->save();

    // std::vector<ob::PlannerDataPtr> pdatas;
    // expPlanner_->getAllPlannerDatas(pdatas);
    // og::PathGeometric path1();
    
    // ob::PlannerStatus solved = path_planner_ptr->solve(planning_time);
    // std::cout<<"QUERY STATUS = "<<solved<<std::endl;
    // std::cout<<"HAS SOLUTION = "<<pdef->hasSolution()<<std::endl;


    std::ofstream path_file(base_location+"/src/i-o_files/path.csv");

    if(solved)
    {
        auto path = ss.getSolutionPath();
        // auto pdef = ss.getProblemDefinition();
        // std::cout<<"CHECK1"<<std::endl;
        // auto path = pdef->getSolutionPath()->as<og::PathGeometric>();
        // std::cout<<"CHECK2"<<std::endl;
        // std::cout<<"PATH = "<<path<<std::endl;
        // std::cout<<"STATE COUNT = "<<path.getStateCount()<<std::endl;
        for(std::size_t path_idx = 0; path_idx < 1; path_idx++)
        {
            std::cout<<"CHECK3"<<std::endl;
            const ob::RealVectorStateSpace::StateType *point = path.getState(path_idx)->as<ob::RealVectorStateSpace::StateType>();
            double *values_ = point->values;
            double x = values_[0];
            double y = values_[1];
            std::cout<<"X, Y = "<<x<<", "<<y<<std::endl;
            std::string waypoint_str = std::to_string(x)+','+std::to_string(y);
            path_file<<waypoint_str<<std::endl;
        }
    }
    path_file.close();
}

mamoll · 2019-02-12T11:10:49Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

Also, How do I use PRM planner's member functions with Thunder. It is little bit confusing. It will be helpful if you can post a little example of these functionalities.

mamoll · 2019-02-12T13:41:05Z

Original comment by Mark Moll (Bitbucket: mamoll, GitHub: mamoll).

You can't get the path from ss which is not used by your code. Instead, replace ss.getSolutionPath() with explPlanner_->getSolutionPath() (Thunder is derived from SimpleSetup).

mamoll · 2019-02-12T14:18:00Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

Okay. And how do I integrate functionalities of PRM like constructRoadmap with the thunder tool?

mamoll · 2019-02-12T15:50:23Z

Original comment by Mark Moll (Bitbucket: mamoll, GitHub: mamoll).

The Thunder implementation is rather tied to the SPARS roadmap algorithm at the moment. By controlling the sparseness parameters of the SPARSdb object stored inside Thunder, you can get SPARS to produce PRM-like roadmaps.

mamoll · 2019-02-13T07:48:08Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

I tried it with PRM in the following manner:

Gist of what I did in the code below:

Ran the grow and expand functionalities of PRM.
Then used the same planner with the Thunder tool to solve the query.

#!c++

void plan(std::string base_location)
{
    std::vector<double> grid_size = {1599,1608};
    auto space(std::make_shared<ob::RealVectorStateSpace>(2));

    ob::RealVectorBounds bounds(2);
    bounds.setLow(0,0);
    bounds.setLow(1,0);
    bounds.setHigh(0,grid_size[1]);
    bounds.setHigh(1,grid_size[0]);

    space->setBounds(bounds);

    double start_x = 600.0;//40.0;
    double start_y = 100.0;//70.0;
    double goal_x = 50.0;//100.0;
    double goal_y = 650.0;//100.0;

    ob::ScopedState<ob::RealVectorStateSpace> start(space);
    start->values[0] = start_x;
    start->values[1] = start_y;

    ob::ScopedState<ob::RealVectorStateSpace> goal(space);
    goal->values[0] = goal_x;
    goal->values[1] = goal_y;

    // ob::SpaceInformationPtr si(new ob::SpaceInformation(space));

    // LengthMotionValidator *motion_validator = new LengthMotionValidator(si,base_location,pdef,start_x,start_y,goal_x,goal_y);
    // ob::MotionValidatorPtr mv(motion_validator);
    // si->setMotionValidator(mv);
    // si->setup();

    //Making a thunder tool to reuse the generated environment.
    ot::ExperienceSetupPtr expPlanner_;
    expPlanner_ = std::make_shared<ot::Thunder>(space);
    expPlanner_->setFilePath("/home/ankur/Flytbase/scaircrow/project/src/i-o_files/thunder2.db");
    space->setup();

    ob::SpaceInformationPtr si = expPlanner_->getSpaceInformation();
    ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));

    LengthMotionValidator *motion_validator = new LengthMotionValidator(si,base_location,pdef,start_x,start_y,goal_x,goal_y);
    ob::MotionValidatorPtr mv(motion_validator);
    si->setMotionValidator(mv);
    si->setup();

    pdef->setStartAndGoalStates(start,goal);

    // auto path_planner = new og::PRM(si);
    auto path_planner = std::make_shared<og::PRM>(si);
    ob::PlannerPtr path_planner_ptr = *(new ob::PlannerPtr(path_planner));
    path_planner->setProblemDefinition(pdef);
    path_planner->setup();

    std::cout<<"Path planner setup done!"<<std::endl;

     std::cout<<"Growing the RoadMap ..."<<std::endl;
     path_planner->growRoadmap(40.0);
     std::cout<<"Roadmap growth complete."<<std::endl;

     std::cout<<"Expanding Roadmap ..."<<std::endl;
     path_planner->expandRoadmap(14.0);
     std::cout<<"Roadmap expansion complete."<<std::endl;



    // auto path_planner = std::make_shared<og::PRM>(si);
    // set state validity checking for this space
    // expPlanner_->setStateValidityChecker(isStateValid);

    // expPlanner_->getSpaceInformation()->setStateValidityCheckingResolution(0.01);

    expPlanner_->setStartAndGoalStates(start,goal);
    expPlanner_->setPlanner(path_planner);
    expPlanner_->setRepairPlanner(path_planner);
    // expPlanner_->setPlanner(std::make_shared<og::PRM>(expPlanner_->getSpaceInformation()));
    // expPlanner_->setRepairPlanner(std::make_shared<og::PRM>(expPlanner_->getSpaceInformation()));

    // auto path_planner2 = new og::PRM(expPlanner_->getSpaceInformation());
    // path_planner2->growRoadmap(10.0);
    // path_planner->expandRoadmap(10.0);

    expPlanner_->print(std::cout);
    bool solved = expPlanner_->solve(0.01);
    expPlanner_->doPostProcessing();
    std::cout<<"SOLVED = "<<solved<<std::endl;
    
    //TO DO: Uncomment below line to save new db.
    expPlanner_->save();

    std::vector<ob::PlannerDataPtr> pdatas;
    expPlanner_->getAllPlannerDatas(pdatas);

    //Printing planner data information
    std::cout<<"PLANNER DATA"<<std::endl<<std::endl;
    std::cout<<"    Num edges = "<<pdatas[0]->numEdges()<<std::endl;
    std::cout<<"    Num vertices = "<<pdatas[0]->numVertices()<<std::endl;
    std::cout<<"    Num start vertices = "<<pdatas[0]->numStartVertices()<<std::endl;
    // og::PathGeometric path1();
    
    // ob::PlannerStatus solved = path_planner_ptr->solve(planning_time);
    // std::cout<<"QUERY STATUS = "<<solved<<std::endl;
    // std::cout<<"HAS SOLUTION = "<<pdef->hasSolution()<<std::endl;


    std::ofstream path_file(base_location+"/src/i-o_files/path.csv");

    if(solved)
    {
        auto path = expPlanner_->getSolutionPath();
        double state_count = path.getStateCount();

        for(std::size_t path_idx = 0; path_idx <state_count; path_idx++)
        {
            const ob::RealVectorStateSpace::StateType *point = path.getState(path_idx)->as<ob::RealVectorStateSpace::StateType>();
            double *values_ = point->values;
            double x = values_[0];
            double y = values_[1];
            std::cout<<"X, Y = "<<x<<", "<<y<<std::endl;
            std::string waypoint_str = std::to_string(x)+','+std::to_string(y);
            path_file<<waypoint_str<<std::endl;
        }
    }
    path_file.close();
}

It returns a valid path. And when I run it again after removing the construction phase of PRM, I can see it in the logs that data from the thunder.db is being used.
But one thing about which I am not sure is whether the PRM planner, again being used for the query phase, is using this graph data to find the path.

LOGS:

Info: ThunderDB: Loaded planner data with

36 vertices

34 edges

0 start states

0 goal states

Info: Adding plannerData to SPARSdb:

Info: Loading PlannerData into SPARSdb

Info: Loading vertices:

Info: Loading edges:

Info: 2 connected components

Info: Loaded database from file in 0.000219 sec

Debug: ParallelPlan.solveOne starting planner Thunder_Retrieve_Repair

Debug: ParallelPlan.solveOne starting planner PRM

Info: Looking for a node near the problem start

Info: Looking for a node near the problem goal

Info: PRM: Starting planning with 2 states already in datastructure

Debug: Starting lazy collision checking

Info: Done lazy collision checking

Info: spars::getSimilarPaths() returned true - found a solution of size 20

Debug: ParallelPlan.solveOne: Solution found by Thunder_Retrieve_Repair in 0.000398 seconds

Info: PRM: Created 2 states

Info: Closest path is still start and goal

Info: ParallelPlan::solve(): Solution found by one or more threads in 0.001280 seconds

Info: Thunder Solve: Possible solution found in 0.001291 seconds

Info: SimpleSetup: Path simplification took 0.004041 seconds and changed from 20 to 7 states

Info: Solution path has 7 states and was generated from planner Thunder_Retrieve_Repair

Info: THUNDER RESULTS: From Recall

Info: NOT adding path to database because SPARS already has it

Info: Performing post-processing

SOLVED = 1

Info: Saving database to file: /home/ankur/Flytbase/scaircrow/project/src/i-o_files/thunder2.db

Info: Get planner data from SPARS2 with

36 vertices

34 edges

0 start states

0 goal states

mamoll · 2019-02-13T11:23:12Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

I made one observation - when I use the above code and give expPlanner_->solve '0.1' seconds to solve, it gives me an optimized path in that little amount of time. But when I again run the above code after commenting out the growing and expanding phase of PRM but using the previously saved db, it does not give me the same path(optimized) as it did in the first iteration. From this, I conclude that the search tree growth and expansion by PRM is not saved in db and hence not used in the second iteration.
I also tried changing the planner from PRM to SPARS and then use the constructRoadmap() of SPARS for 1 minute, still thunder finds a non-optimized path in the second iteration.

mamoll · 2019-02-13T13:12:37Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

Does this tool only use the stored database for the combination of start and goal states that have already been visited in an iteration? If that is the case, then is the solution to make the stored database generic(useful for all start and goal combinations) is to iterate the above code with multiple combinations and saving them in the database?

mamoll · 2019-02-13T14:12:07Z

Original comment by Mark Moll (Bitbucket: mamoll, GitHub: mamoll).

Setting PRM as the planner in Thunder means it is used to "repair" partial solution paths found in the SPARSdb. It does not mean that Thunder will somehow inherit the roadmap from PRM. Please consult the Thunder conference paper and Dave Coleman's PhD thesis for a detailed description of how this works.

mamoll · 2019-02-13T14:36:35Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

I have an idea for storing the PRM like roadmap. If I run mutliple iterations of the above code with different set of start and goal state, it would generate a sparse representation. Does this seem valid to you?

mamoll · 2019-02-13T14:47:10Z

Original comment by Mark Moll (Bitbucket: mamoll, GitHub: mamoll).

Sure, you can do that, but unlike in SPARS, sparseness is not guaranteed / maintained. I am closing this issue since this does not relate to a bug in OMPL. Please use the ompl-users mailing list for discussion of extensions.

mamoll · 2019-02-13T14:52:16Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

Sure, I'll keep that in mind. Just one last question, do nodes from different experiences(start and goal combination) connect with each other to generate a graph like structure. I ask this because I'll be using this to find path between start and goal states that were not recorded by any of the experiences stored in the database.

mamoll · 2019-02-13T15:03:03Z

Original comment by ANKUR SATYA (Bitbucket: [Ankur Satya](https://bitbucket.org/Ankur Satya), ).

mamoll added major ompl enhancement labels Sep 20, 2019

mamoll closed this as completed Jan 10, 2021

rd-bot mentioned this issue Aug 30, 2022

Loading a previous PRMstar graph with python bindings? #936

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

How to load constructed roadmap by PRM? #631

How to load constructed roadmap by PRM? #631

mamoll commented Feb 11, 2019

mamoll commented Feb 11, 2019

mamoll commented Feb 11, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

How to load constructed roadmap by PRM? #631

How to load constructed roadmap by PRM? #631

Comments

mamoll commented Feb 11, 2019

mamoll commented Feb 11, 2019

mamoll commented Feb 11, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 12, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019

mamoll commented Feb 13, 2019