Created a separate test program for optimization based planning/control. Currently test_greedy_cartesian_planner_main.cpp and test_optimization_based_greedy_planner_main.cpp are equivalent; they both test optimization based greedy planning.