In ABB RobotStudio I have the following issue. Given a starting  point p1 defined in robotarget and a final point p2 with unknown robot configuration I wanto to Linearly interpolate the path in order to have a linear path from p1 to p2. The issue is how to change the confdata of the interpolated points so that passing through a point of singularity does not cause sudden changes in the axes. For example the point p1 is defined as follow:
CONST robtarget p1:=[[2216,-1500,2020],[0.707106781,0,0.707106781,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
while p2 doesn't have the confdata. The movement between the interpolated point is made with MoveAbsJ
The interpolating algorithm created is the following one:
LOCAL PROC PathInterpolation(robtarget p1, robtarget p2, INOUT robtarget pInterpolatedIN{*}, PERS tooldata tToolToUse \wobjdata wobjToUse )
        !Support variables
        VAR robtarget dnResult;
        VAR num count := 1;
        VAR num t:= 0;

        WHILE count<=nSize DO
            t:= (count-1)*ndt;
            
            ! Linear interpolation for positions
            dnResult.trans.x := (1 - t) * p1.trans.x + t * p2.trans.x;
            dnResult.trans.y := (1 - t) * p1.trans.y + t * p2.trans.y;
            dnResult.trans.z := (1 - t) * p1.trans.z + t * p2.trans.z;
            
            ! Slerp interpolation for orientations
            dnResult.rot := Slerp(p1.rot, p2.rot, t);
            
            ! Copy the other attributes
            dnResult.robconf := p1.robconf;           
            dnResult.extax := p1.extax;
            
            ! DEBUG
            !TPWrite "t: "+ NumToStr(t,5) + "count: " + NumToStr(count,1);
            
            ! Save in the array
            pInterpolatedIN{count} := dnResult;
            Incr count;

        ENDWHILE
        ! Ensure the last point exactly matches p2
        dnResult := p2;
        pInterpolatedIN{nSize} := dnResult;  ! Ensure the last point is exactly p2

    ENDPROC

    !Slerp : Perform SLERP interpolation for ORIENTATION
    LOCAL FUNC orient Slerp(orient p1, orient p2, num t)
        ! Support variables
        VAR num dot ;
        VAR num n_theta;
        VAR num dnSinTheta;
        VAR num scale1;
        VAR num scale2;
        VAR orient dnResult;
        VAR num magnitude;
        
        ! Compute the dot product (cosine of the angle between the quaternions)
        dot := p1.q1*p2.q1 + p1.q2*p2.q2 + p1.q3*p2.q3 + p1.q4*p2.q4;
        
        ! If the dot product is negative, negate one quaternion to take the shorter path
        IF dot < 0 THEN
            p1.q1 := -p1.q1;
            p1.q2 := -p1.q2;
            p1.q3 := -p1.q3;
            p1.q4 := -p1.q4;
            dot := -dot;
        ENDIF
    
        ! If the quaternions are too close, use linear interpolation
        IF dot > 0.9995 THEN
            dnResult.q1 := (1 - t) * p1.q1 + t * p2.q1;
            dnResult.q2 := (1 - t) * p1.q2 + t * p2.q2;
            dnResult.q3 := (1 - t) * p1.q3 + t * p2.q3;
            dnResult.q4 := (1 - t) * p1.q4 + t * p2.q4;
        ELSE
            ! Compute the angle and sin(n_theta)
            n_theta := ACOS(dot);
            dnSinTheta := SIN(n_theta);
            
            ! Compute the scaling factors
            scale1 := SIN((1 - t) * n_theta) / dnSinTheta;
            scale2 := SIN(t * n_theta) / dnSinTheta;
            
            ! Compute the interpolated quaternion
            dnResult.q1 := scale1 * p1.q1 + scale2 * p2.q1;
            dnResult.q2 := scale1 * p1.q2 + scale2 * p2.q2;
            dnResult.q3 := scale1 * p1.q3 + scale2 * p2.q3;
            dnResult.q4 := scale1 * p1.q4 + scale2 * p2.q4;
        ENDIF
        
        ! Normalize the resulting quaternion
        magnitude := SQRT(dnResult.q1*dnResult.q1 + dnResult.q2*dnResult.q2 + dnResult.q3*dnResult.q3 + dnResult.q4*dnResult.q4);
        dnResult.q1 := dnResult.q1 / magnitude;
        dnResult.q2 := dnResult.q2 / magnitude;
        dnResult.q3 := dnResult.q3 / magnitude;
        dnResult.q4 := dnResult.q4 / magnitude;
        
        ! Return the normalized result
        RETURN dnResult;
    ENDFUNC