Keep longest VALID config seq if cannot solve full

The expected behavior of solveOptimizationProblem () function is that,
if it manages to compute configurations for all waypoints, it will
return that sequence of configurations. In case when it fails, it
should return the longest sequence of valid configurations.

The previous implementation saves each configuration computed, even if
it is invalid. Also, when backtracking to a previous waypoint, the new
points computed will overwrite some of the waypoint in the longest
sequence. So indeed, the final sequence kept is not valid, and not
consistent (not generated in successful consecutive solves).

This has now been fixed by saving the longest sequence of valid
configurations in a separate variable, and only update it when a longer
sequence is found.