Bug in assert that checks f within bounds for IKasin/IKacos

[proyan]

Hello @rdiankov
Thanks for this wonderful library. I recently encountered a problem with IKFast, that I'll describe below. Unfortunately I can't provide a reproducible example because of the time constraints and the really specific nature of the problem, but I hope the problem and solution are easy to understand.
I am available for any information that you might require.
Best,
Rohan

Setup:
I'm using the IKFast feature of the full openrave installation (as a standalone command).
It was possible for me to use it on our robot without any issues for more than a year. Then suddenly, in a specific problem, at one instant, I had the following assertion failed (line 131):

 129 inline double IKasin(double f)                                                                                                                                                                                
 130 {                                                                                                                                                                                                             
 131 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver                                                                      
 132 if( f <= -1 ) return -IKPI_2;                                                                                                                                                                                 
 133 else if( f >= 1 ) return IKPI_2;                                                                                                                                                                              
 134 return asin(f);                                                                                                                                                                                               
 135 }                                                                                                                                                                                                                                                                                   

Problem
After some digging, I found out that the problem is that the equality condition is not taken into account in the assert.

Before IKasin is called, there is a check that verifies - 1- thresh < f < 1 + thresh. But when f == -1-thresh or f==1+thresh, this check is not triggered, and the function is called regardless, which then fails

Solution
A really simple solution is to allow equality inside the assertion at the time of code generation.
For e.g., the following line in

openrave/python/ikfast_generator_cpp_sympy0_6.py

Line 286 in a982f0a

IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver

would change to

 IKFAST_ASSERT( f >= -1-IKFAST_SINCOS_THRESH && f <= 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver 

There are a few more instances of this assert in the generator file. The same change would apply there as well.

Test
I tried it on my problem and it worked.