Yaw and Pitch angles from Point in UF in WF

Hello esteemed colleagues!

I have a CRX5 with iRVision 2D and I am playing around with the math to translate the coordinates of a Point in a UF (found using iRvision) to WF, which I will later create a frame with. As a proof-of-concept, I made a Python program that does (I don't have Karel):

1 - Create the Rotation matrix with given UF Coordinates (by matrix multiplying the rotation matrices of R(X)@R(Y)@R(Z) );

2 - Create the Homogeneous Transformation Matrix by concatenating the Rotation Matrix calculated above, the XYZ coordinates of the UF and [0,0,0,1];

3 - Matrix multiply the Homogeneous Transformation Matrix with the Point XYZ coordinates to get the WF coordinates of that Point;

This part is working great, and is giving me the right results. However it only gives me the XYZ coordinates, so how can I get the angles? I noticed that R is pretty straightforward, just the Point(R) + UF(R) and you get the R angle in WF, but how about W and P?

Thanks in advance for the help!

Nation, this is amazing! Not only you saved me a lot of time, because the next step would be "translating" my Python code into a Robot TP program, but it got the W and P just right. Amazing! Thank you very much!

I might be pushing it, but do you mind explaining how you calculate the W and P in WF? In cartesian representation I mean...

Nation, thanks for your insight! So, after some research I found that I need to multiply my Rotation Matrix generated with the UF WPR with the Rotation Matrix of the Point "P" in the UF (which will be only in RZ).

Then you will have another rotation Matrix in your hands and now you can extract the WPR angles of point "P" in World Frame.

The formula would be:

W = atan2(r32/cos(P), r33/cos(P))

P = atan2(-r31, sqrt(r32^2 + r33^2))

R = atan2(r21/cos(P), r11/cos(P))

r11, r21,r32 and r33 being the positions on the new rotation matrix.

You even used these formulas in your program!

So, all good now! Thanks so much for helping!

Additionally, I was reading the iRVision B-83914EN-1/06 manual and I found a Karel program called MATRIX that does essentially what Nation's program does, which is the linear transformation between two PR's. The advantage of using this program is two:

1 - It is built-in, so it comes with your 2D vision package, so every FANUC Robot with 2D iRVision installed should have it already;

2 - It does not require a UF to perform the operations.

The results are exactly the same.

This program takes 3 arguments with a 4^th optional argument if you wish to output error status:

• Is the PR index containing the UF coordinates [input];
• Is the PR index containing the Feature’s coordinates[input];
• Is the PR index for outputting the transformed coordinates (the feature position and orientation in reference to the World Frame)[output];
• Is the error code(optional)[output].