I want to convert an XYZWPR position from the world frame to a specific user frame programmatically (so via a Karel/TP program instead of menu utility).
I am able to successfully convert the XYZ values from the world frame to a specific user frame by computing the below equation:
x', y', z' = New coordinate values
x, y, z = Old coordinate values
Xt, Yt, Zt, W, P, R = User Frame values
However, it seems that the point's orientation values are not as easy to convert. For instance, if I apply a user frame with a W offset, it also changes a point's P and R values.
I was wondering if anyone has insight into how to calculate the WPR values of a point based on a user frame.