Menu
▾
▴
Re: [ompl-users] Saving and Reusing RRT* tree
|
From: Beth B. <bbo...@en...> - 2016-12-06 22:41:35
|
Mark,
I am back working on this problem of replanning. I have added my planner
(GoalTree) to ompl and the app.
My planner seems to be compiling without error, but when I run it in the
app it crashes, giving a segmentation fault error. I don't know if my
planner is not loading the saved tree or if there is some other issue.
I'm not sure how to find where in the code things are crashing.
Any help or thoughts would be appreciated.
Thanks,
Beth
On Tue, Oct 18, 2016 at 4:06 PM, Beth Boardman <bbo...@en...>
wrote:
> Mark,
>
> Thanks for quick response. with some slight changes,
> auto *addMotion = new Motion(si_);
> si_->copyState(addMotion->state, data.getVertex(j).getState());
>
> seems to work. On to figuring out the next error....
>
> Beth
>
> On Tue, Oct 18, 2016 at 3:53 PM, Mark Moll <mm...@ri...> wrote:
>
>> Hi Beth,
>>
>> Try this:
>>
>> auto *addMotion = new Motion(si_);
>> si_->copyState(motion->state, data.getVertex(j)->getState());
>>
>> Mark
>>
>>
>>
>> On Oct 18, 2016, at 4:27 PM, Beth Boardman <bbo...@en...> wrote:
>>
>> 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::Plann
>> erDataVertex&)’
>> 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/lare
>>> port/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...
>>
>>
>>
>
>
> --
> - Beth Boardman
> bbo...@uc...
>
--
- Beth Boardman
bbo...@uc...
|
