That is one way to do it but there is no pose_inv/pose_trans in the URCap API so you either need to use a socket and calculate it with URScript or use/write a Java library that implements the same functions (e.g. Pose_trans and pose_inv functions in URCap - #9 by r.larsson).
Waypoint poses also change when switching TCPs and I wanted to avoid all that, so I force the TCP to be set to the needed one when adding and moving to positions. This shouldn’t be a problem if the calculations are done properly.