I have the following ABB Robotstudio issue when interpolating linearly between two point from p1 to p2: I don't know how to change the confdata particularly when crossing a singular configuration or a quadrant. I want to implement a series of IF and ELSE based on the previous point in order to prevent changes of 180 degrees of the axes. I know the configuration of the robot in p1 but I don't know the configuration in p2. The movement between the interpolated point is done with MoveAbsJ in order to cross the singularity point without issues.


 LOCAL PROC PathInterpolation(robtarget p1, robtarget p2, INOUT robtarget pInterpolatedIN{*}, PERS tooldata tToolToUse \PERS wobjdata wobjToUse )
        !Support variables
        VAR robtarget dnResult;
        VAR robtarget dnResult2;
        VAR num count := 1;
        VAR num t:= 0;
        VAR jointtarget jResult;
        VAR jointtarget jResultprev:=jResult;
        
        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