mp-cpp-test2 Clean Version of MP no documentation cuz im lazy basically generates trajectories from bezier curve