working on planner, moveit_test works, Library need to be fixed to be more similar to moveit_test.