I want to modify my ABB robotstudio algorithm in order to deal correctly with singularity. I have the following problem, knowing the starting point p1 i want to interpolate the straight line up to p2 but I don't know how to associate the robot configuration data to each interpolated point. Optimize it and correct the errors.
MODULE PolySingV0_63(SYSMODULE)
    ! PolySing : This module combines Singularity Detection, Direct Kinematic (through DH params), Geometric Jacobian Calculation, SVD Computation.
    !
    ! Last Update: 14/08/2024
    ! Author: Paolo Furia (<PRESIDIO_ANONYMIZED_EMAIL_ADDRESS>)
    ! 
    ! ChangeLog:
    !       V0_1 (22/08/24): First version of Singularity detection and movement based on the determinant imported from external software
    !       V0_2 (1/08/24): Added comments and code clarifications
	!       V0_3 (6/08/24): Added calculation of direct kinematics via Denavit-Hartenberg parameters -> rototranslation matrix for each joint up to TCP; added QR algorithm for calculation of eigenvalues.
	!       V0_4 (7/08/24): Used internal robotstudio functions for some steps. Added Singular Value Decomposition -> for redundant robots (#joint<>#DOF); added condition number nIndex for matrix J;
    !       V0_5 (9/08/24): The number of interpolated points depends on the nDistance of the points (minimum nDistance between the point can be changed). All calculation (determinant too) is made here.
    !       V0_6 (13/08/2024): Added reachability checks for the interpolated point. Change function names for clarification
    !       V0_61 (11/09/2024): Fixed SLERP bugs and MoveAbsJ bugs
    !       V0_62 (12/09/2024): Added SingArea \Wrist and ConfJ \Off in the joint movement to prevent changes of configuration for better performance
    !       V0_63 (12/09/2024): Fixed interpolation bug
    
    
    ! STANDARD PREFIX  
    ! t     tooldata        Tool
    ! w     wobjdata        WorkObject
    ! s     speeddata       Speed 
    ! p     robtarget       Punto x,y,z
    ! n     num             Numeric
    ! dn    dnum            Double numeric
    ! b     bool            Boolean
    ! i     intnum          Interrupt
    ! clk   clock           Timer
    ! wz    wzstationary    WorldZone
    ! dev   iodevice        I/O Device
    ! raw   rawbytes        Data for communication
    ! by    byte            Bytes
    ! s     string          String
    ! sp    shapedata       Shape Of WorldZone
    ! pos   Pos             Position x,y,z
    ! trg   triggdata       Trigger
    ! dh    DH              Denavit Hartenberg struct 
    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # CONSTANTS AND VARIABLES!                                                                         #
    ! #                                                                                                  #
    ! ####################################################################################################
    
    !Denavit-Hartenberg parameters struct
    RECORD DH
        num n_theta;
        num n_d;
        num n_a;
        num n_alpha;
    ENDRECORD
    
    ! Size vals
    LOCAL CONST num nMaxSize:= 1E+04;                       ! [-]   Max nSize for each array
    LOCAL VAR num nSize;                                    ! [-]   Size calculated according to the nDistance between the points
    
    ! Interpolation parameters
    LOCAL VAR num nds:= 20;                                 ! [mm]  Distance between interpolated points  
    LOCAL VAR num ndt;                                      ! [-]   Reciprocal of the nSize variable
    VAR robtarget pInterpolated{nMaxSize};                  ! [-]   Array of interpolated path points in robtarget
    VAR jointtarget jInterpolated{nMaxSize};                ! [-]   Array of interpolated path points in jointtarget
    LOCAL VAR bool bIsReachableInterpolatedPoint:=TRUE;     ! [-]   Check if there are not reachable point in the path
    
    ! Degrees Of Freedom (DOF) of the robot
    LOCAL CONST num nDOF := 6;                              ! [-]   # of DOF of the robot
    
    !!! UNCOMMENT THE CORRECT ROBOT !!!
    ! IRB7600_325_310
    ! Convention: theta, d, a, alpha
    LOCAL CONST DH dhParams_init{nDOF}:=
        [
            [0,     0.78,   0.41,   -90 ],
            [-90,   0,      1.075,  0   ],
            [0,     0,      0.165,  -90 ],
            [0,     1.556,  0,      90  ],
            [0,     0,      0,      -90 ],
            [180,   0.25,   0,      0   ]
        ];                                                  ! [-] DH Parameters for the SPECIFIC ROBOT. !!! CHANGE ACCORDINGLY TO THE ROBOT USED !!!!

    ! IRB4600_40_255 
!    LOCAL CONST DH dhParams_init{nDOF}:=
!        [
!            [0,     0.495,     0.175,   -90 ],
!            [-90,   0,         1.095,  0   ],
!            [0,     0,         0.175,  -90 ],
!            [0,     1.270,     0,      90  ],
!            [180,     0,       0,      90 ],
!            [0,    0.085,      0,      0   ]
!        ];

! IRB4600_20_250 
!    LOCAL CONST DH dhParams_init{nDOF}:=
!        [
!            [0,     0.495,     0.175,   -90 ],
!            [-90,   0,         1.095,  0   ],
!            [0,     0,         0.175,  -90 ],
!            [0,     1.2305,     0,      90  ],
!            [180,     0,       0,      90 ],
!            [0,    0.085,      0,      0   ]
!        ];
    LOCAL VAR DH dhParams{nDOF};                            ! [-] DH Parameters var that changes during direct kinematic calculation 
    
    ! Index condition
    LOCAL CONST num nIndexCondition := 0.01;                ! [-] Singularity condition threshold value. Ratio of smallest to larger singular value of the jacboian matrix. If the nIndex is smallest than nIndexCondition we are in singularity condition
    
    ! DEBUG values
    VAR num nJacobian_vals{6,nDOF,nMaxSize};                ! [-] Matrix that holds all the Geometric Jacobian calculated in all the interpolated points
    VAR num nIndex_vals{nMaxSize};                          ! [-] Array that holds Singularity Condition values.
    VAR num nDet_vals{nMaxSize};                            ! [-] Array that holds all the values of the determinant calculated in all the interpolated points
    VAR num nSingular_vals{nDOF, nMaxSize};                 ! [-] Matrix that stores the singular values for each robtarget interpolated point
    
    VAR num nIndexCondition_counter:=0;
    VAR num nIndexCondition_vals{nMaxSize};                 ! This array store all the interpolated point nIndex that satisfies the singularity condition
    
    ! Errors
    VAR errnum  ERR_MAT_SIZE:=-1;                           ! [-] Error for matrix operations
    
    !DH_Init : Initialize DH parameters for the SPECIFIC ROBOT MODEL.  
    LOCAL PROC DH_Init()
        ! Convention: n_theta, n_d, n_a, n_alpha
        dhParams{1}:=dhParams_init{1};
        dhParams{2}:=dhParams_init{2};
        dhParams{3}:=dhParams_init{3};
        dhParams{4}:=dhParams_init{4};
        dhParams{5}:=dhParams_init{5};
        dhParams{6}:=dhParams_init{6};        
    ENDPROC
    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # END CONSTANTS AND VARIABLES                                                                      #
    ! #                                                                                                  #
    ! ####################################################################################################
    
    !________________________________________________________________________________________________________________________________________________________________________
    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # START PUBLIC FUNCTION AND PROC                                                                   #
    ! #                                                                                                  #
    ! ####################################################################################################
    
    !CheckSingularity: Check singularity along n_a linear path
    ! Input:
    !   - <PARAM> pFinal:           Final/Destination robtarget point
    !   - <PARAM> vel :             velocity at which both the robot and the external axes move
    !   - <PARAM> zone :            How close to the programmed position the axes must be before moving towards the next position
    !   - <PARAM> tToolToUse :      Characteristics of the toola as position and orientation of the tool center point (TCP) and the physical characteristics of the tool load
    !   - <PARAM> wobjToUse :       Work object data
    !   - <PARAM_OPT> nds :         Distance between interpolated points  
    FUNC bool CheckSingularity(robtarget pFinal, speeddata vel, zonedata zone, PERS tooldata tToolToUse, PERS wobjdata wobjToUse, \num nds_in)
        
        ! Support variables
        VAR bool bIsSing:=FALSE;
        VAR num nDistance;
        VAR robtarget pInitial;
        
        ! Change distance between interpolated point
        IF Present(nds_in) THEN
            nds := nds_in;    
        ENDIF
        
        ! Initialize DH parameters for the specific robot
        DH_Init;
        
        ! Get the current robotarget pose
        pInitial := CRobT(\Tool:=tToolToUse \WObj:=wobjToUse);
        
        ! Calculate nDistance between the actual robotarget point pInitial and the end robotarget point pFinal:
        nDistance := Euclidean_distance(pInitial, pFinal);
        
        ! Calculate the interpolating nSize according to the nDistance. The nDistance between the interpolated point is equal to nds
        nSize := Round(nDistance/nds)+1;
        ndt := 1/nSize;
        
        ! Make sure the calculated nSize doesn't exceed the maximum nSize
        IF nSize > nMaxSize THEN
            ErrWrite \I, "POLYSING:", "Warning: Calculated nSize exceeds nMaxSize, limiting to nMaxSize.";
            nSize := nMaxSize;
        ENDIF

        ! Interpolate the path
        PathInterpolation pInitial, pFinal, pInterpolated, tToolToUse \wobjToUse:= wobjToUse ;
        
        ! Check for singularity along the interpolated path
        bIsSing := CheckSingularityAlgorithm(pInterpolated, tToolToUse, wobjToUse);
        
        RETURN bIsSing;
    ENDFUNC
    
    !MoveL_Sing : Move the TCP according to the singularity
    ! Input:
    !   - <PARAM> pFinal :      Final/Destination robtarget point
    !   - <PARAM> vel :         velocity at which both the robot and the external axes move
    !   - <PARAM> zone :        How close to the programmed position the axes must be before moving towards the next position
    !   - <PARAM> tToolToUse :  Characteristics of the toola as position and orientation of the tool center point (TCP) and the physical characteristics of the tool load
    !   - <PARAM> wobjToUse :   Work object data
    PROC MoveL_Sing(robtarget pFinal, speeddata vel, zonedata zone, PERS tooldata tToolToUse, PERS wobjdata wobjToUse, \num nds_in)
        
        ! Support variables
        VAR string sOut;
        VAR bool bIsSing:=FALSE;
        
        ! Calculate the singularity
        IF Present(nds_in)=TRUE     THEN bIsSing := CheckSingularity(pFinal, vel, zone,  tToolToUse,  wobjToUse, \nds_in:=nds_in);  ENDIF
        IF Present(nds_in)=FALSE    THEN bIsSing := CheckSingularity(pFinal, vel, zone,  tToolToUse,  wobjToUse);                   ENDIF
        
        ! Move the robot accordingly to the singularity
        IF bIsSing AND bIsReachableInterpolatedPoint=TRUE THEN
            
            FOR t FROM 1 TO nSize STEP 1 DO
                !DEBUG
!                TPWrite "J val: " + NumToStr(jInterpolated{t}.robax.rax_1,6) + "; " + NumToStr(jInterpolated{t}.robax.rax_2,6)+ "; " + NumToStr(jInterpolated{t}.robax.rax_3,6)+ "; " + NumToStr(jInterpolated{t}.robax.rax_4,6)+ "; " + NumToStr(jInterpolated{t}.robax.rax_5,6)+ "; " + NumToStr(jInterpolated{t}.robax.rax_6,6);
!                sOut:="[";
!                FOR n FROM 1 TO nDOF DO
!                    sOut:=sOut + NumToStr(nSingular_vals{n,t},4) + "   ";
!                ENDFOR
!                sOut:=sOut + "] ";
!                TPWrite sOut;
!                TPWrite "nIndex: " + NumToStr(nIndex_vals{t},4);
!                TPWrite " nDet: " + NumToStr(nDet_vals{t},4);
!                TPWrite " ";
                SingArea \Wrist;
                ConfJ \On;
                !ConfL \On;
                MoveJ pInterpolated{t},vel,z1,tToolToUse\WObj:=wobjToUse;
                !MoveAbsJ jInterpolated{t},vel,z1,tToolToUse\WObj:=wobjToUse;
            ENDFOR
            !MoveL pInterpolated{nSize},vel,zone,tool0\WObj:=wobj0;
            
            !DEBUG
!            sOut:="[";
!            FOR n FROM 1 TO nDOF DO
!                    sOut:=sOut + NumToStr(nSingular_vals{n,nSize},4) + "   ";
!            ENDFOR
!            sOut:=sOut + "] ";
!            sOut:=sOut +"nIndex: " + NumToStr(nIndex_vals{nSize},4);
            !TPWrite sOut;
            !SingArea \Off;
        ELSE 
            MoveL pFinal, vel, zone, tToolToUse \WObj:=wobjToUse;
        ENDIF
    ENDPROC
    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # END PUBLIC FUNCTION AND PROC                                                                     #
    ! #                                                                                                  #
    ! ####################################################################################################
    
    !________________________________________________________________________________________________________________________________________________________________________
    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # START LOCAL FUNCTION AND PROC                                                                    #
    ! #                                                                                                  #
    ! ####################################################################################################
    
    !CheckSingularityAlgorithm : Check if there is n_a singularity in the array of points
    ! Input: 
    !   - <PARAM> pInterpolatedIN{*} :      Trajectory path interpolated
    !   - <PARAM> tToolToUse :              Tool used
    !   - <PARAM> wobjToUse :               Work obj used
    ! Output:
    !   - <PARAM> bIsSing:                  TRUE if n_a singularity is present, FALSE otherwise
    LOCAL FUNC bool CheckSingularityAlgorithm(INOUT robtarget pInterpolatedIN{*}, PERS tooldata tToolToUse, PERS wobjdata wobjToUse) 
        
        ! Support variables
        VAR jointtarget jSupport;
        VAR bool bIsSing := FALSE;      
        VAR dnum dnJac{6,nDOF};
        VAR num nIndex;              ! Index : Ratio of the smallest to the largest singular value. Under conditions of singularity you will have 0/x=0
        VAR num nDet;
        ! Support variables for SVD decomposition
        VAR dnum dnU{6,6};
        VAR dnum dnS{nDOF};
        VAR dnum dnV{nDOF, nDOF};
        
        ! Main Loop : Check singularity in all the interpolated points
        FOR i FROM 1 TO nSize DO
            
            ! Initialize the determinant
            nDet := 1;
            
            ! Calculate joint values at the given TCP point
            jSupport := CalcJointT(pInterpolatedIN{i}, tToolToUse \WObj:=wobjToUse);
            
            ! Save the joint variables in an array for DEBUG Purposes
            jInterpolated{i}:=jSupport;
            
            ! Calculate Geometric Jacobian for the given joint configuration
            !TODO: Warning, if the robot is redundant this goes in error.          
            CalcJac dnJac, dhParams, [jSupport.robax.rax_1, jSupport.robax.rax_2, jSupport.robax.rax_3, jSupport.robax.rax_4, jSupport.robax.rax_5, jSupport.robax.rax_6];
            
            ! DEBUG : Save jacobian
            FOR row FROM 1 TO 6 DO
                FOR col FROM 1 TO nDOF DO
                    nJacobian_vals{row,col,i}:=DnumToNum(dnJac{row,col});
                ENDFOR
            ENDFOR
            
            ! Perform the SVD
            MatrixSVD dnJac, dnU, dnS, dnV;
            
            ! Calculate the determinant and save it
            FOR k FROM 1 TO Dim(dnS,1) DO
                ! Calculate the determinant
                nDet:=nDet*DNumToNum(dnS{k});
                ! Store the eigenvalues
                nSingular_vals{k,i}:=DnumToNum(dnS{k});
            ENDFOR
            nDet_vals{i}:=ABS(nDet);
            
            ! Calculate the nIndex
            nIndex := DnumToNum(dnS{Dim(dnS,1)}/dnS{1});
            
            ! DEBUG Store the nIndex value
            nIndex_vals{i}:=nIndex;
            
            ! Check singularity condition
            IF Abs(nIndex) < nIndexCondition THEN
                ! Store which interpolated point satisfies nIndexCondition
                Incr nIndexCondition_counter;
                nIndexCondition_vals{nIndexCondition_counter}:=i;
                
                bIsSing := TRUE;
            ENDIF
        ENDFOR      
        
        ! Return Singularity check result
        IF bIsSing THEN
            ErrWrite\W, "POLYSING:", "Singularity detected in the path.";
            Print_pTarget_sing;
            RETURN bIsSing;
        ELSEIF bIsSing=FALSE THEN 
            ErrWrite \W, "POLYSING:", "No singularity in the path.";
            RETURN bIsSing;
        ENDIF
        
        ! If the algorithm goes in error
        ERROR 
            IF ERRNO=ERR_ROBLIMIT  OR ERRNO=ERR_OUTSIDE_REACH OR ERRNO=ERR_WOBJ_MOVING THEN
                ErrWrite \W, "POLYSING:", "There is a NOT REACHABLE point IN the path!";
                bIsReachableInterpolatedPoint:=FALSE;
                bIsSing:=TRUE;
                RETURN bIsSing;
            ENDIF
            TRYNEXT;
    ENDFUNC
        
    !CalcJac : Compute the direct kinematic rototranslation matrix and the Geometric Jacobian
    ! Input:
    !   - <PARAM> dnJac{*,*} :          Jacobian matrix
    !   - <PARAM> dhParameters{*} :     Denavit-Hartenberg parameters for the SPECIFIC ROBOT
    !   - <PARAM> n_q_vals{*,*} :       joint values
    LOCAL PROC CalcJac(INOUT dnum dnJac{*,*}, INOUT DH dhParameters{*}, num n_q_vals{*})
        
        ! Support parameters
        VAR dnum dnZ{3, nDOF + 1};
        VAR dnum dnP{3, nDOF + 1};
        VAR dnum dnA0iall{4, 4, nDOF + 1};
        VAR dnum dnAim1iall{4, 4, nDOF + 1};
        VAR dnum dnA06_temp{4, 4};
        VAR dnum dnAi{4, 4};
        VAR dnum dnZi{3,1};
        VAR dnum dnPi{3,1};
        VAR dnum dnPn{3,1};
        VAR dnum dnCrossProductRes{3,1};
        ! Rototranslation matrix
        VAR dnum dnA06{4, 4}; 
        
        ! Add actual current joint values
        FOR i FROM 1 TO nDOF DO
            dhParameters{i}.n_theta := n_q_vals{i} + dhParams_init{i}.n_theta;
        ENDFOR
        
        ! Initialize dnA06 to identity matrix
        Init_mat_identity dnA06;

        dnZ{3, 1} := 1; ! Initial dnZ vector

        FOR i FROM 1 TO nDOF DO
            
            ! Compute the roto-translation matrices according to DH params
            DHTransform dhParameters{i}.n_alpha, dhParameters{i}.n_a, dhParameters{i}.n_theta, dhParameters{i}.n_d, dnAi ;
            
            !Calculate the reference frame wrt the previous one.  !!!! IF ROBOTWARE VERSION >= 6.15.04 USE BUILT-IN FUNCTION MatrixMult intstead of Matrix_Mult
            Matrix_Mult  dnA06, dnAi, dnA06_temp ;
            !MatrixMult  dnA06, dnAi, dnA06_temp ;
            
            dnA06 := dnA06_temp;
            
            !DEBUG
            FOR r FROM 1 TO 4 DO
                FOR c FROM 1 TO 4 DO
                    dnA0iall{r, c, i} := dnA06{r, c};
                    dnAim1iall{r, c, i} := dnAi{r, c};
                ENDFOR
            ENDFOR

            FOR j FROM 1 TO 3 DO
                dnZ{j, i + 1} := dnA06{j, 3};
                dnP{j, i + 1} := dnA06{j, 4};
            ENDFOR
            
        ENDFOR
        
        dnPn{1,1} := dnA06{1, 4};
        dnPn{2,1} := dnA06{2, 4};
        dnPn{3,1} := dnA06{3, 4};

        FOR i FROM 1 TO nDOF DO
            
            FOR j FROM 1 TO 3 DO
                dnZi{j,1} := dnZ{j, i};
                dnPi{j,1} := dnP{j, i};
            ENDFOR
            
            Matrix_Sub dnPn,dnPi,dnCrossProductRes; !!!! IF ROBOTWARE VERSION >= 6.15.04 USE BUILT-IN FUNCTION MatrixSub intstead of Matrix_Sub
            !MatrixSub dnPn,dnPi,dnCrossProductRes;
            
            CrossProduct dnZi, dnCrossProductRes, dnCrossProductRes;

            FOR j FROM 1 TO 3 DO
                dnJac{j, i} := dnCrossProductRes{j,1};
                dnJac{j + 3, i} := dnZi{j,1};
            ENDFOR
            
        ENDFOR
    ENDPROC
    
    !DHTransform : Perform rototranslational matrix from frame i-1 to frame i using Denavit-Hartenberg convention
    ! Input:
    !   - <PARAM> n_alpha : 
    !   - <PARAM> n_a :
    !   - <PARAM> n_theta:
    !   - <PARAM> n_d:
    ! Output:
    !   - <PARAM> matrix{*,*} : Rototranslation matrix for the given frame
    LOCAL PROC DHTransform(num n_alpha, num n_a, num n_theta, num n_d, INOUT dnum matrix{*,*})
        ! Support parameters
        VAR dnum dnCosTheta;
        VAR dnum dnSinTheta;
        VAR dnum dnCosAlpha;
        VAR dnum dnSinAlpha;
        
        dnCosTheta := NumToDNum(Cos(n_theta));
        dnSinTheta := NumToDNum(Sin(n_theta));
        dnCosAlpha := NumToDNum(Cos(n_alpha));
        dnSinAlpha := NumToDNum(Sin(n_alpha));

        matrix{1, 1} := dnCosTheta;
        matrix{1, 2} := -dnSinTheta * dnCosAlpha;
        matrix{1, 3} := dnSinTheta * dnSinAlpha;
        matrix{1, 4} := NumToDNum(n_a) * dnCosTheta;

        matrix{2, 1} := dnSinTheta;
        matrix{2, 2} := dnCosTheta * dnCosAlpha;
        matrix{2, 3} := -dnCosTheta * dnSinAlpha;
        matrix{2, 4} := NumToDNum(n_a) * dnSinTheta;

        matrix{3, 1} := 0;
        matrix{3, 2} := dnSinAlpha;
        matrix{3, 3} := dnCosAlpha;
        matrix{3, 4} := NumToDNum(n_d);

        matrix{4, 1} := 0;
        matrix{4, 2} := 0;
        matrix{4, 3} := 0;
        matrix{4, 4} := 1;
    ENDPROC
    
    !CrossProduct : perform cross product between two 3D vectors
    ! INFO: https://en.wikipedia.org/wiki/Cross_product. 
    ! Input:
    !   - <PARAM> A{*} : First vector
    !   - <PARAM> B{*} : Second vector
    ! Output:
    !   - <PARAM> dnResult{*} : Save the dnResult. AxB=dnResult
    LOCAL PROC CrossProduct(dnum A{*,*}, dnum B{*,*}, INOUT dnum dnResult{*,*})
        
        IF Dim(A,1)=Dim(A,1) THEN
            dnResult{1,1} := A{2,1} * B{3,1} - A{3,1} * B{2,1};
            dnResult{2,1} := A{3,1} * B{1,1} - A{1,1} * B{3,1};
            dnResult{3,1} := A{1,1} * B{2,1} - A{2,1} * B{1,1};
        ELSE
            ErrWrite \W, "POLYSING:", "ERROR: VECTOR MUST BE OF EQUAL SIZE";
            !TODO: Handle correctly the error
        ENDIF
    ENDPROC
    
    !PrintArray : function to print an array on the TP
    ! Input:
    !   - <PARAM> dnArr{*} : Array to be printed
    LOCAL PROC PrintArray(dnum dnArr{*})
        ! Support variables
        VAR string sOut;
        sOut:="[";
        FOR i FROM 1 TO Dim(dnArr,1) DO
            sOut:= sOut + DNumToStr(dnArr{i},4) + "   ";
        ENDFOR
        TPWrite sOut + "]";
    ENDPROC

    !PrintMatrix2D : function to print n_a 2D matrix on the TP
    ! Input:
    !   - <PARAM> dnMat{*} : Matrix to be printed
    LOCAL PROC PrintMatrix2D(dnum dnMat{*,*})
        ! Support variables
        VAR string sOut;
        
        FOR i FROM 1 TO Dim(dnMat,1) DO
            sOut:="";
            FOR j FROM 1 TO Dim(dnMat,2) DO
                sOut:= sOut + DNumToStr(dnMat{i,j},4) + "  ";
            ENDFOR
            TPWrite sOut + ";";
        ENDFOR
    ENDPROC
    
    !PrintMatrix3D : function to print n_a 2D matrix on the TP
    ! Input:
    !   - <PARAM> nMat{*} : Matrix to be printed
    LOCAL PROC PrintMatrix3D(num nMat{*, *, *}, num t)
        ! Support variables
        VAR string sOut;
        
        FOR i FROM 1 TO Dim(nMat,1) DO
            sOut:="";
            FOR j FROM 1 TO Dim(nMat,2) DO
                sOut:= sOut + NumToStr(nMat{i,j,t},4) + "  ";
            ENDFOR
            TPWrite sOut + ";";
        ENDFOR
    ENDPROC
    
    !Init_mat_identity: Initialize to an identity matrix n_a given 2D matrix
    ! Input: 
    !   - <PARAM> dnMat{*,*} : Input matrix
    LOCAL PROC Init_mat_identity(INOUT dnum dnMat{*,*})
        FOR i FROM 1 TO Dim(dnMat,1) DO
            FOR j FROM 1 TO Dim(dnMat,2) DO
                IF i = j THEN
                    dnMat{i, j} := 1;
                ELSE
                    dnMat{i, j} := 0;
                ENDIF
            ENDFOR
        ENDFOR
    ENDPROC

    
    !PathInterpolation :  SLERP interpolation path given two point 
    ! INFO: https://en.wikipedia.org/wiki/Slerp
    ! Input:
    !   - <PARAM> p1 : Starting path point
    !   - <PARAM> p2 : End path point
    !   - <PARAM> pInterpolatedIN{*} : Array of interpolated robotarget point
    LOCAL PROC PathInterpolation(robtarget p1, robtarget p2, INOUT robtarget pInterpolatedIN{*}, PERS tooldata tToolToUse \PERS wobjdata wobjToUse )
        !Support variables
        VAR robtarget dnResult;
        VAR num count := 1;
        VAR num t:= 0;
        VAR jointtarget jResult;
        VAR jointtarget jResultPrev;
        VAR robtarget dnResultPrev;
        
        
        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.robconf.cfx := p1.robconf.cfx;
!        dnResult.extax := p1.extax;
!        IF count<> 1 THEN
!            jResult := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
!            IF jResultPrev.robax.rax_5 + jResult.robax.rax_5<=0.1  AND jResultPrev.robax.rax_5<0 THEN
!                dnResult.robconf.cfx:=1;
!            ELSEIF jResultPrev.robax.rax_5 + jResult.robax.rax_5<=0.1  AND jResultPrev.robax.rax_5>0 THEN
!                dnResult.robconf.cfx:=0;
!            ENDIF
!            jResultPrev:=jResult;
!        ELSEIF count = 1 THEN
!            jResultPrev := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
!        ENDIF
!        IF 0<jResult.robax.rax_1 AND jResult.robax.rax_1<= 90 THEN
!            dnResult.robconf.cf1:=1;
!        ELSEIF  90<jResult.robax.rax_1 AND jResult.robax.rax_1<= 180 THEN
!            dnResult.robconf.cf1:=2;
!        ELSEIF  -90<jResult.robax.rax_1 AND jResult.robax.rax_1<= 0 THEN
!            dnResult.robconf.cf1:=-1;
!        ENDIF
            
            
        
        ! Set confdata to undefined to allow the controller to find the best configuration
!        dnResult.robconf := [9E+09, 9E+09, 9E+09, 9E+09];
!        dnResult.extax := [9E+09, 9E+09, 9E+09, 9E+09, 9E+09, 9E+09];
        
        ! 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

    
    !Euclidean_distance : Calculate the euclidean distance between two robtarget points p1 and p2
    LOCAL FUNC num Euclidean_distance(robtarget p1, robtarget p2)
        ! Support variables
        VAR num nDistance;
        ! Calculate Euclidean distance
        nDistance := SQRT(
            Pow(p2.trans.x - p1.trans.x, 2) +
            Pow(p2.trans.y - p1.trans.y, 2) +
            Pow(p2.trans.z - p1.trans.z, 2)
        );
        RETURN nDistance;
    ENDFUNC
    
    !Print_pTarget_sing :  Write on TP all the robotarget that satisfies the singularity condition 
    LOCAL PROC Print_pTarget_sing()
        
        ! Support variable
        VAR string sOut;
        VAR num i;
        
        sOut:= "There are " + NumToStr(nIndexCondition_counter,1) + " interpolated point in Singularity Condition";
        TPWrite sOut;
        
        FOR k FROM 1 TO nIndexCondition_counter DO
            i:=nIndexCondition_vals{k};
            TPWrite "["+NumToStr(k,0) + "]: "+ "[[" + NumToStr(pInterpolated{i}.trans.x,3)+", "+ NumToStr(pInterpolated{i}.trans.y,3)+ ", " + NumToStr(pInterpolated{i}.trans.z,3)+"],["+ NumToStr(EulerZYX(\X, pInterpolated{i}.rot),3) + ", "+ NumToStr(EulerZYX(\Y, pInterpolated{i}.rot),3) + ", " + NumToStr(EulerZYX(\Z, pInterpolated{i}.rot),3)+"]]" + " indx: " + NumToStr(nIndex_vals{i}, 5); 
        ENDFOR
        
    ENDPROC
    
    !Matrix_Mult : Procedure to multiply 2D matrices
    LOCAL PROC Matrix_Mult(dnum dnMatA{*,*}, dnum dnMatB{*,*}, INOUT dnum dnMatResult{*,*})
    
        !Support variables
        VAR num nRowA;
        VAR num nColA;
        VAR num nRowB;
        VAR num nColB;
        VAR num nRowResult;
        VAR num nColResult;
    
        BookErrNo  ERR_MAT_SIZE;
    
        nRowA := Dim(dnMatA, 1);
        nColA := Dim(dnMatA, 2);
        nRowB := Dim(dnMatB, 1);
        nColB := Dim(dnMatB, 2);
        nRowResult := Dim(dnMatResult, 1);
        nColResult := Dim(dnMatResult, 2);
    
        ! Check if the dimensions match for multiplication
        IF nColA <> nRowB OR nRowA <> nRowResult OR nColB <> nColResult THEN
            RAISE ERR_MAT_SIZE;
        ENDIF
    
        ! Perform multiplication
        FOR i FROM 1 TO nRowA DO
            FOR j FROM 1 TO nColB DO
                dnMatResult{i,j} := 0;
                FOR k FROM 1 TO nColA DO
                    dnMatResult{i,j} := dnMatResult{i,j} + dnMatA{i,k} * dnMatB{k,j};
                ENDFOR
            ENDFOR
        ENDFOR
    
        ! Error handling
        ERROR
            IF ERRNO = ERR_MAT_SIZE THEN
                ErrWrite \W, "SING_CHECK:", "Wrong dimensions or wrong values in 'Matrix_Mult'";
            ENDIF 
    ENDPROC

    
    !Matrix_Sub : Procedure to subtract 2D matrices
    LOCAL PROC Matrix_Sub(dnum dnMatA{*,*}, dnum dnMatB{*,*}, INOUT dnum dnMatResult{*,*})
    
        !Support variables
        VAR num nRowA;
        VAR num nColA;
        VAR num nRowB;
        VAR num nColB;
        VAR num nRowResult;
        VAR num nColResult;
    
        BookErrNo  ERR_MAT_SIZE;
    
        nRowA := Dim(dnMatA, 1);
        nColA := Dim(dnMatA, 2);
        nRowB := Dim(dnMatB, 1);
        nColB := Dim(dnMatB, 2);
        nRowResult := Dim(dnMatResult,1);
        nColResult := Dim(dnMatResult,2);
    
        ! Check if dimensions match
        IF nRowA <> nRowB OR nColA <> nColB OR nRowA <> nRowResult OR nColA <> nColResult THEN
            RAISE ERR_MAT_SIZE;
        ENDIF
    
        ! Perform subtraction
        FOR row FROM 1 TO nRowA DO
            FOR col FROM 1 TO nColA DO
                dnMatResult{row,col} := dnMatA{row,col} - dnMatB{row,col};
            ENDFOR
        ENDFOR
    
        ! Error handling
        ERROR
            IF ERRNO = ERR_MAT_SIZE THEN
                ErrWrite \W, "SING_CHECK:", "Wrong dimensions or wrong values in 'Matrix_Sub'";
            ENDIF 
    ENDPROC

    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # END LOCAL FUNCTION AND PROC                                                                      #
    ! #                                                                                                  #
    ! ####################################################################################################

ENDMODULE