Added centroid calculation and grasp angle visualization... almost working