Add non-fixed plane parameters in gurobi optimization

[caomuqing]

This pull request adds the separation plane parameters as optimization variables in Gurobi optimization, thanks to the new Gurobi 9.1 capable of solving non-convex quadratic constraints. It is tested in Ubuntu 18.04 ROS Melodic, with Gurobi 9.1.2.

I have also added a parameter use_linear_collision_constraint to disable/enable using the plane variables in the optimization. When set to true (which is by default), the algorithm works exactly as before.

Performance: using nonconvex quadratic constraints in Gurobi shows significant increase in computation time (~80ms as compared to ~15ms in the single agent random forest test) and a drop in success rate (<80% in the single agent random forest test).

I have also added some printout (commented by default) which can help to evaluate the performance of the optimization.

Hope this is useful.