Probing is a motion controller function. All it is to Mach is a regular feed move. So the first thing I would check is to make sure that the motion controller supports probing and if it does, how to set it up. For instance, on a Galil, the probe has to be wired to inputs 1, 2, 3, and 4. Then we map one of those inputs to the Probe signal in Mach. The probe signal is there just for diagnostics purposes. Just so the user can jiggle the probe and see the LED flash.
But back to your question of solving the problem. Yes, a G31 would be a good way to do it. It is called a "protected move".
Steve