Improve tolerance at fully extended configurations

[gkjohnson]

Once a system has been fully extended like the following it can get "stuck" in that pose with any attempt to solve resulting in divergence because moving one of the fully extended joints even a small amount will result in it diverging before it converges.

Some things to look in to:

• Find joint angles that cause these fully extended configurations (the angles that cause two connected links to be parallel) and avoid them.
• Adjust the angles of the extended joints if they happen to be parallel and the solve diverged. Or adjust all angles to afford more "slack" in the system.
• Analyze the resulting joint deltas to see if we can adjust certain joints based on that.

[gkjohnson]

Taking a step towards a known good pose like the rest pose before solve seems to alleviate this issue a bit but can cause jumping when full extended as the solve diverges and resets the previous position and another step is taken the next frame. Perhaps the step can be undone if the solve diverges?

EDIT: Only setting the angles if the solution CONVERGES seems to be a good compromise. It lets the IK solution thrash around but only applies valid angles to visible model