Add IK Test Cases for Non serial chains

[StoneT2000]

Tests for #42

Visualized looks like this:

Not sure why some generated IK goals are under the floor, maybe the robot URDF has poorly set joint limits?

Otherwise seems to all solve anyway:

Testing kuka_iiwa IK
IK took 0.373344 seconds
IK converged number: 2673 / 10000
IK took 30 iterations
IK solved 912 / 1000 goals
Testing widowx IK
IK took 0.244910 seconds
IK converged number: 3472 / 10000
IK took 30 iterations
IK solved 977 / 1000 goals

[LemonPi]

The tests failed because of some pathing issues to the new ur5 robot I added for a test - could you rebase this branch onto the latest master?

The goals are generated by sampling joint values within the joint limits without considering obstacles, so the floor is actually cosmetic. Adding constraints to IK would be interesting. The naive way is probably to accept soft constraints as a differentiable external cost function on the joint values q. Another way would be to do gradient projection using the null space of the Jacobian to satisfy the constraints.