I am workin with my first nachi robot and am looking to write an offset program. Normally when programming robots I will read in my current position, then apply either apply a set z height that I know is clear to the current position or apply a static z offset. Then go to the same position that it was left at (random position in travel) then apply the offset and it will move up to a safe height, then from there it can go home. What does this look like in nachi? I am currently attempting to do this using XYZ shift FN58 but even with it all being linear moves when I apply the shift the robot will move up but it doesnt maintain the pitch/roll It moves the TCP straight up, but also rolls it quite a bit. I have tried this using world and TCP frames. Please let me know if you have any suggestions.