ineq constraints working
1D trajectory generation refinement and test code written