Fixed state observability checking while sampling roadmap nodes.
Now, in planner, nodes stability is checked before adding to graph.