 # Transforming Coordinates from one user frame to another

• 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.

• Example demonstrating what you're asking for (from the karel reference manual): • To get a point into a user frame from the world frame, you just times the point by the inverse of the user frame.

ufPoint = uFrame.Inverse() * worldPoint; (Pretty much what TitusLepic shows)

I do this in matrix form. Here is the code for converting a point to matrix rep:

And the code for turning a matrix back to XYZWPR:

Check out the Fanuc position converter I wrote here!

• TitusLepic Thanks! The ':' operator might be what I need to avoid doing all the math for transforming frames. I will look into implementing it in Karel to see if it works.

Nation Thanks for the code on doing the calculations. I think I was missing the additional math for calculating WPR. Do you know of a source I can look at to understand the intuition behind the WPR math?

• The ':' operator worked. Thanks for the insight!

Below is the Karel code that allows me to convert a position between any pair of user frames:

• Do you know of a source I can look at to understand the intuition behind the WPR math?

You can look over at the "Geometry" forum for a lot of discussions on this topic.

The key is Euler angles, and understanding that the rotations are done sequentially, not in parallel.

(There's also Tait-Bryant angles, which are really just a particular subset of Euler)

The pain point is, there are many different, equally valid, sequences for Euler rotations -- the only real rule is that you cannot rotate around the same axis twice in a row. And every brand uses a different Euler sequence.

Kawasaki uses ZYZ, Fanuc uses XYZ, KUKA uses ZYX, and there are a few others.

And just to complicate things further, there's intrinsic vs extrinsic methods -- KUKAs use intrinsic, while Fanucs (IIRC, someone check me on this) use extrinsic. Which means that on a KUKA, you rotate around Z, then rotate around the new position of its own Y axis (Y'), then around the new X axis (X''). While if KUKA was an extrinsic system, you would rotate your tool/point/whatever around the Z axis of the reference frame (World or Base), then rotated the rotated point around the Y axis of the reference frame, then around the X of the reference frame.

So, intrinsic rotates around itself, with each rotation affecting the rotations after it, while extrinsic rotates around the unmoving axes of the reference frame.

All of which means that, thanks to Euler angles, adding 90deg to your W value in a Fanuc does not rotate your point 90deg around X... unless the point started at all 0s for WPR. And if your point starts at some combination of 90deg (like 0,90,0, or 0,0,90, or 0,180,0), you can get into a strange situation where rotating the point around the X axis has no real effect on the W value, but might have major effects on the P and R values. This is why you have to use matrix math when doing rotations in robot coordinates, while simple addition will work for the XYZ values. Fortunately, most (all?) robots offer some sort of tool in their programming language to handle this.

The free version of RoboDK is nice for illustrating how the different robot brands do this -- you can create a point, then change its representation between the different major brands to see how the same location&orientation in Cartesian space is represented with completely different values in different Euler schemes.