Integrating Model Predictive Control and Dynamic Waypoints Generation for Motion Planning in Surgical Scenario