Registration working, triangle detector integrated, grasp point sligtly off
Can pick up the block autonomously, frame transf for grasp point fixed