Control For Robot-Assisted Image-Guided Beating-Heart Surgery