Feature/optimization objective

[roym899]

This adds support for changing the optimization objective. Currently supported:

• path length (as before)
• maximize minimum clearance
• OMPL smoothness (doesn't seem to work)
• minimize normalized curvature

It also adds a corresponding metric for the normalized curvature. Clearance and OMPL smoothness were already supported before.