fix piece placement, update poses of gripper/head, use proper height, remove captured pieces from planning scene
fix lack of timeout on remove/add