A surgical manipulator for manipulating a surgical instrument and an energy applicator extending from the surgical instrument and a method for controlling a manipulator to manipulate an instrument and an energy applicator extending from the instrument are described. The surgical manipulator comprises at least one controller configured to operate said surgical manipulator in a manual mode or a semi-autonomous mode, said at least one controller including a feed rate calculator configured to calculate an instrument feed rate. Said instrument feed rate is a velocity at which a distal end of the energy applicator advances along a path segment of a tool path in said semi-autonomous mode and said feed rate calculator calculates said instrument feed rate by adjusting a defined feed rate based on a plurality of variables. Said at least one controller is configured to model the surgical instrument and the energy applicator as a virtual rigid body and said feed rate calculator is configured to adjust said defined feed rate based on a virtual force applied to said virtual rigid body.
