FANUC XYZ-WPR format

The pose format that is used by FANUC robots consists of a position XYZ in millimeter and an orientation WPR that is given by three angles in degrees, with W rotating around x-axis, P rotating around y-axis and R rotating around z-axis. The rotation order is x-y-z and computed by rz(R)ry(P)rx(W).

Conversion from FANUC-WPR to quaternion

The conversion from the WPR angles in degrees to a quaternion q=(xyzw)T can be done by first converting all angles to radians

Wr=Wπ180,Pr=Pπ180,Rr=Rπ180,

and then calculating the quaternion with

x=cos(Rr/2)cos(Pr/2)sin(Wr/2)sin(Rr/2)sin(Pr/2)cos(Wr/2),y=cos(Rr/2)sin(Pr/2)cos(Wr/2)+sin(Rr/2)cos(Pr/2)sin(Wr/2),z=sin(Rr/2)cos(Pr/2)cos(Wr/2)cos(Rr/2)sin(Pr/2)sin(Wr/2),w=cos(Rr/2)cos(Pr/2)cos(Wr/2)+sin(Rr/2)sin(Pr/2)sin(Wr/2).

Conversion from quaternion to FANUC-WPR

The conversion from a quaternion q=(xyzw)T with ||q||=1 to the WPR angles in degrees can be done as follows.

R=atan2(2(wz+xy),12(y2+z2))180πP=asin(2(wyzx))180πW=atan2(2(wx+yz),12(x2+y2))180π