RRTConnect is slow - Githubissues

mkoval commented 9 years ago

RRT-Connect takes several seconds to find a path in an almost entirely unconstrained environment. It also prints a suspicious message Created 374 states (184 start + 190 goal), which makes me think that we're repeatedly sampling the same start and goal configurations (unless, perhaps, these refer to the number of nodes in the start and goal trees). This is especially strange because I am planning from one configuration to another configuration, so no sampling is necessary at all.

Here is some example output on HERB's right arm. In this case, the straight-line trajectory is valid:

[INFO] [planning:base.py:238]:plan: Sequence - Calling planner "OMPL PR_RRTConnect".
[basemanipulation.cpp:117 main] BaseManipulation: using BiRRT planner
[taskmanipulation.cpp:179 main] using BiRRT planner
[OMPLPlanner.cpp:99 InitPlan] Creating state space.
[OMPLConversions.cpp:110 CreateStateSpace] Using default seed of 3570288693 for OMPL.
[OMPLConversions.cpp:118 CreateStateSpace] Setting joint limits.
[OMPLConversions.cpp:136 CreateStateSpace] Setting resolution.
[OMPLConversions.cpp:157 CreateStateSpace] Computed resolution of 0.020000 (0.003331 fraction of extents).
[OMPLConversions.cpp:162 CreateStateSpace] Setting joint weights.
[OMPLPlanner.cpp:106 InitPlan] Creating OMPL setup.
[OMPLPlanner.cpp:109 InitPlan] Setting initial configuration.
[OMPLPlanner.cpp:126 InitPlan] Setting goal configuration.
[OMPLPlanner.cpp:168 InitPlan] Creating planner.
[GenericParam.h:197] The value of parameter 'extension_type' is now: 'con-con'
[GenericParam.h:197] The value of parameter 'range' is now: '0.019998999999999999'
[OMPLPlanner.cpp:176 InitPlan] Setting state validity checker.
[RRTConnect.cpp:245] Starting with 1 states
[RRTConnect.cpp:365] Created 374 states (184 start + 190 goal)
[SimpleSetup.cpp:101] Solution found in 2.507072 seconds

jeking04 commented 9 years ago

FYI: Those numbers do refer to the number of states in the start and goal trees:

 OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());

siddhss5 commented 9 years ago

Looking at the code, it looks like that's the size of the start and goal trees. This is even more suspicious now. Are our default parameters crappy?

siddhss5 commented 9 years ago

Ah, @jeking04, our messages crossed.

cdellin commented 9 years ago

I won't be able to look at this until later tonight, but this planner should be identical to the stock OMPL RRTConnect planner (unless you set the extension type to a non-default value). So my first step will be to also install or_ompl and compare to our wrapping of that planner. With the same seed, they should be identical.

cdellin commented 9 years ago

I wasn't able to replicate this. Here's my test script:

#!/usr/bin/env python
from __future__ import print_function, unicode_literals, absolute_import, division
import atexit
import openravepy

openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
atexit.register(openravepy.RaveDestroy)
e = openravepy.Environment()
atexit.register(e.Destroy)

r = e.ReadRobotXMLFile('robots/barrettwam.robot.xml')
e.Add(r)
r.SetActiveDOFs(r.GetActiveManipulator().GetArmIndices())

for planstr in ('OMPL_RRTConnect','OMPL_PR_RRTConnect'):
   for i in range(5):
      print(planstr, i)

      r.SetActiveDOFValues([0.,0.,0.,0.,0.,0.,0])

      p = openravepy.RaveCreatePlanner(e, planstr)
      pp = p.PlannerParameters()
      pp.SetRobotActiveJoints(r)
      pp.SetGoalConfig([0.,1.,0.,0.,0.,0.,0.])
      pp.SetExtraParameters('<range>0.02</range>')
      p.InitPlan(r, pp)

      t = openravepy.RaveCreateTrajectory(e, '')
      res = p.PlanPath(t)
      print('result:', res)

I got the same results between OMPL_RRTConnect and OMPL_PR_RRTConnect (on this admittedly very simple problem). Mike, update this with a poorly-performing case?

cdellin commented 9 years ago

Mike and I decided that what he's really complaining about is some combination of "collision checking is slow" and "snap planner is faster than the OMPL RRT implementation on problems where the straight-line path is valid because the OMPL RRT does not first check the straight-line path, although I wouldn't want it to anyway because we have the snap planner". So I'm closing this (-:

mkoval commented 9 years ago

@cdellin is, as always, correct. :smile:

It turns out that these numbers are actually reasonable. I wrote a test case that plans back-and-forth from home to relaxed_home. The straight-line path is feasible and has a length of 4.72 radians and should require approximately 236 collision checks to verify.

Here is the outcome of calling our four randomized motion planners on this problem:

Planner	μ (s)	σ (s)
OMPL PR_RRTConnect	3.390	1.350
CBiRRT	1.067	0.290
OpenRAVE BiRRT	0.578	0.043
SnapPlanner	1.219	0.055

A few important notes:

CBiRRT and the OpenRAVE BiRRT both sample the goal first; OMPL does not.
It's not surprising that CBiRRT is slightly faster than SnapPlanner because it is performing the same operation in C++.
I have no idea how the OpenRAVE BiRRT is so much faster than CBiRRT at checking the straight-line path.

I think it is a good thing that SnapPlanner does not check the straight-line trajectory first. We only call a randomized planner if SnapPlanner fails, so this is a complete waste in effort. I do not know why: (1) collision checking the straight-line path takes almost 1 second and (2) the OpenRAVE BiRRT is so much faster at doing the collision checks.

psigen commented 9 years ago

Are you already passing that flag that only does collision checking with the ActiveDOFs?

On Tue, Apr 28, 2015 at 3:29 PM, Michael Koval notifications@github.com wrote:

@cdellin https://github.com/cdellin is, as always, correct. [image: :smile:]

It turns out that these numbers are actually reasonable. I wrote a test case https://gist.github.com/mkoval/1c76ef0db5f53db1b284 that plans back-and-forth from home to relaxed_home. The straight-line path is feasible and has a length of 4.72 radians and should require approximately 236 collision checks to verify.

Here is the outcome of calling our four randomized motion planners on this problem: Planner μ (s) σ (s) OMPL PR_RRTConnect 3.390 1.350 CBiRRT 1.067 0.290 OpenRAVE BiRRT 0.578 0.043 SnapPlanner 1.219 0.055

A few important notes:

CBiRRT and the OpenRAVE BiRRT both sample the goal first; OMPL does not.

It's not surprising that CBiRRT is slightly faster than SnapPlanner because it is performing the same operation in C++.

I have no idea how the OpenRAVE BiRRT is so much faster than CBiRRT at checking the straight-line path.

I think it is a good thing that SnapPlanner does not check the straight-line trajectory first. We only call a randomized planner if SnapPlanner fails, so this is a complete waste in effort. I do not know why: (1) collision checking the straight-line path takes almost 1 second and (2) the OpenRAVE BiRRT is so much faster at doing the collision checks.

— Reply to this email directly or view it on GitHub https://github.com/personalrobotics/pr-ompl/issues/2#issuecomment-97177347 .

mkoval commented 9 years ago

@psigen: Yes, CO_ActiveOnly is set:

in the PrPy bindings for CBiRRT
internally in or_ompl (we should really move this out into the PrPy bindings)
internally in the OpenRAVE BiRRT (I think? The code is pretty crazy in there.)

It was not set in SnapPlanner. Here are the new results with that addition:

Planner	μ (s)	σ (s)
OMPL PR_RRTConnect	3.644	1.117
CBiRRT	1.185	0.289
OpenRAVE birrt	0.651	0.032
SnapPlanner	0.989	0.039

As expected, SnapPlanner is now as fast as CBiRRT.

psigen commented 9 years ago

That makes more sense. Could the remaining difference between OpenRAVE BiRRT and SnapPlanner be call overheads from python?

On Tue, Apr 28, 2015 at 3:47 PM, Michael Koval notifications@github.com wrote:

@psigen https://github.com/psigen: Yes, CO_ActiveOnly is set:

in the PrPy bindings for CBiRRT

internally in or_ompl (we should really move this out into the PrPy bindings)

internally in the OpenRAVE BiRRT (I think? The code is pretty crazy in there.)

It was not set in SnapPlanner. Here are the new results with that addition: OMPL PR_RRTConnect 3.644 1.117 CBiRRT 1.185 0.289 OpenRAVE birrt 0.651 0.032 SnapPlanner 0.989 0.039

As expected, SnapPlanner is now as fast as CBiRRT.

— Reply to this email directly or view it on GitHub https://github.com/personalrobotics/pr-ompl/issues/2#issuecomment-97183879 .

mkoval commented 9 years ago

@siddhss5 thinks CheckPathAllConstraints is doing some shenanigans internally that makes it slow.

I modified SnapPlanner to discretize the trajectory and manually check for collisions (see here). Note that this implementation does not correctly handle continuous rotation (aka circular) joints.

Here are the new results:

Planner	μ (s)	σ (s)
OMPL PR_RRTConnect	3.006	0.662
CBiRRT	1.213	0.394
OpenRAVE birrt	0.688	0.392
SnapPlanner	0.520	0.069

SnapPlanner is now the fastest planner, by far.

personalrobotics / pr-ompl

RRTConnect is slow #2