Issues regarding small IK Twists (~1e-6)

[IanTheEngineer]

@MatthewPeterKelly can you add some extra color here?

[MatthewPeterKelly]

This gets complicated...

About a year ago I did some testing with a new trajectory IK algorithm that used SNS-IK for the core IK solver. I observed that SNS-IK sometimes gave poor results (especially with regard to task scaling) when the desired twist values were smaller than roughly 1e-6. I didn't have time to dive into the details of what was going wrong, so instead side-stepped the issue by bypassing SNS-IK in these situations.

It is unclear where the issue is coming from, and I'm not completely sure that the trouble was in SNS-IK, although that is my prime suspect. Within SNS-IK, my guess is that the hard-coded thresholds in the singular value decomposition might have something to do with it.

What to do for this ticket?

Create a unit test that exercises SNS-IK for twist with angular velocity norms that are distributed on a log scale between 1e-10 and 1e-4. The solver should return reasonable results in all cases.