Learning OMPL

Compiling

I just start learning the Open Motion Planning Library (OMPL) to implement some new robot control ideas. Was able to build the current C++ version from Git without problems. However, building python binding did not went smooth because of missing dependencies and lack of required versions in standard Ubuntu repository. So I leave it because I am interesting in C++ anyway.

Running demos

The closest demo to what I need is RigidBodyPlanningWithODESolverAndControls.cpp . The first run produces result which I do not understand for 100%. It reach the goal, but trajectory looks strange to me.

In particular, I can not explain the movements around x=0.1, y=0.05 and similar back and forth on the other places. Maybe there are certain parameters to tweak, but it would be nice if demo will already have them set right. Of course, it could be that am just interpreting the output wrong. Anyway, I am not going to give up that soon and will try to ask this question on mail list. Let's see if the community friendly there ;-) .

Tailoring the demo

In my case the map has size somewhere around 1000x1000 in contrast to the size 2 (-1:1) used in demo. So I decide to adjust the demo by increasing the state space bounds correspondingly. In addition, I've made the car longer (5 instead of original 0.2). Also to let it drive faster, I've adjust control space bounds to 0:50 for the speed dimension. The following is reported by printSettings() function:

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

OMPL was able to find solution. But again, similar to the original demo, the path is quite weird as could be seen on the following image:

Of course, my first reaction was to blame OMPL for it ;-) . But it seems like a lot of projects are using it, so it could not be that bad. Obviously I am doing something wrong here. But I still have to find out what... Maybe someone in mail-list would respond to my questions and can help.

I hear and I forget. I see and I remember. I do and I understand. (c) Confucius.

Actually, my own robotics vehicle is differentially steered, not car-like as assumed in the OMPL example mentioned above. It looks like this:

Rendering from CAD

The very first working prototype

So I decide to adjust the kinematics model appropriately. There is a nice easy to understand article by G.W. Lucas to this topic. So after spending quite some time tweaking propagation step size, the propagation duration and map size I was able to get more or less reasonable result:

It is obviously not optimal, but much better then my previous attempts described above. Also, I've got response from Mark Moll to my question in mail list. He states that with this implementation it is expected to get not optimal solution which confirms my experience so far. In addition, he suggests to try SST planner. This is what I am going to do next.