Vision and Contact based Optimal Control for Autonomous Trocar Docking