! Additional RAPID functions module intended for use with 
! an ABB IRC5 controller running RobotWare 5.13+.
! Developed by T.Kamf, 2016 at Uppsala University (Sweden), 
! with the intent of providing extra functionality related 
! to automatic wedge mounting and work-piece calibration.
 

MODULE EXTRA_RAPID_FUNCTIONS(SYSMODULE)

! ####### Declaration of extra data-types ########

RECORD dualrobtarget
    robtarget rob1;
    robtarget rob2;
ENDRECORD

! ************************************************

RECORD trirobtarget
    robtarget rob1;
    robtarget rob2;
    robtarget rob3;
ENDRECORD

! ************************************************

RECORD dualnum
    num num1;
    num num2;
ENDRECORD

! ################################################
    
! ####### Declaration of global variables ########

VAR num callCNT:=111;

! ################################################

! ######## Declaration of extra functions ########
    
! Convert a bool 0 or 1 to a num 0 or 1.
FUNC num Bool2Num(bool a)
    VAR num tmp:=0;
    IF a tmp:=1;
    RETURN tmp;
ENDFUNC

! ************************************************

! Calculate the conjugate of a quaternion. 
FUNC orient QuatConj(orient q)  
    RETURN [q.q1,-q.q2,-q.q3,-q.q4];
ENDFUNC

! ************************************************

! Calculate the Hamilton product of two quaternions, p & q.
FUNC orient HamProd(orient p, orient q)
    VAR orient tmp;

    ! Calculate quaternion parts
    tmp.q1:=p.q1*q.q1-p.q2*q.q2-p.q3*q.q3-p.q4*q.q4;
    tmp.q2:=p.q1*q.q2+p.q2*q.q1+p.q3*q.q4-p.q4*q.q3;
    tmp.q3:=p.q1*q.q3-p.q2*q.q4+p.q3*q.q1+p.q4*q.q2;
    tmp.q4:=p.q1*q.q4+p.q2*q.q3-p.q3*q.q2+p.q4*q.q1;

    ! Return calculated quaternions
    RETURN tmp;
ENDFUNC

! ************************************************

! Rotate a vector using a normalized quaternion.
FUNC pos VecQuatRot(pos vector, orient quat)  
    VAR orient tmpori;
    VAR orient quat_conj;
    VAR num absquat;

    ! Make sure input quaternion is normalized
    absquat:=Sqrt(Pow(quat.q1,2)+Pow(quat.q2,2)+Pow(quat.q3,2)+Pow(quat.q4,2));
    quat:=[quat.q1/absquat,quat.q2/absquat,quat.q3/absquat,quat.q4/absquat];

    ! Calculate rotation
    tmpori:=[0,vector.x,vector.y,vector.z];
    tmpori:=HamProd(quat,tmpori);
    quat_conj:=QuatConj(quat);
    tmpori:=HamProd(tmpori,quat_conj);

    ! Return rotated vector
    RETURN [tmpori.q2,tmpori.q3,tmpori.q4];
ENDFUNC

! ************************************************

! Rotate a quaternion using a vector and an angle (in deg).
FUNC orient QuatRot(orient q,pos v,num a)
    VAR orient ori_tmp;

    ! Convert angular error to a quaternion
    ori_tmp:=RotVec2Quat(v,a);

    ! Calculate new quaternion using error
    ori_tmp:=HamProd(q,ori_tmp);

    ! Return answer
    RETURN ori_tmp;
ENDFUNC

! ************************************************

FUNC orient PipePoints2Quat(pos p0, pos p1, pos p2)
    ! Calculate a quaternion from three points from the pipe. 
    ! z-axis is along p0->p1 
    ! y-axis is then adjusted so that p2 lies in the yz-plane.
    ! x-axis is the cross product between the y and z axis.

    !               * p2                     ref. cord-system
    !               ^                     y
    !               |                     ^
    !              _|                     |
    !             | |                     |
    !     *---------*--------->*        x x-----> z
    !   p0          ps         p1

    ! Define local variables
    VAR pos zvec;
    VAR pos yvec;
    VAR pos xvec;
    VAR num absvec;
    VAR orient quat;

    ! Make z-axis vector:
    zvec.x:=-p0.x+p1.x-((p0.x-p1.x)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    zvec.y:=-p0.y+p1.y-((p0.y-p1.y)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    zvec.z:=-p0.z+p1.z-((p0.z-p1.z)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    ! Normalize z:
    absvec:=Sqrt(Pow(zvec.x,2)+Pow(zvec.y,2)+Pow(zvec.z,2));
    zvec:=[zvec.x/absvec,zvec.y/absvec,zvec.z/absvec];

    ! Make y-axis vector:
    yvec.x:=-p0.x+p2.x-((p0.x-p1.x)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    yvec.y:=-p0.y+p2.y-((p0.y-p1.y)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    yvec.z:=-p0.z+p2.z-((p0.z-p1.z)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    ! Normalize y:
    absvec:=Sqrt(Pow(yvec.x,2)+Pow(yvec.y,2)+Pow(yvec.z,2));
    yvec:=[yvec.x/absvec,yvec.y/absvec,yvec.z/absvec];

    ! Make x-axis vector:
    xvec:=cross(yvec,zvec);

    ! Normalize x:
    absvec:=Sqrt(Pow(xvec.x,2)+Pow(xvec.y,2)+Pow(xvec.z,2));
    xvec:=[xvec.x/absvec,xvec.y/absvec,xvec.z/absvec];

    ! Convert vectors into a quaternion
    quat:=Rotm2Quat(xvec,yvec,zvec);

    ! Return calculated quaternion
    RETURN quat;
ENDFUNC

! ************************************************

FUNC orient ShoePoints2Quat(pos p0, pos p1, pos p2)
    ! Calculate a quaternion from three points on the pole-shoe. 
    ! y-axis is along p0->p1. 
    ! x-axis is then adjusted so that p2 lies in the xy-plane.
    ! z-axis is the cross product of the x and y axis.

    !               * p2                    ref. cord-system
    !               ^                    z
    !               |                    O-----> y
    !              _|                    |
    !             | |                    |
    !     *---------*--------->*         v x
    !   p0          ps         p1

    ! Define local variables
    VAR pos xvec;
    VAR pos yvec;
    VAR pos zvec;
    VAR num absvec;
    VAR orient quat;

    ! Make x-axis vector:
    xvec.x := -p0.x+p2.x-((p0.x-p1.x)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    xvec.y := -p0.y+p2.y-((p0.y-p1.y)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    xvec.z := -p0.z+p2.z-((p0.z-p1.z)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    ! Normalize x:
    absvec := Sqrt(Pow(xvec.x,2)+Pow(xvec.y,2)+Pow(xvec.z,2));
    xvec := [xvec.x/absvec, xvec.y/absvec, xvec.z/absvec];

    ! Make y-axis vector:
    yvec.x := p0.x-p1.x+((p0.x-p1.x)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    yvec.y := p0.y-p1.y+((p0.y-p1.y)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    yvec.z := p0.z-p1.z+((p0.z-p1.z)*(p0.x*p1.x+p0.x*p2.x-p1.x*p2.x+p0.y*p1.y+p0.y*p2.y-p1.y*p2.y+p0.z*p1.z+p0.z*p2.z-p1.z*p2.z-Pow(p0.x,2)-Pow(p0.y,2)-Pow(p0.z,2)))/(p0.x*p1.x*(-2.0)-p0.y*p1.y*2.0-p0.z*p1.z*2.0+Pow(p0.x,2)+Pow(p1.x,2)+Pow(p0.y,2)+Pow(p1.y,2)+Pow(p0.z,2)+Pow(p1.z,2));

    ! Normalize y:
    absvec := Sqrt(Pow(yvec.x,2)+Pow(yvec.y,2)+Pow(yvec.z,2));
    yvec := [yvec.x/absvec, yvec.y/absvec, yvec.z/absvec];

    ! Make z-axis vector:
    zvec := cross(xvec,yvec);

    ! Normalize z:
    absvec := Sqrt(Pow(zvec.x,2)+Pow(zvec.y,2)+Pow(zvec.z,2));
    zvec := [zvec.x/absvec, zvec.y/absvec, zvec.z/absvec];

    ! Convert vectors into a quaternion
    quat:=Rotm2Quat(xvec,yvec,zvec);

    ! Return the calculated quaternion
    RETURN quat;
ENDFUNC

! ************************************************

! Calculate the cross-product of u and v
FUNC pos cross(pos u, pos v)
    RETURN [u.y*v.z-u.z*v.y,u.z*v.x-u.x*v.z,u.x*v.y-u.y*v.x];
ENDFUNC

! ************************************************

FUNC orient Rotm2Quat(pos xp, pos yp, pos zp)
    ! Calculate a quaternion from a normalized rotation matrix
    ! Input is the primed(') axis vectors of the matrix.
    !      x'   y'   z'
    !  x  xp.x yp.x zp.x
    !  y  xp.y yp.y zp.y   ==>  [q1, q2, q3, q4]
    !  z  xp.z yp.z zp.z

    ! Define user variables
    VAR orient quat;
    VAR num absvec;
    VAR num a;
    VAR num b;
    VAR num c;
    VAR num d;

    ! Make sure no root-part is negative, due to round off errors
    a := xp.x+yp.y+zp.z+1;
    a := a*BoolToNum(a>=0);
    b := xp.x-yp.y-zp.z+1;
    b := b*BoolToNum(b>=0);
    c := yp.y-xp.x-zp.z+1;
    c := c*BoolToNum(c>=0);
    d := zp.z-xp.x-yp.y+1;
    d := d*BoolToNum(d>=0);

    ! Calculate quaternion parts
    quat.q1:=Sqrt(a)/2*(2*BoolToNum(1>=0)-1);
    quat.q2:=Sqrt(b)/2*(2*BoolToNum(yp.z>=zp.y)-1);
    quat.q3:=Sqrt(c)/2*(2*BoolToNum(zp.x>=xp.z)-1);
    quat.q4:=Sqrt(d)/2*(2*BoolToNum(xp.y>=yp.x)-1);

    ! Normalize
    absvec := Sqrt(Pow(quat.q1,2)+Pow(quat.q2,2)+Pow(quat.q3,2)+Pow(quat.q4,2));
    quat := [quat.q1/absvec, quat.q2/absvec, quat.q3/absvec, quat.q4/absvec];

    ! Return calculated quaternion
    RETURN quat;
ENDFUNC

! ************************************************  

! Calculate a quaternion from a vector and an angle in deg.
FUNC orient RotVec2Quat(pos v,num a)
    VAR orient tmp;
    VAR num absvec;

    ! Calculate quaternion
    absvec:=Sqrt(Pow(v.x,2)+Pow(v.y,2)+Pow(v.z,2));
    tmp:=[cos(a/2),v.x*sin(a/2)/absvec,v.y*sin(a/2)/absvec,v.z*sin(a/2)/absvec];

    ! Return calculated quaternion
    RETURN tmp;
ENDFUNC

! ************************************************  

! Calculate a quaternion from the three euler angles (in deg).
! a = (psi)  b = (theta)  c = (phi)
FUNC orient Euler2Quat(num a,num b, num c)
    VAR orient quat;
    VAR num absvec;

    ! Calculate quaternion
    quat.q1:=cos(c/2)*cos(b/2)*cos(a/2)+sin(c/2)*sin(b/2)*sin(a/2);
    quat.q2:=sin(c/2)*cos(b/2)*cos(a/2)-cos(c/2)*sin(b/2)*sin(a/2);
    quat.q3:=cos(c/2)*sin(b/2)*cos(a/2)+sin(c/2)*cos(b/2)*sin(a/2);
    quat.q4:=cos(c/2)*cos(b/2)*sin(a/2)-sin(c/2)*sin(b/2)*cos(a/2);

    ! Normalize the quaternion
    absvec:=Sqrt(Pow(quat.q1,2)+Pow(quat.q2,2)+Pow(quat.q3,2)+Pow(quat.q4,2));
    quat:=[quat.q1/absvec,quat.q2/absvec,quat.q3/absvec,quat.q4/absvec];

    ! Return calculated quaternion
    RETURN quat;
ENDFUNC

! ************************************************  

! Pseudo random number generator.
! Adapted from: https://forums.robotstudio.com/discussion/3742/random-value-needed (2016-08-05)
FUNC NUM RANDOM()
    VAR num val:=1;
    VAR pos pos_current;

    ! Get current robot position
    pos_current:=CPos(\Tool:=tool0,\WObj:=wobj0);

    ! Use position as seed
    val:=val+(Abs(pos_current.x*1000)+1);
    val:=val+(Abs(pos_current.y*1000)+1);
    val:=val+(Abs(pos_current.z*1000)+1);
    val:=val/333;

    ! Add time information to seed
    val:=val*(GetTime(\Sec)+1);
    val:=val*(GetTime(\Min)+1);

    ! Add callCNT number to seed
    ! Rem: Define global VAR: VAR num callCNT := 111;
    callCNT:=callCNT+1;

    IF callCNT>999 THEN
        callCNT:=111;
    ENDIF

    ! Division to get a pseudo random float number
    val:=val/callCNT;

    ! Choose one (1 or 2):
    ! 1. Normalize val to a value between 0 and 1 using Cos(x).
    ! RETURN ((0.5+Cos(val)/2));

    ! 2. Normalize val to a value between 0 and 1 using Round(x).
    RETURN 0.5+(val-Round(val));
ENDFUNC

! ************************************************  

! Find the maximum value in a pos
FUNC num pos_max(pos a)
    VAR num max := 0;

    IF ((a.x>a.y) AND (a.x>a.z)) THEN
        max := a.x;  
    ELSEIF ((a.y>a.x) AND (a.y>a.z)) THEN
        max := a.y;
    ELSE
        max := a.z;
    ENDIF

    ! Return maximum
    RETURN max;
ENDFUNC

! ################################################

ENDMODULE