It does not matter how we do this really. This will change in the fall, with upcoming changes in OMPL.
For now, i think we should have two different functions that compute cost: the one that was there and the one you added.

Then, cost(state) simply calls one of the two functions. Leave whichever as default.