Check seed for validity

[simonschmeisser]

StompSmoothingAdapter:

Currently validity (ie collision free) is only checked after the first iteration. This means that even though a valid seed is given, STOMP might invalidate this during it's first iteration and never recover. This has led to discussions about an additional option allowing the adapter to fail and return the seed as a fall-back solution.

I propose to

1. calculate the cost of the seed and thereby see if it's valid
2. discuss about whether num_iteration_after_valid: 0 should then be ignored and still at least one iteration performed
3. add an option to fail if the cost of the trajectory did not improve, returning the seed iff valid otherwise, fail if it isn't

this is a continuation of the discussion in #7 and #20

[jrgnicho]

@simonschmeisser I'm on board with making stop evaluate the cost of the seed trajectory prior to the first iteration, it should be a matter of tweaking the main loop of stomp here