20321- Not-a-number CalcJointT

lfbaillargeon
lfbaillargeon Canada
Hi,

I'm using a PROC to evaluate if a pos is reachable before moving to it as all the poses are dynamically computed in my project. Here is the code:

    FUNC jointtarget robotReach(robtarget ToPoint,tooldata tool,\wobjdata WObj)
        !LOCAL VAR
        VAR jointtarget jtToPoint;
        VAR num nCalcResult:=0;

        IF ToPoint=[[0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,0,0,0]] RETURN [[0,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        
        tdR1:=tool;

        IF Present(WObj) THEN
            !Check robot reach according to a work object;
            wdR1:=WObj;
            jtToPoint:=CalcJointT(ToPoint,tdR1,\WObj:=wdR1\ErrorNumber:=nCalcResult);
            break;
            jtToPoint.robax.rax_1:=jtToPoint.robax.rax_1+1;
            jtToPoint.robax.rax_2:=jtToPoint.robax.rax_2+1;
            jtToPoint.robax.rax_3:=jtToPoint.robax.rax_3+1;
            jtToPoint.robax.rax_4:=jtToPoint.robax.rax_4+1;
            jtToPoint.robax.rax_5:=jtToPoint.robax.rax_5+1;
            jtToPoint.robax.rax_6:=jtToPoint.robax.rax_6+1;
            IF nCalcResult<>0 THEN
                RETURN jtToPoint;
            ENDIF
        ELSE
            !Check robot reach according to its wobj0;
            jtToPoint:=CalcJointT(ToPoint,tdR1\ErrorNumber:=nCalcResult);
            break;
            jtToPoint.robax.rax_1:=jtToPoint.robax.rax_1+1;
            jtToPoint.robax.rax_2:=jtToPoint.robax.rax_2+1;
            jtToPoint.robax.rax_3:=jtToPoint.robax.rax_3+1;
            jtToPoint.robax.rax_4:=jtToPoint.robax.rax_4+1;
            jtToPoint.robax.rax_5:=jtToPoint.robax.rax_5+1;
            jtToPoint.robax.rax_6:=jtToPoint.robax.rax_6+1;
            IF nCalcResult<>0 THEN
                RETURN jtToPoint;
            ENDIF
        ENDIF

        RETURN [[0,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        
        ERROR
        IF ERRNO=ERR_NUM_LIMIT THEN
            RETURN [[1,2,3,4,5,6],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        ENDIF
    ENDFUNC


IF the function successes, it return a NULL jointtarget. If the value is non-NULL, then I can see in main code what axis is problematic. However, the CalcJointT sometimes returns values in the E-40 and even lower which triggers a controller alarm 20321 - Not-a-number. The alarm says:

 "A symbol of type 'num' was found holding an undefined number.
Consequences
The undefined number was replaced with '0'."

But the alarm just keeps triggering as if the controller wouldn't really write 0 to the data it were supposed to reset and just keeps seeing undefined values... Now the controller is stuck on that alarm even when I reboot it (triggers about 3 times/sec.....). PP to main doesn't reset either... Would really really hate to have to reset the controller memory and setup everything back to they way it were.

Also, why is the function returning invalid data for jointtarget data types num? It seems to me the position that I tried in the function is just completely unreachable with any combination of joint angles but to have to crash the whole controller seems a little harsh. Sadly the ERR_NUM_LIMIT only manages really huge values and not really really small.

Comments

  • lemster68
    lemster68 United States ✭✭✭
    Try to catch these really small numbers by evaluating each component.  Either IF x < .001 THEN
    Have you tried using ERR_ROBLIMIT in your error handler also?  Finally, change IF in error handler to a TEST CASE with a DEFAULT.  use a placeholder, <smt> or Stop so that you can have a moment to check the system variable ERRNO.  There might be other errors that you have not anticipated.
    Lee Justice