Problem after recent update

[Shushman]

Sorry for this rather unspecific question, but after recently updating or_ompl to be consistent with the origin, a script that previously worked with a custom planner now fails with the following error:

[OMPLPlanner.cpp:223 InitPlan] InitPlan failed: This only supports real vector state spaces!

I can guess what the error is about - something about either the start/goal configs in the params maybe? However, as I have changed nothing in the script code, I cannot tell what is causing it to fail. I know that's not much to go on, but is there any recent change that might affect this?

[cdellin]

This is because of a recent merge (#36) .. the state space passed to all planners now constructed with or_ompl will be a CompoundStateSpace [0] containing one or more RealVectorStateSpace and/or SO2StateSpace instances. This is to handle robots with continuous joints. In the case of HERB (and other robots without any continuous joints), this means that instead of getting a RealVectorStateSpace (e.g. R^7), you get a CompoundStateSpace containing one RealVectorStateSpace (R^7).

Planners internal to OMPL generally handle sampling and distances and things on arbitrary state types, so they will mostly work identically (though with perhaps some performance loss due to (a) additional pointer indirections when working with states, and (b) potentially different projections and other things).

However, some planners that some of us have been developing so far don't yet handle all the abstraction necessary to do this properly. For example, the constrained planners in pr-constraint currently do constraint projections in an assumed Euclidean space, and so they broke with the new change. I fixed this temporarily [1] by handling the special case of compound-containing-one-real in the planner code. Longer-term, we will need to determine how to handle this in the correct abstract way.

For your planner(s), in the short term you can apply a similar hack to [1]: handle the special case. Longer-term, you should see to what extent your planner requires the state space to be a RealVectorStateSpace, and then design interfaces (in addition to those already provided in OMPL) to do the operations you need on the states in a way that is ignorant of the particular type of state space provided.

(In terms of OMPL performance, I still think it would be a good idea for or_ompl to provide a RealVectorStateSpace as before for robots without any continuous joints, for performance reasons. But that doesn't fix the underlying issue with planners that currently don't work for robots with continuous joints.)

[0] http://ompl.kavrakilab.org/classompl_1_1base_1_1CompoundStateSpace.html
[1] personalrobotics/pr-constraint@8676ebef762e420a27f11a5528652e55903d327a