this code is based on https://github.com/cagik/path_Optimizer
there is a new corridor-building method and new collision-avoidence constraints. And we can solve the NLP quickly.
git clone https://github.com/cagik/trajectoryOpt.git
cd trajectoryOpt/bin
cmake ..
make
./cartesianPlanner_demo
python Scene.py