Penalizing reversing?

[SteveMacenski]

I see this PR in the other repo FastSense/mppic#11

Is this worth pulling in here? While I haven't tested yet, it seemed like reversing was already not particularly a problem -- but I could be wrong if you're starting at a path heading the opposite from the robot heading so reversing would allow tracking the path without turning around.

[artofnothingness]

it seemed like reversing was already not particularly a problem

I think it's not

This cost function could be added too, for preferring moving forward.