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),1−2(y2+z2))180πP=asin(2(wy−zx))180πW=atan2(2(wx+yz),1−2(x2+y2))180π