A system and method of operating the same are disclosed. The system includes a surgical instrument including an energy applicator and a manipulator including a plurality of links and a plurality of actuators operatively coupled to the plurality of links for moving the energy applicator in one or more degrees of freedom. At least one controller is configured to establish an initial position of the energy applicator. The at least one controller evaluates a plurality of possible final positions for the energy applicator with respect to one or more boundaries within which the energy applicator is allowed to move and beyond which the energy applicator is restricted from moving. Based on the evaluation of the plurality of possible final positions for the energy applicator, a commanded position to which the energy applicator is able to be moved by the manipulator without crossing the one or more boundaries is established.
