Dynamic Motion Planning for Autonomous Assistive Surgical Robots