Check current probe location before driving, some steps may be redundant

[kjy5]

Returning to surface when at surface makes probe drive down first before returning

[kjy5]

Also implement unified manipulator space