Add G60 and G61.

Add command G60 put in memory actual position.
Add G61 move X Y Z to position in memory.