Linked the ompl_app library
Another day's work. Everything works except the final output of the
state vectors is non-sensical