Corrected logging through testing on robot

Fixed issue with not recording the EE pose
Added position setting to the current