ompl-users — Users mailing list

Re: [ompl-users] Using OMPL with .map files

[Mark M. <mm...@ri...>]

Hi Mor,

In the ompl/demos directory there is a Point2DPlanning.{cpp,py} demo that shows how you can use an image in PPM format to model a 2D grid world. Perhaps this code can be modified for your needs.

Mark



> On Apr 19, 2017, at 4:38 AM, Mor Vered <mve...@gm...> wrote:
> 
> Hello All,
> 
> Would it be possible to somehow use ompl on a grid world 2D .map file ?
> 
> Thanks

[ompl-users] Using OMPL with .map files

[Mor V. <mve...@gm...>]

Hello All,

Would it be possible to somehow use ompl on a grid world 2D .map file ?

Thanks

Re: [ompl-users] Strange results from RigidBodyPlanningWithODESolverAndControls

[Andrey N. <and...@go...>]

Hi Mark,

> You may need to explicitly set the propagation step size and maybe the
> propagation duration as well. The paths well likely never look very optimal,
> since by default OMPL is simply sampling random controls.

Thank you very much for the clarification! As you said, playing with
the propagation step size, the propagation duration and map size leads
to the better solution. One of the best runs is shown on the very last
picture in my blog post about it:
http://veter-project.blogspot.com/2017/04/learning-ompl.html . I am
wondering if there are any "scientific" methods to automatically
select good values for parameters mentioned above? I mean kind of
estimation of propagation step size and duration based on state and
control space bounds or some other model properties?

> You can try the SST planner to get asymptotic optimality.

Thanks for the hint. I will definitely try it.

Best regards,
Andrey.

Re: [ompl-users] Strange results from RigidBodyPlanningWithODESolverAndControls

[Mark M. <mm...@ri...>]

Hi Andrey,

You may need to explicitly set the propagation step size and maybe the propagation duration as well. The paths well likely never look very optimal, since by default OMPL is simply sampling random controls. You can try the SST planner to get asymptotic optimality. You can also develop your own control sampler that will allow a planner to discover more reasonable trajectories.

Mark



> On Apr 15, 2017, at 12:55 PM, Andrey Nechypurenko <and...@go...> wrote:
> 
>> The second question is regarding the bounds of the state space. My map
>> has dimensions about 1000x1000. So I thought that changing the X and Y
> 
> After playing with bounds and car length, I was able to find the
> configuration where OMPL can find solution. Here is the output of
> printSettings() call:
> 
> OMPL version: 1.3.0
> Settings for the state space 'SE2CompoundSpace0'
>  - state validity check resolution: 3%
>  - valid segment count factor: 1
>  - state space:
> Compound state space 'SE2CompoundSpace0' of dimension 3 (locked) [
> Real vector state space 'RealVectorSpace1' of dimension 2 with bounds:
>  - min: 0 0
>  - max: 1000 1000
> of weight 1
> SO2 state space 'SO2Space2'
> of weight 0.5
> ]
> No registered projections
> 
> Declared parameters:
> longest_valid_segment_fraction = 0.029999999999999999
> valid_segment_count_factor = 1
> Valid state sampler named uniform with parameters:
> nr_attempts = 100
>  - control space:
> Real vector control space 'RealVectorControl[SE2CompoundSpace0]' with bounds:
>  - min: 0 -0.3
>  - max: 50 0.3
>  - can propagate backward: yes
>  - propagation step size: 0
>  - propagation duration: [0, 0]
> Warning: Assuming propagation will always have between 1 and 10 steps
>         at line 56 in
> /home/andrey/projects/piwars/ompl/src/ompl/control/src/SpaceInformation.cpp
> Warning: The propagation step size is assumed to be 42.473531
>         at line 66 in
> /home/andrey/projects/piwars/ompl/src/ompl/control/src/SpaceInformation.cpp
> Info:    No planner specified. Using default.
> Info:    KPIECE1: Attempting to use default projection.
> Info:    KPIECE1: Starting planning with 1 states already in datastructure
> Info:    ProblemDefinition: Adding approximate solution from planner KPIECE1
> Info:    KPIECE1: Created 563 states in 165 cells (55 internal + 110 external)
> Info:    Solution found in 5.002863 seconds
> 
> However, similar to the original demo, the path looks quite weird:
> https://goo.gl/KRfTm1
> 
> Obviously I am doing something wrong and would appreciate if
> experienced OMPL users can provide some hints how to improve my
> results.
> 
> Regards,
> Andrey.
> 
> ------------------------------------------------------------------------------
> Check out the vibrant tech community on one of the world's most
> engaging tech sites, Slashdot.org! http://sdm.link/slashdot
> _______________________________________________
> ompl-users mailing list
> omp...@li...
> https://lists.sourceforge.net/lists/listinfo/ompl-users
> 

Re: [ompl-users] Strange results from RigidBodyPlanningWithODESolverAndControls

[Andrey N. <and...@go...>]

> The second question is regarding the bounds of the state space. My map
> has dimensions about 1000x1000. So I thought that changing the X and Y

After playing with bounds and car length, I was able to find the
configuration where OMPL can find solution. Here is the output of
printSettings() call:

OMPL version: 1.3.0
Settings for the state space 'SE2CompoundSpace0'
  - state validity check resolution: 3%
  - valid segment count factor: 1
  - state space:
Compound state space 'SE2CompoundSpace0' of dimension 3 (locked) [
Real vector state space 'RealVectorSpace1' of dimension 2 with bounds:
  - min: 0 0
  - max: 1000 1000
 of weight 1
SO2 state space 'SO2Space2'
 of weight 0.5
]
No registered projections

Declared parameters:
longest_valid_segment_fraction = 0.029999999999999999
valid_segment_count_factor = 1
Valid state sampler named uniform with parameters:
nr_attempts = 100
  - control space:
Real vector control space 'RealVectorControl[SE2CompoundSpace0]' with bounds:
  - min: 0 -0.3
  - max: 50 0.3
  - can propagate backward: yes
  - propagation step size: 0
  - propagation duration: [0, 0]
Warning: Assuming propagation will always have between 1 and 10 steps
         at line 56 in
/home/andrey/projects/piwars/ompl/src/ompl/control/src/SpaceInformation.cpp
Warning: The propagation step size is assumed to be 42.473531
         at line 66 in
/home/andrey/projects/piwars/ompl/src/ompl/control/src/SpaceInformation.cpp
Info:    No planner specified. Using default.
Info:    KPIECE1: Attempting to use default projection.
Info:    KPIECE1: Starting planning with 1 states already in datastructure
Info:    ProblemDefinition: Adding approximate solution from planner KPIECE1
Info:    KPIECE1: Created 563 states in 165 cells (55 internal + 110 external)
Info:    Solution found in 5.002863 seconds

However, similar to the original demo, the path looks quite weird:
https://goo.gl/KRfTm1

Obviously I am doing something wrong and would appreciate if
experienced OMPL users can provide some hints how to improve my
results.

Regards,
Andrey.

[ompl-users] Strange results from RigidBodyPlanningWithODESolverAndControls

[Andrey N. <and...@go...>]

Hi OMPL Users,

I just start learning OMPL and got couple of questions. Would really
appreciate if someone could help me. I've compiled the recent version
from Git on x86 XUbuntu. The RigidBodyPlanningWithODESolverAndControls
demo application is closest to what I need so I start playing with it.

Running the demo without any changes generates the path shown on this
image: https://goo.gl/eHW4pk . It reaches the goal, but it does not
look like a "good" solution. At least I would not drive this way
myself :-) . In particular, the movements around x=0.1, y=0.05 and
similar back and forth on the other places looks kind of strange.

I assume there are many parameters the one can tweak to achieve better
result, but I did not found any hints in documentation. So my first
question is - is what I see is an expected behavior and if yes, how to
come from it to smooth trajectory to reach the goal?

The second question is regarding the bounds of the state space. My map
has dimensions about 1000x1000. So I thought that changing the X and Y
bounds for the state space and setting start and goal to let's say
50,50,0 and 500,800,0 would lead me to similar solution as with
original -1:1 bounds. However, in this case the planner did not found
the path from start to goal. It said that path was found, but the
output definitely shows that the goal is not reached. I would also
appreciate if someone can give some hints where to look at to make
this case running. My guess was that playing with
setMinMaxControlDuration() and setPropagationStepSize() functions
might be useful, but it does not lead to improvements. I have to admit
that I was just guessing parameters, so if there are explanations on
how to select appropriate ones, it would be helpful.


Thank you very much,
Andrey.

[ompl-users] Adding obstacles during run-time

[saurav a. <sa...@gm...>]

Hi,

I'm working on implementing dynamic online replanning with OMPL.

Problem: What I intend is to construct a PRM (offline phase), then solve
and get the trajectory / nodes to follow. I create open loop controls to
follow this particular trajectory and what I would like is that a new
obstacle is loaded into the environment when the robot is moving. Assume I
have a method to detect and replan around the new obstacle.

My approach:
After constructing the initial PRM and solving for a plan, while executing
the online control, I use RigidBodyGeometry::addEnvironmentMesh to add a
new obstacle, which is loaded successfully and I can see it in the graphics
display. However the stateValidityChecker does not get updated and isn't
able to detect collisions with the new object loaded into the environment.

Would you have any suggestions to get collision checking working with the
updated environment geometry?

Thanks
Saurav

[ompl-users] OMPL 1.3.0 released

[Mark M. <mm...@ri...>]

Hi all,

We are  are happy to announce version 1.3.0 of the Open Motion Planning Library (OMPL).

OMPL 1.3 contains the following main changes:

• Added new planners:
	• RRT#, a variant of RRT* with an improved convergence rate.
	• RRTX, a variant of RRT* with an improved convergence rate. Only the static part of the RRTX algorithm is implemented. Dynamical obstacles and updates of the robot position are not available in this implementation.
	• SORRT*, a variant of Informed RRT* that orders states in the subproblem that could provide a better solution by their potential solution cost.
• New refactored versions of BIT* and Informed RRT*.
• Various changes throughout to follow standard C++11 practices that improve efficiency, safety, or legibility.
• Fixes for Boost 1.63 and pygccxml 1.8.5.
• Misc. small bug fixes.

Major contributions to this release were made by:
• Jonathan Gammell, Oxford University
• Florian Hauer, Georgia Tech
• Scott Paulin, University of Canterbury, New Zealand
• Dave Coleman, University of Colorado, Boulder

The OMPL web page can be found at http://ompl.kavrakilab.org.


Regards,

Mark Moll

--
Mark Moll
Sr. Research Scientist   |   http://mmoll.rice.edu   |   713-348-5934
Rice University, Dept. of Computer Science — MS 132, PO Box 1892, Houston, TX 77251

[ompl-users] q_near and q_rand function

[Elisabetta C. <e.c...@un...>]

Dear all,

I'm a new ompl user, I want to check the connection between the sampled 
state (q_rand) and the nearest one (q_near), there is function that 
permit to have at the same time q_rand and q_near states? I have to 
introduce a cost function using both and in case the cost function will 
fail the q_rand have to eliminated.

thank in advance

Elisabetta

Re: [ompl-users] using thunder frame work under ros indigo

[MUKHTAR G. R. <gha...@gm...>]

Well, I manage to tackle my issues, just for future users that might run in
those problems I have several pointers that might be relevant.

   1. Regarding the problem with OMPL version 1.2 and its compatibility
   with ros indigo moveit I replaced the fix given here
   <https://groups.google.com/forum/#!topic/moveit-users/VPUhA46UkR4> by
   the building against a catkenized ompl for ros indigo that I found here
   <https://github.com/DavidB-CMU/OMPL_catkin_pkg/tree/master/ompl> where
   all shared pointers have been changed directly into boost::shared_ptr. But
   anyway this was not the cause of my segfault, because I kept getting it.
   2. After following the error trace I found that the genesis of the error
   comes from passing an empty ompl::base::state to some other planning
   functions (there are several of them) so I added a couple of tests in all
   relevant places and now I can run the planning for 100 thousands iteration
   and more. But I don't know why those empty states were coming, may it is my
   naive implementation of the thunder frame work (I am new to this). As
   example of the places where I inserted the test of the ompl::base::state:


   - For example the error related to FCL comes from the fact that when the
      program tries to validate a sampled state in
      (ompl::base::UniformValidStateSampler::sample(*state) ) and the state is
      empty it gives this segfault. So I added a simple if() condition
to verify
      that the state isn't NULL before calling the validation.
      - Another example is in ompl::base::PlannerInputStates::nextStart()
      after calling the problem definition member (getStartState())
sometimes it
      returns empty states, so I verify there also.
      - In general you can follow the segfault by running your node with a
      gdb debugger.

Cheers!

On Fri, Jan 20, 2017 at 3:54 PM, MUKHTAR GHANIM RIDAELDIN <
gha...@gm...> wrote:

> So regarding the question above, I found out that when I disable the
> "state validity check" part, everything works fine and the database is
> produced. And I can reuse the produced data to plan for the baxter arm
> around (look at the attached files: "generate_baxter_database.cpp" produce
> the database for the planner, and "test_baxter_database.cpp" uses it to
> move the arm via a movegroup class) but what I noticed is that this way the
> produced solutions later aren't always collision free and if I activate the
> "state validity check" in "test_baxter_database.cpp" then again after some
> iterations I got a segmentation fault.
>
> Any ideas of how to correctly set this validity checker?
>
> Cheers!
>
> On Wed, Jan 18, 2017 at 6:12 PM, MUKHTAR GHANIM RIDAELDIN <
> gha...@gm...> wrote:
>
>> *Hi all,*
>>
>> *I am trying to use the "Thunder frame work" with moveit! under ubuntu
>> 14.04 and ros indigo. So I had to install everything from source to make it
>> work. You can see all the details of problems I faced and how I worked
>> around them here
>> <https://groups.google.com/forum/#!topic/moveit-users/VPUhA46UkR4>, now
>> everything works and I am trying to build a database for my robot "Baxter"
>> using the attached code and launch file. The problem is that after some
>> iterations I always get a Segmentation fault, and it is random, for example
>> this one:*
>>
>> Program received signal SIGSEGV, Segmentation fault.
>> [Switching to Thread 0x7fffdcb73700 (LWP 13212)]
>> 0x00007ffff7ba092c in ompl_interface::ModelBasedStat
>> eSpace::copyState(ompl::base::State*, ompl::base::State const*) const ()
>> from /home/ghanim/ws_moveit/devel/.private/moveit_planners_ompl/l
>> ib/libmoveit_ompl_interface.so.0.7.3
>> (gdb) bt
>> #0  0x00007ffff7ba092c in ompl_interface::ModelBasedStat
>> eSpace::copyState(ompl::base::State*, ompl::base::State const*) const ()
>> from /home/ghanim/ws_moveit/devel/.private/moveit_planners_ompl/l
>> ib/libmoveit_ompl_interface.so.0.7.3
>> #1  0x00007ffff7555368 in ompl::base::StateSpace::cloneState(ompl::base::State
>> const*) const () from /home/ghanim/ws_moveit/devel/l
>> ib/x86_64-linux-gnu/libompl.so.12
>> #2  0x00007ffff78195b7 in ompl::geometric::SPARSdb::find
>> GraphNeighbors(ompl::base::State const*, std::vector<unsigned long,
>> std::allocator<unsigned long> >&) () from /home/ghanim/ws_moveit/devel/l
>> ib/x86_64-linux-gnu/libompl.so.12
>> #3  0x00007ffff7821062 in ompl::geometric::SPARSdb::getSimilarPaths(int,
>> ompl::base::State const*, ompl::base::State const*,
>> ompl::geometric::SPARSdb::CandidateSolution&,
>> ompl::base::PlannerTerminationCondition const&) ()
>>    from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
>> #4  0x00007ffff782419d in ompl::tools::ThunderDB::findNearestStartGoal(int,
>> ompl::base::State const*, ompl::base::State const*,
>> ompl::geometric::SPARSdb::CandidateSolution&,
>> ompl::base::PlannerTerminationCondition const&) ()
>>    from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
>> #5  0x00007ffff7726d1a in ompl::geometric::ThunderRetrie
>> veRepair::solve(ompl::base::PlannerTerminationCondition const&) () from
>> /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
>> #6  0x00007ffff7801085 in ompl::tools::ParallelPlan::solveOne(ompl::base::Planner*,
>> unsigned long, ompl::base::PlannerTerminationCondition const*) () from
>> /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
>> #7  0x00007ffff5102c30 in ?? () from /usr/lib/x86_64-linux-gnu/libs
>> tdc++.so.6
>> #8  0x00007ffff55f7184 in start_thread (arg=0x7fffdcb73700) at
>> pthread_create.c:312
>> #9  0x00007ffff4b9037d in clone () at ../sysdeps/unix/sysv/linux/x86
>> _64/clone.S:111
>>
>>
>> *This is another one:*
>>
>> Program received signal SIGSEGV, Segmentation fault.
>> 0x00007fffeeec5e96 in fcl::HierarchyTree<fcl::AABB>:
>> :bottomup(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >) () from /opt/ros/indigo/lib/libfcl.so
>> (gdb) bt
>> #0  0x00007fffeeec5e96 in fcl::HierarchyTree<fcl::AABB>:
>> :bottomup(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >) () from /opt/ros/indigo/lib/libfcl.so
>> #1  0x00007fffeeec7c65 in fcl::HierarchyTree<fcl::AABB>:
>> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >) () from /opt/ros/indigo/lib/libfcl.so
>> #2  0x00007fffeeec7c21 in fcl::HierarchyTree<fcl::AABB>:
>> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >) () from /opt/ros/indigo/lib/libfcl.so
>> #3  0x00007fffeeec7c0f in fcl::HierarchyTree<fcl::AABB>:
>> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >) () from /opt/ros/indigo/lib/libfcl.so
>> #4  0x00007fffeeec7c0f in fcl::HierarchyTree<fcl::AABB>:
>> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >) () from /opt/ros/indigo/lib/libfcl.so
>> #5  0x00007fffeeec7c0f in fcl::HierarchyTree<fcl::AABB>:
>> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >) () from /opt/ros/indigo/lib/libfcl.so
>> #6  0x00007fffeeec7c0f in fcl::HierarchyTree<fcl::AABB>:
>> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
>> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
>> > >) () from /opt/ros/indigo/lib/libfcl.so
>> #7  0x00007fffeeec8331 in fcl::HierarchyTree<fcl::AABB>:
>> :init_0(std::vector<fcl::NodeBase<fcl::AABB>*,
>> std::allocator<fcl::NodeBase<fcl::AABB>*> >&) () from
>> /opt/ros/indigo/lib/libfcl.so
>> #8  0x00007fffeeec4df3 in fcl::DynamicAABBTreeCollisionM
>> anager::registerObjects(std::vector<fcl::CollisionObject*,
>> std::allocator<fcl::CollisionObject*> > const&) () from
>> /opt/ros/indigo/lib/libfcl.so
>> #9  0x00007ffff2382d86 in collision_detection::FCLObject
>> ::registerTo(fcl::BroadPhaseCollisionManager*) () from
>> /home/ghanim/ws_moveit/devel/.private/moveit_core/lib/libmov
>> eit_collision_detection_fcl.so.0.7.3
>> #10 0x00007ffff2391cad in collision_detection::Collision
>> RobotFCL::checkSelfCollisionHelper(collision_detection::CollisionRequest
>> const&, collision_detection::CollisionResult&, moveit::core::RobotState
>> const&, collision_detection::AllowedCollisionMatrix const*) const ()
>> from /home/ghanim/ws_moveit/devel/.private/moveit_core/lib/libmov
>> eit_collision_detection_fcl.so.0.7.3
>> #11 0x000000000043ccd4 in isStateValid(ompl::base::State const*,
>> boost::shared_ptr<moveit::core::RobotModel>&,
>> boost::shared_ptr<ompl_interface::ModelBasedPlanningContext>&) ()
>> #12 0x00007ffff756c4bf in ompl::base::SpaceInformation::
>> setStateValidityChecker(std::function<bool (ompl::base::State const*)>
>> const&)::FnStateValidityChecker::isValid(ompl::base::State const*) const
>> ()
>>    from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
>> #13 0x00007ffff7583e85 in ompl::base::DiscreteMotionVali
>> dator::checkMotion(ompl::base::State const*, ompl::base::State const*)
>> const () from /home/ghanim/ws_moveit/devel/l
>> ib/x86_64-linux-gnu/libompl.so.12
>> #14 0x00007ffff7784be6 in ompl::geometric::PathSimplifie
>> r::collapseCloseVertices(ompl::geometric::PathGeometric&, unsigned int,
>> unsigned int) () from /home/ghanim/ws_moveit/devel/l
>> ib/x86_64-linux-gnu/libompl.so.12
>> #15 0x00007ffff7786933 in ompl::geometric::PathSimplifie
>> r::simplify(ompl::geometric::PathGeometric&,
>> ompl::base::PlannerTerminationCondition const&) () from
>> /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
>> #16 0x00007ffff7786c14 in ompl::geometric::PathSimplifie
>> r::simplify(ompl::geometric::PathGeometric&, double) () from
>> /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
>> #17 0x00007ffff77793b6 in ompl::geometric::SimpleSetup::simplifySolution(double)
>> () from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
>> #18 0x000000000043b6ee in main ()
>>
>>
>> *So I am wondering about what could be the problem exactly? *
>>
>> *Thanks for your help *
>>
>> *Cheers!*
>>
>
>

Re: [ompl-users] using thunder frame work under ros indigo

[MUKHTAR G. R. <gha...@gm...>]

So regarding the question above, I found out that when I disable the "state
validity check" part, everything works fine and the database is produced.
And I can reuse the produced data to plan for the baxter arm around (look
at the attached files: "generate_baxter_database.cpp" produce the database
for the planner, and "test_baxter_database.cpp" uses it to move the arm via
a movegroup class) but what I noticed is that this way the produced
solutions later aren't always collision free and if I activate the "state
validity check" in "test_baxter_database.cpp" then again after some
iterations I got a segmentation fault.

Any ideas of how to correctly set this validity checker?

Cheers!

On Wed, Jan 18, 2017 at 6:12 PM, MUKHTAR GHANIM RIDAELDIN <
gha...@gm...> wrote:

> *Hi all,*
>
> *I am trying to use the "Thunder frame work" with moveit! under ubuntu
> 14.04 and ros indigo. So I had to install everything from source to make it
> work. You can see all the details of problems I faced and how I worked
> around them here
> <https://groups.google.com/forum/#!topic/moveit-users/VPUhA46UkR4>, now
> everything works and I am trying to build a database for my robot "Baxter"
> using the attached code and launch file. The problem is that after some
> iterations I always get a Segmentation fault, and it is random, for example
> this one:*
>
> Program received signal SIGSEGV, Segmentation fault.
> [Switching to Thread 0x7fffdcb73700 (LWP 13212)]
> 0x00007ffff7ba092c in ompl_interface::ModelBasedStateSpace::copyState(ompl::base::State*,
> ompl::base::State const*) const () from /home/ghanim/ws_moveit/devel/.
> private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.0.7.3
> (gdb) bt
> #0  0x00007ffff7ba092c in ompl_interface::ModelBasedStateSpace::copyState(ompl::base::State*,
> ompl::base::State const*) const () from /home/ghanim/ws_moveit/devel/.
> private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.0.7.3
> #1  0x00007ffff7555368 in ompl::base::StateSpace::cloneState(ompl::base::State
> const*) const () from /home/ghanim/ws_moveit/devel/
> lib/x86_64-linux-gnu/libompl.so.12
> #2  0x00007ffff78195b7 in ompl::geometric::SPARSdb::
> findGraphNeighbors(ompl::base::State const*, std::vector<unsigned long,
> std::allocator<unsigned long> >&) () from /home/ghanim/ws_moveit/devel/
> lib/x86_64-linux-gnu/libompl.so.12
> #3  0x00007ffff7821062 in ompl::geometric::SPARSdb::getSimilarPaths(int,
> ompl::base::State const*, ompl::base::State const*,
> ompl::geometric::SPARSdb::CandidateSolution&, ompl::base::PlannerTerminationCondition
> const&) ()
>    from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
> #4  0x00007ffff782419d in ompl::tools::ThunderDB::findNearestStartGoal(int,
> ompl::base::State const*, ompl::base::State const*,
> ompl::geometric::SPARSdb::CandidateSolution&, ompl::base::PlannerTerminationCondition
> const&) ()
>    from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
> #5  0x00007ffff7726d1a in ompl::geometric::ThunderRetrieveRepair::solve(
> ompl::base::PlannerTerminationCondition const&) () from
> /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
> #6  0x00007ffff7801085 in ompl::tools::ParallelPlan::solveOne(ompl::base::Planner*,
> unsigned long, ompl::base::PlannerTerminationCondition const*) () from
> /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
> #7  0x00007ffff5102c30 in ?? () from /usr/lib/x86_64-linux-gnu/
> libstdc++.so.6
> #8  0x00007ffff55f7184 in start_thread (arg=0x7fffdcb73700) at
> pthread_create.c:312
> #9  0x00007ffff4b9037d in clone () at ../sysdeps/unix/sysv/linux/
> x86_64/clone.S:111
>
>
> *This is another one:*
>
> Program received signal SIGSEGV, Segmentation fault.
> 0x00007fffeeec5e96 in fcl::HierarchyTree<fcl::AABB>:
> :bottomup(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >) () from /opt/ros/indigo/lib/libfcl.so
> (gdb) bt
> #0  0x00007fffeeec5e96 in fcl::HierarchyTree<fcl::AABB>:
> :bottomup(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >) () from /opt/ros/indigo/lib/libfcl.so
> #1  0x00007fffeeec7c65 in fcl::HierarchyTree<fcl::AABB>:
> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >) () from /opt/ros/indigo/lib/libfcl.so
> #2  0x00007fffeeec7c21 in fcl::HierarchyTree<fcl::AABB>:
> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >) () from /opt/ros/indigo/lib/libfcl.so
> #3  0x00007fffeeec7c0f in fcl::HierarchyTree<fcl::AABB>:
> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >) () from /opt/ros/indigo/lib/libfcl.so
> #4  0x00007fffeeec7c0f in fcl::HierarchyTree<fcl::AABB>:
> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >) () from /opt/ros/indigo/lib/libfcl.so
> #5  0x00007fffeeec7c0f in fcl::HierarchyTree<fcl::AABB>:
> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >) () from /opt/ros/indigo/lib/libfcl.so
> #6  0x00007fffeeec7c0f in fcl::HierarchyTree<fcl::AABB>:
> :topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
> std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*>
> > >) () from /opt/ros/indigo/lib/libfcl.so
> #7  0x00007fffeeec8331 in fcl::HierarchyTree<fcl::AABB>:
> :init_0(std::vector<fcl::NodeBase<fcl::AABB>*,
> std::allocator<fcl::NodeBase<fcl::AABB>*> >&) () from
> /opt/ros/indigo/lib/libfcl.so
> #8  0x00007fffeeec4df3 in fcl::DynamicAABBTreeCollisionManage
> r::registerObjects(std::vector<fcl::CollisionObject*, std::allocator<fcl::CollisionObject*>
> > const&) () from /opt/ros/indigo/lib/libfcl.so
> #9  0x00007ffff2382d86 in collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManager*)
> () from /home/ghanim/ws_moveit/devel/.private/moveit_core/lib/
> libmoveit_collision_detection_fcl.so.0.7.3
> #10 0x00007ffff2391cad in collision_detection::CollisionRobotFCL::
> checkSelfCollisionHelper(collision_detection::CollisionRequest const&,
> collision_detection::CollisionResult&, moveit::core::RobotState const&,
> collision_detection::AllowedCollisionMatrix const*) const () from
> /home/ghanim/ws_moveit/devel/.private/moveit_core/lib/
> libmoveit_collision_detection_fcl.so.0.7.3
> #11 0x000000000043ccd4 in isStateValid(ompl::base::State const*,
> boost::shared_ptr<moveit::core::RobotModel>&, boost::shared_ptr<ompl_
> interface::ModelBasedPlanningContext>&) ()
> #12 0x00007ffff756c4bf in ompl::base::SpaceInformation::
> setStateValidityChecker(std::function<bool (ompl::base::State const*)>
> const&)::FnStateValidityChecker::isValid(ompl::base::State const*) const
> ()
>    from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
> #13 0x00007ffff7583e85 in ompl::base::DiscreteMotionValidator::checkMotion(ompl::base::State
> const*, ompl::base::State const*) const () from
> /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
> #14 0x00007ffff7784be6 in ompl::geometric::PathSimplifier::
> collapseCloseVertices(ompl::geometric::PathGeometric&, unsigned int,
> unsigned int) () from /home/ghanim/ws_moveit/devel/
> lib/x86_64-linux-gnu/libompl.so.12
> #15 0x00007ffff7786933 in ompl::geometric::PathSimplifier::simplify(ompl::geometric::PathGeometric&,
> ompl::base::PlannerTerminationCondition const&) () from
> /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
> #16 0x00007ffff7786c14 in ompl::geometric::PathSimplifier::simplify(ompl::geometric::PathGeometric&,
> double) () from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.
> so.12
> #17 0x00007ffff77793b6 in ompl::geometric::SimpleSetup::simplifySolution(double)
> () from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
> #18 0x000000000043b6ee in main ()
>
>
> *So I am wondering about what could be the problem exactly? *
>
> *Thanks for your help *
>
> *Cheers!*
>

[ompl-users] using thunder frame work under ros indigo

[MUKHTAR G. R. <gha...@gm...>]

*Hi all,*

*I am trying to use the "Thunder frame work" with moveit! under ubuntu
14.04 and ros indigo. So I had to install everything from source to make it
work. You can see all the details of problems I faced and how I worked
around them here
<https://groups.google.com/forum/#!topic/moveit-users/VPUhA46UkR4>, now
everything works and I am trying to build a database for my robot "Baxter"
using the attached code and launch file. The problem is that after some
iterations I always get a Segmentation fault, and it is random, for example
this one:*

Program received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffdcb73700 (LWP 13212)]
0x00007ffff7ba092c in
ompl_interface::ModelBasedStateSpace::copyState(ompl::base::State*,
ompl::base::State const*) const () from
/home/ghanim/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.0.7.3
(gdb) bt
#0  0x00007ffff7ba092c in
ompl_interface::ModelBasedStateSpace::copyState(ompl::base::State*,
ompl::base::State const*) const () from
/home/ghanim/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.0.7.3
#1  0x00007ffff7555368 in
ompl::base::StateSpace::cloneState(ompl::base::State const*) const () from
/home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#2  0x00007ffff78195b7 in
ompl::geometric::SPARSdb::findGraphNeighbors(ompl::base::State const*,
std::vector<unsigned long, std::allocator<unsigned long> >&) () from
/home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#3  0x00007ffff7821062 in ompl::geometric::SPARSdb::getSimilarPaths(int,
ompl::base::State const*, ompl::base::State const*,
ompl::geometric::SPARSdb::CandidateSolution&,
ompl::base::PlannerTerminationCondition const&) ()
   from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#4  0x00007ffff782419d in ompl::tools::ThunderDB::findNearestStartGoal(int,
ompl::base::State const*, ompl::base::State const*,
ompl::geometric::SPARSdb::CandidateSolution&,
ompl::base::PlannerTerminationCondition const&) ()
   from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#5  0x00007ffff7726d1a in
ompl::geometric::ThunderRetrieveRepair::solve(ompl::base::PlannerTerminationCondition
const&) () from
/home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#6  0x00007ffff7801085 in
ompl::tools::ParallelPlan::solveOne(ompl::base::Planner*, unsigned long,
ompl::base::PlannerTerminationCondition const*) () from
/home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#7  0x00007ffff5102c30 in ?? () from
/usr/lib/x86_64-linux-gnu/libstdc++.so.6
#8  0x00007ffff55f7184 in start_thread (arg=0x7fffdcb73700) at
pthread_create.c:312
#9  0x00007ffff4b9037d in clone () at
../sysdeps/unix/sysv/linux/x86_64/clone.S:111


*This is another one:*

Program received signal SIGSEGV, Segmentation fault.
0x00007fffeeec5e96 in
fcl::HierarchyTree<fcl::AABB>::bottomup(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >,
__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () from
/opt/ros/indigo/lib/libfcl.so
(gdb) bt
#0  0x00007fffeeec5e96 in
fcl::HierarchyTree<fcl::AABB>::bottomup(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >,
__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () from
/opt/ros/indigo/lib/libfcl.so
#1  0x00007fffeeec7c65 in
fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >,
__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () from
/opt/ros/indigo/lib/libfcl.so
#2  0x00007fffeeec7c21 in
fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >,
__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () from
/opt/ros/indigo/lib/libfcl.so
#3  0x00007fffeeec7c0f in
fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >,
__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () from
/opt/ros/indigo/lib/libfcl.so
#4  0x00007fffeeec7c0f in
fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >,
__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () from
/opt/ros/indigo/lib/libfcl.so
#5  0x00007fffeeec7c0f in
fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >,
__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () from
/opt/ros/indigo/lib/libfcl.so
#6  0x00007fffeeec7c0f in
fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >,
__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**,
std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () from
/opt/ros/indigo/lib/libfcl.so
#7  0x00007fffeeec8331 in
fcl::HierarchyTree<fcl::AABB>::init_0(std::vector<fcl::NodeBase<fcl::AABB>*,
std::allocator<fcl::NodeBase<fcl::AABB>*> >&) () from
/opt/ros/indigo/lib/libfcl.so
#8  0x00007fffeeec4df3 in
fcl::DynamicAABBTreeCollisionManager::registerObjects(std::vector<fcl::CollisionObject*,
std::allocator<fcl::CollisionObject*> > const&) () from
/opt/ros/indigo/lib/libfcl.so
#9  0x00007ffff2382d86 in
collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManager*)
() from
/home/ghanim/ws_moveit/devel/.private/moveit_core/lib/libmoveit_collision_detection_fcl.so.0.7.3
#10 0x00007ffff2391cad in
collision_detection::CollisionRobotFCL::checkSelfCollisionHelper(collision_detection::CollisionRequest
const&, collision_detection::CollisionResult&, moveit::core::RobotState
const&, collision_detection::AllowedCollisionMatrix const*) const () from
/home/ghanim/ws_moveit/devel/.private/moveit_core/lib/libmoveit_collision_detection_fcl.so.0.7.3
#11 0x000000000043ccd4 in isStateValid(ompl::base::State const*,
boost::shared_ptr<moveit::core::RobotModel>&,
boost::shared_ptr<ompl_interface::ModelBasedPlanningContext>&) ()
#12 0x00007ffff756c4bf in
ompl::base::SpaceInformation::setStateValidityChecker(std::function<bool
(ompl::base::State const*)>
const&)::FnStateValidityChecker::isValid(ompl::base::State const*) const ()
   from /home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#13 0x00007ffff7583e85 in
ompl::base::DiscreteMotionValidator::checkMotion(ompl::base::State const*,
ompl::base::State const*) const () from
/home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#14 0x00007ffff7784be6 in
ompl::geometric::PathSimplifier::collapseCloseVertices(ompl::geometric::PathGeometric&,
unsigned int, unsigned int) () from
/home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#15 0x00007ffff7786933 in
ompl::geometric::PathSimplifier::simplify(ompl::geometric::PathGeometric&,
ompl::base::PlannerTerminationCondition const&) () from
/home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#16 0x00007ffff7786c14 in
ompl::geometric::PathSimplifier::simplify(ompl::geometric::PathGeometric&,
double) () from
/home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#17 0x00007ffff77793b6 in
ompl::geometric::SimpleSetup::simplifySolution(double) () from
/home/ghanim/ws_moveit/devel/lib/x86_64-linux-gnu/libompl.so.12
#18 0x000000000043b6ee in main ()


*So I am wondering about what could be the problem exactly? *

*Thanks for your help *

*Cheers!*

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

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

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

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

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

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::
> 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/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...

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

[Mark M. <mm...@ri...>]

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::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... <mailto: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... <mailto: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 <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... <mailto: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... <mailto: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... <mailto:omp...@li...>
>>>> https://lists.sourceforge.net/lists/listinfo/ompl-users <https://lists.sourceforge.net/lists/listinfo/ompl-users>
>>> 
>> 
> 
> 
> 
> 
> -- 
> - Beth Boardman
> bbo...@uc... <mailto:bbo...@uc...>

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

Re: [ompl-users] Re-using data from previous planning requests.

[Yoann S. <ys...@la...>]

Hi Mark, hi Dave,

Mark, thank you for your answers. I like your idea. In a first time, I 
will use the RRT* in order to have a better database. About CForest, I 
need to read the scientific article and the explanation on the OMPL 
website. I will work on this after I have worked on the first idea. I 
will keep you informed.

Dave, thank you for your answers. I have written my answers below.

> > So it is possible to force the PFS thread to work alone (to focuse 
> on the learing) some time in order to build a more "optimal" database? 
> If yes, Is it possible to do this offline?
>
> In my current implementation, no - but you could add this feature. 
> Since your start and goal states are the same, should I assume you are 
> referring to changing obstacle environments? I think one key detail 
> you are missing is that even recalled paths are run through a smoother 
> which, in OMPL's case has a random aspect to it, and this smoother 
> will have varying solution paths given changing obstacle environments. 
> This smoothed path is then re-inserted back into the experience 
> database and will often improve/enrich the future solution quality.
In a first time the environment is static, but later moving obstacles 
can be taken into account. Indeed, I missed the point about the 
smoother. Ok, I will add this feature in the algorithm in order to focus 
on the learning phase.

> >The database is *guaranteed* not to grow indefinitely.
>
> This is true in theory, but I've found implementation issues in how 
> SPARS's smooths and inserts quality paths around corners, such that in 
> my testing it will continue to slowly grow indefinitely after hours of 
> running on the simplest 2D toy problems. I've had some luck working 
> around this through using variable clearance around obstacles.
Ok, just to be sure. You applied a kind of algorithm which removes 
states which are too close from any obstacles. Is it right? If it is the 
case, the database can continue to grow but more slowly.

> > Is it possible to have more documentation about the implementation in Moveit?
>
> The setup I used was complicated and never merged into the main 
> MoveIt! codebase - it exists somewhere in the commit history of my 
> forks. I did create a PR for the key feature necessary to use 
> experience planning in MoveIt! - the ability to re-use a planning 
> context such that you don't have to load from file the experience 
> database every problem. But it was never merged in due to 
> a dormant period in MoveIt!: 
> https://github.com/ros-planning/moveit_ros/pull/477 
> <https://github.com/ros-planning/moveit_ros/pull/477>
Ok, thank you. The PR will be useful for me. My idea about the 
implementation of the experience based planner in Moveit! is not fully 
clear yet, but your works will be very helpful.

Thank you again,
Regards,
Yoann.



>
> On Mon, Oct 3, 2016 at 9:13 PM, Mark Moll <mm...@ri... 
> <mailto:mm...@ri...>> wrote:
>
>     Hi Yoann,
>
>     You can set the "repair” planner
>     <http://ompl.kavrakilab.org/classompl_1_1tools_1_1Thunder.html#ae2206a7c1f4a324519d443ce1acad468> in
>     Thunder to RRT* or some other optimizing planner. By default RRT*
>     will always use all the time you give it to compute a more optimal
>     solution, but you can change this by specifying a optimality
>     threshold
>     <http://ompl.kavrakilab.org/classompl_1_1base_1_1OptimizationObjective.html#a1f133b98a94cb5b20c4b1a3a998aa7e9> for
>     your optimization objective.
>
>     A completely wild idea would be to use CForest
>     <http://ompl.kavrakilab.org/classompl_1_1geometric_1_1CForest.html> with
>     Thunder as one of your planners and RRT* as another one of your
>     planners (or perhaps use a few RRT* instance). If you try that and
>     it works, let me know :-).
>
>     Mark
>
>
>
>
>>     On Oct 3, 2016, at 1:55 AM, Yoann Solana <ys...@la...
>>     <mailto:ys...@la...>> wrote:
>>
>>     Hi Mark,
>>
>>
>>     Thank you for your answers.
>>
>>     I have completed and reformulated the remaining question, see below.
>>
>>>         _ Our experimentation consists in planning motion for
>>>         several objects to manipulate. During the experimentation
>>>         the start and goal poses of the objects will be always the
>>>         same for each object. The data base of Thunder grows at each
>>>         time the planner from scratch (PFS) find a solution before
>>>         the retreive-repair planner (RR). However, when the database
>>>         contains the path, the PFS has a low probability to find a
>>>         path first and maybe to improve the quality of the database
>>>         in order to have more optimal path for future planning
>>>         request. So, is it possible to force the PFS to generate
>>>         trajectory in order to improve the database?
>>>
>>         I am a little confused. PFS and RR run in parallel threads.
>>         Whichever finds a solution first will terminate the other thread.
>>
>>     What I mean is, once a path exists (or almost) in the database,
>>     the probability for the PFS thread to return a solution first is
>>     lower than this one for the RR thread. Because a single planning
>>     request takes a lot of time (20-30s) for this problem. Moreover,
>>     in this case the database has a low probability to be enriched
>>     and improved.
>>     So it is possible to force the PFS thread to work alone (to
>>     focuse on the learing) some time in order to build a more
>>     "optimal" database? If yes, Is it possible to do this offline?
>>
>>     Thank you again and congratulation to the whole team, OMPL is a
>>     nice library and well documented.
>>     Regards,
>>     Yoann.
>>
>>
>>
>>
>>
>>
>>     On 09/30/2016 06:40 PM, Mark Moll wrote:
>>>     Hi Yoann,
>>>
>>>     My answers are below inline.
>>>
>>>     Mark
>>>
>>>
>>>
>>>>     On Sep 28, 2016, at 7:23 AM, Yoann Solana <ys...@la...
>>>>     <mailto:ys...@la...>> wrote:
>>>>
>>>>     Hi all,
>>>>
>>>>     We are working on the implementation of research work about
>>>>     dual-arm manipulation.Our goal is to develop in Moveit a new
>>>>     functionality able to execute complex closed kinematics chain
>>>>     motions in a cluttered environment. For now, we are able to
>>>>     plan motion with the RRT planner. However, the workspace is
>>>>     very complex, so that planning requests are almost impossible
>>>>     to solve in a short time (1-2s). Therefore, we want to take
>>>>     advantage of multi-query planners like PRM or SRS to compute
>>>>     offline a roadmap of the workspace in order to efficiently
>>>>     perform a motion request later.
>>>>
>>>>     In previous posts
>>>>     (https://sourceforge.net/p/ompl/mailman/message/32657097/
>>>>     <https://sourceforge.net/p/ompl/mailman/message/32657097/>,
>>>>     https://sourceforge.net/p/ompl/mailman/message/35074389/
>>>>     <https://sourceforge.net/p/ompl/mailman/message/35074389/>),
>>>>     Mark Moll explained that is was not possible to restart the
>>>>     planner using a previously constructed roadmap without a lot of
>>>>     work in the code. However, he proposed experienced based planners.
>>>>
>>>>     I read the article "Experience-Based planning with Sparce
>>>>     Roadmap Spanners" and the LightningThunder demo. I'm thinking
>>>>     about using the Thunder framework inside Moveit, but I need to
>>>>     be sure if it will be in accordance to our requirements.
>>>>     I have several question about the Thunder framework:
>>>>     _ The exp-based planning is suited for robots with large number
>>>>     of invariant constraints. So, for our case, additional
>>>>     constraints generated by closed kinematic chains can be easily
>>>>     encoded in the experience database. This will lead to have a
>>>>     robust database. Am I right?
>>>>
>>>
>>>     Yes.
>>>>
>>>>     _ I see the size of the data base have a kind of theoretical
>>>>     limit. However, can I be sure that the database will have a
>>>>     size limit, even after months of running?
>>>>
>>>
>>>     The database is *guaranteed* not to grow indefinitely. This is
>>>     because it is using the spanner properties of the underlying
>>>     SPARS2-based planner. By changing the planner parameters you can
>>>     control how sparse the roadmap (and how large the database) will
>>>     be. We can’t predict what the size of the database will be in
>>>     the limit, though.
>>>>
>>>>     _ Our experimentation consists in planning motion for several
>>>>     objects to manipulate. During the experimentation the start and
>>>>     goal poses of the objects will be always the same for each
>>>>     object. The data base of Thunder grows at each time the planner
>>>>     from scratch (PFS) find a solution before the retreive-repair
>>>>     planner (RR). However, when the database contains the path, the
>>>>     PFS has a low probability to find a path first and maybe to
>>>>     improve the quality of the database in order to have more
>>>>     optimal path for future planning request. So, is it possible to
>>>>     force the PFS to generate trajectory in order to improve the
>>>>     database?
>>>>
>>>
>>>     I am a little confused. PFS and RR run in parallel threads.
>>>     Whichever finds a solution first will terminate the other thread.
>>>>
>>>>     _ Last question, I see the experimentation in the article was
>>>>     perform with Moveit/OMPL. Is it possible to have more
>>>>     documentation about the implementation in Moveit?
>>>>
>>>     That’s a great idea. Dave Coleman (cc-ed) has written most of
>>>     this code. I hope he can find some time to write some
>>>     instructions on how to use Thunder/Lightning with MoveIt!.
>>>
>>>
>>>
>>
>
>
>
>
> ------------------------------------------------------------------------------
> Check out the vibrant tech community on one of the world's most
> engaging tech sites, SlashDot.org! http://sdm.link/slashdot
>
>
> _______________________________________________
> ompl-users mailing list
> omp...@li...
> https://lists.sourceforge.net/lists/listinfo/ompl-users

Re: [ompl-users] Re-using data from previous planning requests.

[Dave C. <Dav...@Co...>]

Thanks Mark for your answers, sorry for being late to the discussion. I
wanted to add some thoughts:

> So it is possible to force the PFS thread to work alone (to focuse on the
learing) some time in order to build a more "optimal" database? If yes, Is
it possible to do this offline?

In my current implementation, no - but you could add this feature. Since
your start and goal states are the same, should I assume you are referring
to changing obstacle environments? I think one key detail you are missing
is that even recalled paths are run through a smoother which, in OMPL's
case has a random aspect to it, and this smoother will have varying
solution paths given changing obstacle environments. This smoothed path is
then re-inserted back into the experience database and will often
improve/enrich the future solution quality.

> The database is *guaranteed* not to grow indefinitely.

This is true in theory, but I've found implementation issues in how SPARS's
smooths and inserts quality paths around corners, such that in my testing
it will continue to slowly grow indefinitely after hours of running on the
simplest 2D toy problems. I've had some luck working around this through
using variable clearance around obstacles.

> Is it possible to have more documentation about the implementation in
Moveit?

The setup I used was complicated and never merged into the main MoveIt!
codebase - it exists somewhere in the commit history of my forks. I did
create a PR for the key feature necessary to use experience planning in
MoveIt! - the ability to re-use a planning context such that you don't have
to load from file the experience database every problem. But it was never
merged in due to a dormant period in MoveIt!: https://github.com/
ros-planning/moveit_ros/pull/477

- davetcoleman

On Mon, Oct 3, 2016 at 9:13 PM, Mark Moll <mm...@ri...> wrote:

> Hi Yoann,
>
> You can set the "repair” planner
> <http://ompl.kavrakilab.org/classompl_1_1tools_1_1Thunder.html#ae2206a7c1f4a324519d443ce1acad468> in
> Thunder to RRT* or some other optimizing planner. By default RRT* will
> always use all the time you give it to compute a more optimal solution, but
> you can change this by specifying a optimality threshold
> <http://ompl.kavrakilab.org/classompl_1_1base_1_1OptimizationObjective.html#a1f133b98a94cb5b20c4b1a3a998aa7e9> for
> your optimization objective.
>
> A completely wild idea would be to use CForest
> <http://ompl.kavrakilab.org/classompl_1_1geometric_1_1CForest.html> with
> Thunder as one of your planners and RRT* as another one of your planners
> (or perhaps use a few RRT* instance). If you try that and it works, let me
> know :-).
>
> Mark
>
>
>
>
> On Oct 3, 2016, at 1:55 AM, Yoann Solana <ys...@la...> wrote:
>
> Hi Mark,
>
>
> Thank you for your answers.
> I have completed and reformulated the remaining question, see below.
>
> _ Our experimentation consists in planning motion for several objects to
> manipulate. During the experimentation the start and goal poses of the
> objects will be always the same for each object. The data base of Thunder
> grows at each time the planner from scratch (PFS) find a solution before
> the retreive-repair planner (RR). However, when the database contains the
> path, the PFS has a low probability to find a path first and maybe to
> improve the quality of the database in order to have more optimal path for
> future planning request. So, is it possible to force the PFS to generate
> trajectory in order to improve the database?
>
> I am a little confused. PFS and RR run in parallel threads. Whichever
> finds a solution first will terminate the other thread.
>
> What I mean is, once a path exists (or almost) in the database, the
> probability for the PFS thread to return a solution first is lower than
> this one for the RR thread. Because a single planning request takes a lot
> of time (20-30s) for this problem. Moreover, in this case the database has
> a low probability to be enriched and improved.
> So it is possible to force the PFS thread to work alone (to focuse on the
> learing) some time in order to build a more "optimal" database? If yes, Is
> it possible to do this offline?
>
> Thank you again and congratulation to the whole team, OMPL is a nice
> library and well documented.
> Regards,
> Yoann.
>
>
>
>
>
>
> On 09/30/2016 06:40 PM, Mark Moll wrote:
>
> Hi Yoann,
>
> My answers are below inline.
>
> Mark
>
>
>
> On Sep 28, 2016, at 7:23 AM, Yoann Solana <ys...@la...> wrote:
>
> Hi all,
>
> We are working on the implementation of research work about dual-arm
> manipulation. Our goal is to develop in Moveit a new functionality able
> to execute complex closed kinematics chain motions in a cluttered
>  environment. For now, we are able to plan motion with the RRT planner.
> However, the workspace is very complex, so that planning requests are
> almost impossible to solve in a short time (1-2s). Therefore, we want to
> take advantage of multi-query planners like PRM or SRS to compute offline a
> roadmap of the workspace in order to efficiently perform a motion request
> later.
>
> In previous posts (https://sourceforge.net/p/omp
> l/mailman/message/32657097/, https://sourceforge.net/p/ompl/
> mailman/message/35074389/), Mark Moll explained that is was not possible
> to restart the planner using a previously constructed  roadmap without a
> lot of work in the code. However, he proposed experienced based planners.
>
> I read the article "Experience-Based planning with Sparce Roadmap
> Spanners" and the LightningThunder demo. I'm thinking about using the
> Thunder framework inside Moveit, but I need to be sure if it will be in
> accordance to our requirements.
> I have several question about the Thunder framework:
> _ The exp-based planning is suited for robots with large number of
> invariant constraints. So, for our case, additional constraints generated
> by closed kinematic chains can be easily encoded in the experience
> database. This will lead to have a robust database. Am I right?
>
>
> Yes.
>
> _ I see the size of the data base have a kind of theoretical limit.
> However, can I be sure that the database will have a size limit, even after
> months of running?
>
>
> The database is *guaranteed* not to grow indefinitely. This is because it
> is using the spanner properties of the underlying SPARS2-based planner. By
> changing the planner parameters you can control how sparse the roadmap (and
> how large the database) will be. We can’t predict what the size of the
> database will be in the limit, though.
>
> _ Our experimentation consists in planning motion for several objects to
> manipulate. During the experimentation the start and goal poses of the
> objects will be always the same for each object. The data base of Thunder
> grows at each time the planner from scratch (PFS) find a solution before
> the retreive-repair planner (RR). However, when the database contains the
> path, the PFS has a low probability to find a path first and maybe to
> improve the quality of the database in order to have more optimal path for
> future planning request. So, is it possible to force the PFS to generate
> trajectory in order to improve the database?
>
>
> I am a little confused. PFS and RR run in parallel threads. Whichever
> finds a solution first will terminate the other thread.
>
> _ Last question, I see the experimentation in the article was perform with
> Moveit/OMPL. Is it possible to have more documentation about the
> implementation in Moveit?
>
> That’s a great idea. Dave Coleman (cc-ed) has written most of this code. I
> hope he can find some time to write some instructions on how to use
> Thunder/Lightning with MoveIt!.
>
>
>
>
>
>

Re: [ompl-users] Re-using data from previous planning requests.

[Mark M. <mm...@ri...>]

Hi Yoann,

You can set the "repair” planner <http://ompl.kavrakilab.org/classompl_1_1tools_1_1Thunder.html#ae2206a7c1f4a324519d443ce1acad468> in Thunder to RRT* or some other optimizing planner. By default RRT* will always use all the time you give it to compute a more optimal solution, but you can change this by specifying a optimality threshold <http://ompl.kavrakilab.org/classompl_1_1base_1_1OptimizationObjective.html#a1f133b98a94cb5b20c4b1a3a998aa7e9> for your optimization objective. 

A completely wild idea would be to use CForest <http://ompl.kavrakilab.org/classompl_1_1geometric_1_1CForest.html> with Thunder as one of your planners and RRT* as another one of your planners (or perhaps use a few RRT* instance). If you try that and it works, let me know :-).

Mark



> On Oct 3, 2016, at 1:55 AM, Yoann Solana <ys...@la...> wrote:
> 
> Hi Mark, 
> 
> 
> Thank you for your answers. 
> I have completed and reformulated the remaining question, see below.
>> _ Our experimentation consists in planning motion for several objects to manipulate. During the experimentation the start and goal poses of the objects will be always the same for each object. The data base of Thunder                           grows at each time the planner from scratch (PFS) find a solution before the retreive-repair planner (RR). However, when the database contains the path, the PFS has a low probability to find a path first and maybe to improve the quality of the database in order to have more optimal path for future planning request. So, is it possible to force the PFS to generate trajectory in order to improve the database?
>> 
> I am a little confused. PFS and RR run in parallel threads. Whichever finds a solution first will terminate the other thread.
> What I mean is, once a path exists (or almost) in the database, the probability for the PFS thread to return a solution first is lower than this one for the RR thread. Because a single planning request takes a lot of time (20-30s) for this problem. Moreover, in this case the database has a low probability to be enriched and improved. 
> So it is possible to force the PFS thread to work alone (to focuse on the learing) some time in order to build a more "optimal" database? If yes, Is it possible to do this offline?
> 
> Thank you again and congratulation to the whole team, OMPL is a nice library and well documented.
> Regards,
> Yoann.
> 
> 
> 
> 
> 
> 
> On 09/30/2016 06:40 PM, Mark Moll wrote:
>> Hi Yoann,
>> 
>> My answers are below inline.
>> 
>> Mark
>> 
>> 
>> 
>>> On Sep 28, 2016, at 7:23 AM, Yoann Solana <ys...@la... <mailto:ys...@la...>> wrote:
>>> 
>>> Hi all,
>>> 
>>> We are working on the implementation of research work about dual-arm manipulation. Our goal is to develop in Moveit a new functionality able to execute complex closed kinematics chain motions in a cluttered environment. For now, we are able to plan motion with the RRT planner. However, the workspace is very complex, so that planning requests are almost impossible to solve in a short time (1-2s). Therefore, we want to take advantage of multi-query planners like PRM or SRS to compute offline a roadmap of the workspace in order to efficiently perform a motion request later.
>>> 
>>> In previous posts (https://sourceforge.net/p/ompl/mailman/message/32657097/ <https://sourceforge.net/p/ompl/mailman/message/32657097/>, https://sourceforge.net/p/ompl/mailman/message/35074389/ <https://sourceforge.net/p/ompl/mailman/message/35074389/>), Mark Moll explained that is was not possible to restart the planner using a previously constructed  roadmap without a lot of work in the code. However, he proposed experienced based planners. 
>>> 
>>> I read the article "Experience-Based planning with Sparce Roadmap Spanners" and the LightningThunder demo. I'm thinking about using the Thunder framework inside Moveit, but I need to be sure if it will be in accordance to our requirements.
>>> I have several question about the Thunder framework:
>>> _ The exp-based planning is suited for robots with large number of invariant constraints. So, for our case, additional constraints generated by closed kinematic chains can be easily encoded in the experience database. This will lead to have a robust database. Am I right?
>>> 
>> 
>> Yes.
>>> _ I see the size of the data base have a kind of theoretical limit. However, can I be sure that the database will have a size limit, even after months of running?
>>> 
>> 
>> The database is *guaranteed* not to grow indefinitely. This is because it is using the spanner properties of the underlying SPARS2-based planner. By changing the planner parameters you can control how sparse the roadmap (and how large the database) will be. We can’t predict what the size of the database will be in the limit, though.
>>> _ Our experimentation consists in planning motion for several objects to manipulate. During the experimentation the start and goal poses of the objects will be always the same for each object. The data base of Thunder grows at each time the planner from scratch (PFS) find a solution before the retreive-repair planner (RR). However, when the database contains the path, the PFS has a low probability to find a path first and maybe to improve the quality of the database in order to have more optimal path for future planning request. So, is it possible to force the PFS to generate trajectory in order to improve the database?
>>> 
>> 
>> I am a little confused. PFS and RR run in parallel threads. Whichever finds a solution first will terminate the other thread.
>>> _ Last question, I see the experimentation in the article was perform with Moveit/OMPL. Is it possible to have more documentation about the implementation in Moveit?
>>> 
>> That’s a great idea. Dave Coleman (cc-ed) has written most of this code. I hope he can find some time to write some instructions on how to use Thunder/Lightning with MoveIt!.
>> 
>> 
>> 
> 

Re: [ompl-users] Re-using data from previous planning requests.

[Manuel K. <kas...@po...>]

Hi,

there is a possibility to set, if there should be a planning from
scratch and/or planning from recall. Just look into the thunder or
lightning files, there you will find the function!

lg Manu

Re: [ompl-users] Re-using data from previous planning requests.

[Yoann S. <ys...@la...>]

Hi Mark,


Thank you for your answers.

I have completed and reformulated the remaining question, see below.

>     _ Our experimentation consists in planning motion for several
>     objects to manipulate. During the experimentation the start and
>     goal poses of the objects will be always the same for each object.
>     The data base of Thunder grows at each time the planner from
>     scratch (PFS) find a solution before the retreive-repair planner
>     (RR). However, when the database contains the path, the PFS has a
>     low probability to find a path first and maybe to improve the
>     quality of the database in order to have more optimal path for
>     future planning request. So, is it possible to force the PFS to
>     generate trajectory in order to improve the database?
>
    I am a little confused. PFS and RR run in parallel threads.
    Whichever finds a solution first will terminate the other thread.

What I mean is, once a path exists (or almost) in the database, the 
probability for the PFS thread to return a solution first is lower than 
this one for the RR thread. Because a single planning request takes a 
lot of time (20-30s) for this problem. Moreover, in this case the 
database has a low probability to be enriched and improved.
So it is possible to force the PFS thread to work alone (to focuse on 
the learing) some time in order to build a more "optimal" database? If 
yes, Is it possible to do this offline?

Thank you again and congratulation to the whole team, OMPL is a nice 
library and well documented.
Regards,
Yoann.






On 09/30/2016 06:40 PM, Mark Moll wrote:
> Hi Yoann,
>
> My answers are below inline.
>
> Mark
>
>
>
>> On Sep 28, 2016, at 7:23 AM, Yoann Solana <ys...@la... 
>> <mailto:ys...@la...>> wrote:
>>
>> Hi all,
>>
>> We are working on the implementation of research work about dual-arm 
>> manipulation.Our goal is to develop in Moveit a new functionality 
>> able to execute complex closed kinematics chain motions in a 
>> cluttered environment. For now, we are able to plan motion with the 
>> RRT planner. However, the workspace is very complex, so that planning 
>> requests are almost impossible to solve in a short time (1-2s). 
>> Therefore, we want to take advantage of multi-query planners like PRM 
>> or SRS to compute offline a roadmap of the workspace in order to 
>> efficiently perform a motion request later.
>>
>> In previous posts 
>> (https://sourceforge.net/p/ompl/mailman/message/32657097/, 
>> https://sourceforge.net/p/ompl/mailman/message/35074389/), Mark Moll 
>> explained that is was not possible to restart the planner using 
>> a previously constructed roadmap without a lot of work in the code. 
>> However, he proposed experienced based planners.
>>
>> I read the article "Experience-Based planning with Sparce Roadmap 
>> Spanners" and the LightningThunder demo. I'm thinking about using the 
>> Thunder framework inside Moveit, but I need to be sure if it will be 
>> in accordance to our requirements.
>> I have several question about the Thunder framework:
>> _ The exp-based planning is suited for robots with large number of 
>> invariant constraints. So, for our case, additional constraints 
>> generated by closed kinematic chains can be easily encoded in the 
>> experience database. This will lead to have a robust database. Am I 
>> right?
>>
>
> Yes.
>>
>> _ I see the size of the data base have a kind of theoretical limit. 
>> However, can I be sure that the database will have a size limit, even 
>> after months of running?
>>
>
> The database is *guaranteed* not to grow indefinitely. This is because 
> it is using the spanner properties of the underlying SPARS2-based 
> planner. By changing the planner parameters you can control how sparse 
> the roadmap (and how large the database) will be. We can’t predict 
> what the size of the database will be in the limit, though.
>>
>> _ Our experimentation consists in planning motion for several objects 
>> to manipulate. During the experimentation the start and goal poses of 
>> the objects will be always the same for each object. The data base of 
>> Thunder grows at each time the planner from scratch (PFS) find a 
>> solution before the retreive-repair planner (RR). However, when the 
>> database contains the path, the PFS has a low probability to find a 
>> path first and maybe to improve the quality of the database in order 
>> to have more optimal path for future planning request. So, is it 
>> possible to force the PFS to generate trajectory in order to improve 
>> the database?
>>
>
> I am a little confused. PFS and RR run in parallel threads. Whichever 
> finds a solution first will terminate the other thread.
>>
>> _ Last question, I see the experimentation in the article was perform 
>> with Moveit/OMPL. Is it possible to have more documentation about the 
>> implementation in Moveit?
>>
> That’s a great idea. Dave Coleman (cc-ed) has written most of this 
> code. I hope he can find some time to write some instructions on how 
> to use Thunder/Lightning with MoveIt!.
>
>
>

Re: [ompl-users] Re-using data from previous planning requests.

[Mark M. <mm...@ri...>]

Hi Yoann,

My answers are below inline.

Mark



> On Sep 28, 2016, at 7:23 AM, Yoann Solana <ys...@la...> wrote:
> 
> Hi all,
> 
> We are working on the implementation of research work about dual-arm manipulation. Our goal is to develop in Moveit a new functionality able to execute complex closed kinematics chain motions in a cluttered environment. For now, we are able to plan motion with the RRT planner. However, the workspace is very complex, so that planning requests are almost impossible to solve in a short time (1-2s). Therefore, we want to take advantage of multi-query planners like PRM or SRS to compute offline a roadmap of the workspace in order to efficiently perform a motion request later.
> 
> In previous posts (https://sourceforge.net/p/ompl/mailman/message/32657097/ <https://sourceforge.net/p/ompl/mailman/message/32657097/>, https://sourceforge.net/p/ompl/mailman/message/35074389/ <https://sourceforge.net/p/ompl/mailman/message/35074389/>), Mark Moll explained that is was not possible to restart the planner using a previously constructed  roadmap without a lot of work in the code. However, he proposed experienced based planners. 
> 
> I read the article "Experience-Based planning with Sparce Roadmap Spanners" and the LightningThunder demo. I'm thinking about using the Thunder framework inside Moveit, but I need to be sure if it will be in accordance to our requirements.
> I have several question about the Thunder framework:
> _ The exp-based planning is suited for robots with large number of invariant constraints. So, for our case, additional constraints generated by closed kinematic chains can be easily encoded in the experience database. This will lead to have a robust database. Am I right?
> 

Yes.
> _ I see the size of the data base have a kind of theoretical limit. However, can I be sure that the database will have a size limit, even after months of running?
> 

The database is *guaranteed* not to grow indefinitely. This is because it is using the spanner properties of the underlying SPARS2-based planner. By changing the planner parameters you can control how sparse the roadmap (and how large the database) will be. We can’t predict what the size of the database will be in the limit, though.
> _ Our experimentation consists in planning motion for several objects to manipulate. During the experimentation the start and goal poses of the objects will be always the same for each object. The data base of Thunder grows at each time the planner from scratch (PFS) find a solution before the retreive-repair planner (RR). However, when the database contains the path, the PFS has a low probability to find a path first and maybe to improve the quality of the database in order to have more optimal path for future planning request. So, is it possible to force the PFS to generate trajectory in order to improve the database?
> 

I am a little confused. PFS and RR run in parallel threads. Whichever finds a solution first will terminate the other thread.
> _ Last question, I see the experimentation in the article was perform with Moveit/OMPL. Is it possible to have more documentation about the implementation in Moveit?
> 
That’s a great idea. Dave Coleman (cc-ed) has written most of this code. I hope he can find some time to write some instructions on how to use Thunder/Lightning with MoveIt!.

[ompl-users] Re-using data from previous planning requests.

[Yoann S. <ys...@la...>]

Hi all,

We are working on the implementation of research work about dual-arm 
manipulation.Our goal is to develop in Moveit a new functionality able 
to execute complex closed kinematics chain motions in a 
cluttered environment. For now, we are able to plan motion with the RRT 
planner. However, the workspace is very complex, so that planning 
requests are almost impossible to solve in a short time (1-2s). 
Therefore, we want to take advantage of multi-query planners like PRM or 
SRS to compute offline a roadmap of the workspace in order to 
efficiently perform a motion request later.

In previous posts 
(https://sourceforge.net/p/ompl/mailman/message/32657097/, 
https://sourceforge.net/p/ompl/mailman/message/35074389/), Mark Moll 
explained that is was not possible to restart the planner using 
a previously constructed  roadmap without a lot of work in the code. 
However, he proposed experienced based planners.

I read the article "Experience-Based planning with Sparce Roadmap 
Spanners" and the LightningThunder demo. I'm thinking about using the 
Thunder framework inside Moveit, but I need to be sure if it will be in 
accordance to our requirements.
I have several question about the Thunder framework:
_ The exp-based planning is suited for robots with large number of 
invariant constraints. So, for our case, additional constraints 
generated by closed kinematic chains can be easily encoded in the 
experience database. This will lead to have a robust database. Am I right?
_ I see the size of the data base have a kind of theoretical limit. 
However, can I be sure that the database will have a size limit, even 
after months of running?
_ Our experimentation consists in planning motion for several objects to 
manipulate. During the experimentation the start and goal poses of the 
objects will be always the same for each object. The data base of 
Thunder grows at each time the planner from scratch (PFS) find a 
solution before the retreive-repair planner (RR). However, when the 
database contains the path, the PFS has a low probability to find a path 
first and maybe to improve the quality of the database in order to have 
more optimal path for future planning request. So, is it possible to 
force the PFS to generate trajectory in order to improve the database?
_ Last question, I see the experimentation in the article was perform 
with Moveit/OMPL. Is it possible to have more documentation about the 
implementation in Moveit?

Thanks in advance for your answers,
Regards,
Yoann.

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

[Mark M. <mm...@ri...>]

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 <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... <mailto: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... <mailto: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... <mailto:omp...@li...>
>>> https://lists.sourceforge.net/lists/listinfo/ompl-users <https://lists.sourceforge.net/lists/listinfo/ompl-users>
>> 
> 

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

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

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 <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... <mailto: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... <mailto:omp...@li...>
>> https://lists.sourceforge.net/lists/listinfo/ompl-users
>