Intrinsic z-y'-z'' to angle vector and back

[vdg]

Hi

I need to send to and from a UR5 robot position and rotation information. I found some information on this forum and in odcumentation but I still have troubles to figure out the correct way:
I am kind of novice with UR robots so please bear with me
My host sytem (PC software) uses Euler intrinsic z - y’ - z’’ notation and from what I gather UR robot is using rotation angles in a pose variable. Is there an easy way to convert between these two way of expressing an orientation? I found documentation of functions rotvec2rpy and its inverse function. But I understand that these two functions use intrinsic z-y’-x’’ notation…
So my question is if there is a function for rotation vector <-> intrinsic z’ - y’ - z’’…

Regards

Volker

[inu]

Hello! Unfortunately there are no built in functions but you can create you own script.

For example here is a function, rpy2rv, which converts your current ‘roll’,‘pitch’,‘yaw’ into a rotation vector representation:

From Wiki:

Script Equivalent:

Likewise here is a script example that takes your rx,ry,rz values and converts into Rpy vector representation, rv2rpy:

From Wiki:

Script example:

This is how the current script functions you mentioned (rotvec2pry and rpy2rotvec) work. Below you can see the corresponding rotation matrices for other conventions. I’d imagine you can try replacing the rotation matrices used in the above script functions to the Z1Y2Z3 representation shown below to achieve what you are looking for. Hope this helps!

[vdg]

Hi Inu

thanks for the extensive answer! Based on your input I finally found a solution, however I implemented the routine on the host side.

[vdg]

@inu please correct me if I am wrong but I noticed that the UR script rpy2rv() does not cover the special condition where roll, pitch and yaw are 0:
That means the terms r11, r22 and r33 evaluate to 1 and thus theta is 0. This causes a division by zero in the formulas for kx, ky and kz.
So a condition must be implemented that checks for theta == 0 and hence sets kx, ky and kz to 0.