Added Dumped Least Square mode to kineIK
Changed initial value at IK to improve stability
Changed test file to test all 3 modes and plot the error