You could use a feature and then have all the points relative to that feature. Then when you update the feature every cycle all of the points would update automatically. This would require you to use a moveL if you want to use standard Polyscope move functions and not script.
After adding pose from distance sensor to the actual pose, movement is OK, and goes where I expected. I need to change left over waypoints with new Z coordinate. This is like giving offset for robot axis in main setup(Installation). But i need to do that for every cycle.