OMPL "invalid bounds" error in 6DoFHolonomicpathPlanning if orientation of object "Start" modified

Report crashes, strange behaviour, or apparent bugs
Post Reply
Uli_v
Posts: 35
Joined: 14 Dec 2018, 18:07

OMPL "invalid bounds" error in 6DoFHolonomicpathPlanning if orientation of object "Start" modified

Post by Uli_v »

Hi,

If I run the 6DoFHolonomicpathPlanning example without any modifications, then it works fine. I am using V-REP PRO EDU version 3.5.0 (rev. 6) on Ubuntu 16.04.
But if I change the orientation for the Dummy object "Start" (e.g. set "gamma" to 10 degrees), then I get an "invalid bounds" error for the start state:

Code: Select all

Info:    RRTConnect: Space information setup was not yet called. Calling now.
Debug:   RRTConnect: Planner range detected to be 0.804057
Warning: RRTConnect: Skipping invalid start state (invalid bounds)
         at line 248 in /home/marc/Documents/ompl-1.3.1-Source/src/ompl/base/src/Planner.cpp
Debug:   RRTConnect: Discarded start state Compound state [
Compound state [
RealVectorState [0.466748 0.15302 0.915532]
SO3State [-6.90681e-10 6.04268e-11 0.0871557 0.996195]
]
]

Error:   RRTConnect: Motion planning start tree could not be initialized!
         at line 177 in /home/marc/Documents/ompl-1.3.1-Source/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
I can host the modified example if you think it is necessary.

I observed this behavior also in my scene, which is more complex. In my application, I need to be able to find paths from random initial orientations (and positions).

Thank you for your help.
Uli

Uli_v
Posts: 35
Joined: 14 Dec 2018, 18:07

Re: OMPL "invalid bounds" error in 6DoFHolonomicpathPlanning if orientation of object "Start" modified

Post by Uli_v »

I think I know what the issue is. The precision of the values for the quaternions passed from V-REP to the OMPL library might not as high as expected by OMPL.

In SO3StateSpace.cpp of OMPL the MAX_QUATERNION_NORM_ERROR is defined as:

Code: Select all

static const double MAX_QUATERNION_NORM_ERROR = 1e-9;
But the norm of SO3State [-6.90681e-10 6.04268e-11 0.0871557 0.996195] is 1.000000297 and the error 2.97e-7 is greater than MAX_QUATERNION_NORM_ERROR = 1e-9.

If I change the MAX_QUATERNION_NORM_ERROR to 1e-7 then the issue is resolved (this requires to recompile the OMPL library/plugin).

An alternative solution would be to increase the precision of the quaternions on the V-REP side.

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: OMPL "invalid bounds" error in 6DoFHolonomicpathPlanning if orientation of object "Start" modified

Post by coppelia »

Thanks for that information!

Cheers

Post Reply