Kinematic planner (round 2)

Clean-up and speed up the kinematic planning classes from the previous pull request.