Re: [ompl-users] Saving and Reusing RRT* tree

[Beth B. <bbo...@en...>]

Mark,

Again, thanks for the reply. Sorry I haven't gotten back to you sooner, I
have been playing around with the code and figuring things out.
What I have so far is this (placed in the RRT* code after the tree is
initialized but before the iterative building has started):

//Problem initialized, load old Tree and reconstruct
//Load planner Data (old tree)
base::PlannerData data(si_);
// Loading an instance of PlannerData from disk.
base::PlannerDataStorage dataStorage;
dataStorage.load("myPlannerData", data);


//Reconstruct
std::vector<int> indices; //create vector to store vertex indices
indices.resize(data.numVertices()); //resize to accomadate all vertices
indices[0] = data.getStartIndex(0); //initial index is start state
int index = 0; //counter
std::vector<Motion*>       dummyTree; //dummy Tree in vector form
for (unsigned int i = 0; i < data.numVertices(); ++i)
{
    // get next vertex
    nn_->list(dummyTree);
    Motion *pmotion = dummyTree[i];

    for (unsigned int j = 0; j < data.numVertices(); ++j)
    {
        //search for all edges where pmotion->state is the "parent"

        if(edgeExists(indices[i], j)) //is the edge in the orginal tree
        {
            Motion *addMotion = new Motion(data.getVertex(j))); //create
new motion from jth vertex
            addMotion->parent = pmotion;
            addMotion->incCost = opt_->motionCost(pmotion->state,
addMotion->state);
            addMotion->cost = opt_->combineCosts(pmotion->cost,
addMotion->incCost);
            nn_->add(addMotion);
            addMotion->parent->children.push_back(addMotion);
            index++; //increase counter
            indices[index] = j; //store index of the vertex
            dummyTree[index] = addMotion;
        }
    }
}

The above is meant to load the saved tree and then reconstruct that tree.

I am getting an error at
Motion *addMotion = new Motion(data.getVertex(j))); //create new motion
from jth vertex
due to data.getVertex. My guess is getVertex is not returning a State. Any
ideas on how I would go about getting the State? This line (and subsequent
lines) should create a Motion for the vertex returned via getVertex.

This is the error I am getting
/rrt/src/GoalTree.cpp: In member function ‘virtual
ompl::base::PlannerStatus ompl::geometric::GoalTree::solve(const
ompl::base::PlannerTerminationCondition&)’:
/rrt/src/GoalTree.cpp:290:65: error: no matching function for call to
‘ompl::geometric::GoalTree::Motion::Motion(ompl::base::PlannerDataVertex&)’
                 Motion *addMotion = new Motion(data.getVertex(j)));
//create new motion from jth vertex
                                                                 ^
/rrt/src/GoalTree.cpp:290:65: note: candidates are:
In file included from planners/rrt/src/GoalTree.cpp:37:0:
/rrt/GoalTree.h:311:17: note:
ompl::geometric::GoalTree::Motion::Motion(const SpaceInformationPtr&)
                 Motion(const base::SpaceInformationPtr &si) :
                 ^
/rrt/GoalTree.h:311:17: note:   no known conversion for argument 1 from
‘ompl::base::PlannerDataVertex’ to ‘const SpaceInformationPtr& {aka const
std::shared_ptr<ompl::base::SpaceInformation>&}’
/rrt/GoalTree.h:307:19: note:
ompl::geometric::GoalTree::Motion::Motion(const
ompl::geometric::GoalTree::Motion&)
             class Motion


Sorry if the code is a bit confusing, I am not a very strong coder.

Thanks for all the help,
Beth

On Tue, Sep 27, 2016 at 5:38 PM, Mark Moll <mm...@ri...> wrote:

> Hi Beth,
>
> Sorry for the late reply. What you propose sounds reasonable. As a sanity
> check, you should make sure that planning from scratch once a new obstacle
> is introduced is indeed slower than pruning an existing tree.
>
> If you only care about RRT*, you could potentially avoid the
> RRTstar::Motion -> PlannerData -> RRTstar::Motion translation steps.
> Instead, you could add methods in your modified/derived RRT* to directly
> prune the tree. If you really want to use PlannerData, the PlannerData API
> allows you to save the cost of an edge, but it looks like RRT* doesn’t
> actually use that right now. The ordering of vertices and edges is really
> up to the planner. Look at RRTstar::getPlannerData.
>
>
> Mark
>
>
>
> On Sep 22, 2016, at 12:18 PM, Beth Boardman <bbo...@en...> wrote:
>
> Mark,
>
> Thanks for the reply.
>
> My plan is to write a new planner that does replanning. My paper that has
> the basic concept (somethings have now changed) on what I’m trying to
> implement is
> http://permalink.lanl.gov/object/tr?what=info:lanl-repo/
> lareport/LA-UR-14-24990
>
> I’ve focus on using the RRT* as the planner the robot would be using.
> Ideally the RRT* would be rooted at the goal state not the initial state,
> which is something else I know I will have to change in the code, but for
> now I can work around it.
> Once a new obstacle is introduced, the tree is pruned of all invalid
> states, which I think i have figured out how to do in the framework of the
> OMPL and RRT*.
> I would then run this new, pruned, tree back through the RRT* to continue
> building it in the space defined by the states that were removed.
>
> What I need to save from the first run of the RRT* is the full tree. All
> vertices, edges, edge costs, and possibly cost-to-go though I can work
> around that. Does PlannerData (PlannerDataStorage) save all vertices,
> edges, edge costs? I’m look through PlannerDataStorage and this looks like
> the case using PlannerDataVertex and PlannerDataEdge. How are the edge end
> points ordered, do you know? For a tree would they be ordered parent then
> child?
> Using these I should be able to reconstruct the tree in the structure used
> by the OMPL RRT*.
>
> Bottom line is I need to know how to save the graph vertices, edges, and
> edge costs/weights and then load them inside another planner.
>
> Beth
>
>
>
> On Sep 21, 2016, at 9:02 PM, Mark Moll <mm...@ri...> wrote:
>
> Hi Beth,
>
> Support for replanning is not built into OMPL. PlannerData (or
> PlannerDataStorage if you want to save to / load from disk) is probably a
> good starting point for replanning. If the environment is changing (or at
> least the model used for planning is changing), then it’s not clear how
> much information you want to retain between re-planning cycles. Extracting
> the subtree of states rooted at the last valid state along the path that
> was executed is something that has to be done separately for each planner.
>
> Perhaps you could derive a new planner from LazyPRM*, where you add a
> method to reset the validity of all states and edges to UNKNOWN when you
> wan to replan? This way you start with a roadmap of samples that probably
> still collision-free (i.e., you don’t have to sample a whole bunch of
> states that are in collision), you can easily restart at any state in the
> roadmap, more alternative routes than RRT* in case unknown obstacles
> appear, and still preserve asymptotic optimality.
>
> Mark
>
>
>
> On Sep 21, 2016, at 1:02 PM, Beth Boardman <bbo...@en...> wrote:
>
> Hi,
>
> I am looking for advice on how to save a tree, created using RRT*, and
> then be able to load the saved tree into another planner to be reused.
>
> This pertains to my work on a replanning when an unexpected obstacle shows
> up in the environment. Ideally, what I want to do is build a tree that is
> rooted at the goal (the goal part isnt important for this post). Then,
> after the robot moves to some new location it realizes its path is blocked
> by a previously unknown obstacle. The tree would then be pruned to remove
> any conflicting states and edges (this part I think I've figured out).
> Then I want to run this pruned tree back through the RRT* to fill in the
> hole that was made from pruning.
>
> I have most of this figured out, except how to either
> 1) save the tree and then open it again for reuse; OR
> 2) update the initial (or goal) state and obstacle set midway through a
> planning session
>
> It seems like PlannerData might have some of the information I need, but
> not entirely sure.
>
> Any ideas or places I should look are much appreciated. Most of my coding
> has been done in matlab, where I have a basic working version of the above
> described algorithm.
>
>
> Thanks,
> - Beth
> ------------------------------------------------------------
> ------------------
> _______________________________________________
> ompl-users mailing list
> omp...@li...
> https://lists.sourceforge.net/lists/listinfo/ompl-users
>
>
>
>
>


-- 
- Beth Boardman
bbo...@uc...