search

RethinkRobotics-opensource / sns_ik

Saturation in the Null Space (SNS) Inverse Kinematic Library
82 stars 41 forks source link

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

Open IanTheEngineer opened 6 years ago

IanTheEngineer commented 6 years ago

@MatthewPeterKelly can you add some extra color here?

MatthewPeterKelly commented 6 years ago

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.