Checking for Singularity

SteveMob

Hello,

I am sending the robot arbitrary target data via Sockets. I am attempting to check for singularity issues while the robot is moving. I am currently using a timed interrupt TRAP routine that is checking the joint angles 10 times a second. If the axis 4 or 5 joint angles are within +/-0.1, I want to turn SingArea\Wrist active, and when it is done with the move, SingArea\Off.
It isn't really working how I want it to... Anytime it detects a singularity issue, it ends program execution, but I want the program to continue running, and just have the robot deviate from its linear path for a short time to avoid singularity, then continue linear movement. Has anyone done this before, or can anyone offer any advice on how to get an active singularity detection while the robot is moving?
My code:
CONNECT timeInt2 WITH IntRoutineCheckSingularity;
ITimer 0.1,timeInt2;
TRAP IntRoutineCheckSingularity
        VAR jointtarget currentJointAngles;
        currentJointAngles:=CJointT(\TaskName:="T_ROB1");

        IF ((currentJointAngles.robax.rax_5>-0.1) AND (currentJointAngles.robax.rax_5<0.1)) OR ((currentJointAngles.robax.rax_4>-0.1) AND (currentJointAngles.robax.rax_4<0.1)) THEN

            SingArea\Wrist;
            SocketSend clientSocket\Str:="***Singularity Off***";
        ELSE
            SocketSend clientSocket\Str:="Singularity On";
        ENDIF
ENDTRAP

Thanks for any help!
SM