this code is based on https://github.com/libai1943/CartesianPlanner
I changed some of the variable constraints and the objective function to optmize my trajectory
git clone https://github.com/cagik/path_Optimizer.git
cd path_Optimizer/bin
cmake ..
make
./cartesianPlanner_demo
python Scene.py