First version of the optimal position calculator

First working version where an object can be given to the calculator and it optimizes the position of the gripping points, the position of the ur5 base links and the orientation of the mir200 base