## fsolve doesn't give correct answer even with close initial guess

### sanam (view profile)

on 9 Jun 2019
Latest activity Commented on by sanam

### sanam (view profile)

on 9 Jun 2019
Accepted Answer by John D'Errico

### John D'Errico (view profile)

hello everyone
i'm trying to solve these equations with fsolve, but even with close initial guess it doesn't give me the correct answer. here are the equations:
function F=IK(x)
F=[cos(x(4))*sin(x(1))*sin(x(6)) - 1.0*cos(x(5))*cos(x(6))*sin(x(1))*sin(x(4)) + cos(x(1))*cos(x(2))*cos(x(3))*sin(x(4))*sin(x(6)) - 1.0*cos(x(1))*cos(x(2))*cos(x(6))*sin(x(3))*sin(x(5)) + cos(x(1))*cos(x(3))*cos(x(6))*sin(x(2))*sin(x(5)) + cos(x(1))*sin(x(2))*sin(x(3))*sin(x(4))*sin(x(6)) + cos(x(1))*cos(x(2))*cos(x(3))*cos(x(4))*cos(x(5))*cos(x(6)) + cos(x(1))*cos(x(4))*cos(x(5))*cos(x(6))*sin(x(2))*sin(x(3));
cos(x(1))*cos(x(5))*cos(x(6))*sin(x(4)) - 1.0*cos(x(1))*cos(x(4))*sin(x(6)) + cos(x(2))*cos(x(3))*sin(x(1))*sin(x(4))*sin(x(6)) - 1.0*cos(x(2))*cos(x(6))*sin(x(1))*sin(x(3))*sin(x(5)) + cos(x(3))*cos(x(6))*sin(x(1))*sin(x(2))*sin(x(5)) + sin(x(1))*sin(x(2))*sin(x(3))*sin(x(4))*sin(x(6)) + cos(x(2))*cos(x(3))*cos(x(4))*cos(x(5))*cos(x(6))*sin(x(1)) + cos(x(4))*cos(x(5))*cos(x(6))*sin(x(1))*sin(x(2))*sin(x(3));
cos(x(2))*cos(x(3))*cos(x(6))*sin(x(5)) + cos(x(2))*sin(x(3))*sin(x(4))*sin(x(6)) - 1.0*cos(x(3))*sin(x(2))*sin(x(4))*sin(x(6)) + cos(x(6))*sin(x(2))*sin(x(3))*sin(x(5)) + cos(x(2))*cos(x(4))*cos(x(5))*cos(x(6))*sin(x(3)) - 1.0*cos(x(3))*cos(x(4))*cos(x(5))*cos(x(6))*sin(x(2));
cos(x(1))*cos(x(3))*sin(x(2))*sin(x(5))*sin(x(6)) - 1.0*cos(x(5))*sin(x(1))*sin(x(4))*sin(x(6)) - 1.0*cos(x(1))*cos(x(2))*cos(x(3))*cos(x(6))*sin(x(4)) - 1.0*cos(x(1))*cos(x(6))*sin(x(2))*sin(x(3))*sin(x(4)) - 1.0*cos(x(1))*cos(x(2))*sin(x(3))*sin(x(5))*sin(x(6)) - 1.0*cos(x(4))*cos(x(6))*sin(x(1)) + cos(x(1))*cos(x(2))*cos(x(3))*cos(x(4))*cos(x(5))*sin(x(6)) + cos(x(1))*cos(x(4))*cos(x(5))*sin(x(2))*sin(x(3))*sin(x(6));
cos(x(1))*cos(x(4))*cos(x(6)) + cos(x(1))*cos(x(5))*sin(x(4))*sin(x(6)) - 1.0*cos(x(2))*cos(x(3))*cos(x(6))*sin(x(1))*sin(x(4)) - 1.0*cos(x(6))*sin(x(1))*sin(x(2))*sin(x(3))*sin(x(4)) - 1.0*cos(x(2))*sin(x(1))*sin(x(3))*sin(x(5))*sin(x(6)) + cos(x(3))*sin(x(1))*sin(x(2))*sin(x(5))*sin(x(6)) + cos(x(2))*cos(x(3))*cos(x(4))*cos(x(5))*sin(x(1))*sin(x(6)) + cos(x(4))*cos(x(5))*sin(x(1))*sin(x(2))*sin(x(3))*sin(x(6));
cos(x(3))*cos(x(6))*sin(x(2))*sin(x(4)) - 1.0*cos(x(2))*cos(x(6))*sin(x(3))*sin(x(4)) + cos(x(2))*cos(x(3))*sin(x(5))*sin(x(6)) + sin(x(2))*sin(x(3))*sin(x(5))*sin(x(6)) + cos(x(2))*cos(x(4))*cos(x(5))*sin(x(3))*sin(x(6)) - 1.0*cos(x(3))*cos(x(4))*cos(x(5))*sin(x(2))*sin(x(6));
sin(x(1))*sin(x(4))*sin(x(5)) - 1.0*cos(x(1))*cos(x(2))*cos(x(5))*sin(x(3)) + cos(x(1))*cos(x(3))*cos(x(5))*sin(x(2)) - 1.0*cos(x(1))*cos(x(2))*cos(x(3))*cos(x(4))*sin(x(5)) - 1.0*cos(x(1))*cos(x(4))*sin(x(2))*sin(x(3))*sin(x(5));
cos(x(3))*cos(x(5))*sin(x(1))*sin(x(2)) - 1.0*cos(x(2))*cos(x(5))*sin(x(1))*sin(x(3)) - 1.0*cos(x(1))*sin(x(4))*sin(x(5)) - 1.0*cos(x(2))*cos(x(3))*cos(x(4))*sin(x(1))*sin(x(5)) - 1.0*cos(x(4))*sin(x(1))*sin(x(2))*sin(x(3))*sin(x(5));
cos(x(5))*sin(x(2))*sin(x(3)) + cos(x(2))*cos(x(3))*cos(x(5)) - 1.0*cos(x(2))*cos(x(4))*sin(x(3))*sin(x(5)) + cos(x(3))*cos(x(4))*sin(x(2))*sin(x(5));
0.16*cos(x(1)) - 0.0006763*sin(x(1)) + 0.59*cos(x(1))*cos(x(2)) - 0.10587*cos(x(4))*sin(x(1)) + 0.2*cos(x(1))*sin(x(2))*sin(x(3)) + 0.2015*sin(x(1))*sin(x(4))*sin(x(5)) + 0.2*cos(x(1))*cos(x(2))*cos(x(3)) + 0.723*cos(x(1))*cos(x(2))*sin(x(3)) - 0.723*cos(x(1))*cos(x(3))*sin(x(2)) - 0.10587*cos(x(1))*cos(x(2))*cos(x(3))*sin(x(4)) - 0.2015*cos(x(1))*cos(x(2))*cos(x(5))*sin(x(3)) + 0.2015*cos(x(1))*cos(x(3))*cos(x(5))*sin(x(2)) - 0.10587*cos(x(1))*sin(x(2))*sin(x(3))*sin(x(4)) - 0.2015*cos(x(1))*cos(x(2))*cos(x(3))*cos(x(4))*sin(x(5)) - 0.2015*cos(x(1))*cos(x(4))*sin(x(2))*sin(x(3))*sin(x(5));
0.0006763*cos(x(1)) + 0.16*sin(x(1)) + 0.10587*cos(x(1))*cos(x(4)) + 0.59*cos(x(2))*sin(x(1)) + 0.723*cos(x(2))*sin(x(1))*sin(x(3)) - 0.723*cos(x(3))*sin(x(1))*sin(x(2)) - 0.2015*cos(x(1))*sin(x(4))*sin(x(5)) + 0.2*sin(x(1))*sin(x(2))*sin(x(3)) + 0.2*cos(x(2))*cos(x(3))*sin(x(1)) - 0.10587*cos(x(2))*cos(x(3))*sin(x(1))*sin(x(4)) - 0.2015*cos(x(2))*cos(x(5))*sin(x(1))*sin(x(3)) + 0.2015*cos(x(3))*cos(x(5))*sin(x(1))*sin(x(2)) - 0.10587*sin(x(1))*sin(x(2))*sin(x(3))*sin(x(4)) - 0.2015*cos(x(2))*cos(x(3))*cos(x(4))*sin(x(1))*sin(x(5)) - 0.2015*cos(x(4))*sin(x(1))*sin(x(2))*sin(x(3))*sin(x(5));
0.2*cos(x(2))*sin(x(3)) - 0.723*cos(x(2))*cos(x(3)) - 0.59*sin(x(2)) - 0.2*cos(x(3))*sin(x(2)) - 0.723*sin(x(2))*sin(x(3)) - 0.10587*cos(x(2))*sin(x(3))*sin(x(4)) + 0.10587*cos(x(3))*sin(x(2))*sin(x(4)) + 0.2015*cos(x(5))*sin(x(2))*sin(x(3)) + 0.2015*cos(x(2))*cos(x(3))*cos(x(5)) - 0.2015*cos(x(2))*cos(x(4))*sin(x(3))*sin(x(5)) + 0.2015*cos(x(3))*cos(x(4))*sin(x(2))*sin(x(5)) + 0.456]
and the code:
clc;
clear;
tic;
x0=[2.5570, 3.005,1,0,2.3685,2.0853];
% x0=[2.55707, 3.13087,0.68765,0,2.443221,2.557073]; exact initial guess
[x,fval] = fsolve(@IK,x0)
toc;
%answer with exact initial guess : x = 1.6735 0.4234 -0.8127 0.4869 1.6022 1.3645
%answer with another guess yet close to answer: x = 4.1680 0.5210 -0.7761 0.4922 1.4093 4.2110
what the problem can be?

Walter Roberson

### Walter Roberson (view profile)

on 9 Jun 2019
12 equations in 6 variables is difficult
dpb

### dpb (view profile)

on 9 Jun 2019
Good point, Walter...
What's "3 of 4 DH parameters"? and there are six variables, not 4.
Given the complexity of the expressions, it's not out of the realm that there's a mistake or two in the formulation--be hard to avoid. You confirmed for absolute certain that the system is coded correctly? That could explain the symptom.
Are there any X that are known that could be removed to solve for the others to reduce dimensionality?
sanam

### sanam (view profile)

on 9 Jun 2019
4 parameters for 6 joints. 6 variables are the angles of these joints.
AND they are all unknowns
I don't confirm for absolute certain that the system is 'coded' correctly but I don't find any problem in the formulation except 2 decimal digits and that's because the data is given to me by the original problem in the way which produces coefficients like that.
I expected to get the angles that have formed these equations due to 3 other parameter related to them.

### Tags ### John D'Errico (view profile)

Answer by John D'Errico

### John D'Errico (view profile)

on 9 Jun 2019
Edited by John D'Errico

### John D'Errico (view profile)

on 9 Jun 2019

A serious issue is that you use many numbers accurate to only 2 decimal digits. So even though you may think the answer is correct, it can easily be not a solutrion at all. dpb showed that to be a fact. As well, any time you have multiple trig functions in there, expect any solution to be non-unique.
Let me try your start points. I'll call x0 the value that you CLAIM to be a solution. x0 is the vector that you started fsolve at.
x0=[2.55707, 3.13087,0.68765,0,2.443221,2.557073];
>> xinit=[2.5570, 3.005,1,0,2.3685,2.0853];
>> [IK(xinit),IK(x0)]
ans =
0.86398 1
0.47215 -3e-06
-0.17497 -8.3398e-07
-0.40696 3e-06
0.85941 1
0.30952 5.518e-07
0.2965 8.3398e-07
-0.19621 -5.518e-07
0.93466 1
0.97211 0.81522
-0.77106 -0.66715
0.68671 1.0763
>> norm(IK(x0))
ans =
2.2952
>> norm(IK(xinit))
ans =
2.2385
As you can see, evaluating IK at each of those points gives very different results, yet the norm of the objective is quite close. That bodes very poorly for your claim that this is truly a valid solution.
What does fsolve give us?
[xf,fv,exitflag] = fsolve(@IK,x0)
Warning: Trust-region-dogleg algorithm of FSOLVE cannot handle non-square systems; using Levenberg-Marquardt algorithm instead.
> In fsolve (line 310)
No solution found.
fsolve stopped because the last step was ineffective. However, the vector of function
values is not near zero, as measured by the value of the function tolerance.
<stopping criteria details>
xf =
4.1679 0.52101 -0.77606 0.49216 1.4093 4.2109
fv =
0.93396
0.12102
0.33626
-0.0022887
0.94293
-0.33299
-0.35737
0.31023
0.88093
5.0244e-06
-2.8169e-06
1.2031e-08
exitflag =
-2
In fact, fsolve is not terribly happy with the result. HOWEVER, it is considerably better than either your start point OR your claimed true "solution".
norm(IK(xf))
ans =
1.7321
So fsolve did indeed improve the result. With 12 equations in 6 unknowns, expect there to be no true solution. Worse, when you use 2 digit numbers in such a complex problem, expect garbage out the end. The set in xf is indeed an improvement, by any measure we can apply.
[IK(xinit),IK(x0),IK(xf)]
ans =
0.86398 1 0.93396
0.47215 -3e-06 0.12102
-0.17497 -8.3398e-07 0.33626
-0.40696 3e-06 -0.0022887
0.85941 1 0.94293
0.30952 5.518e-07 -0.33299
0.2965 8.3398e-07 -0.35737
-0.19621 -5.518e-07 0.31023
0.93466 1 0.88093
0.97211 0.81522 5.0244e-06
-0.77106 -0.66715 -2.8169e-06
0.68671 1.0763 1.2031e-08
If you want a better solution, then I would start by using more accurate coefficients inside the objective. Then I would recognize that the solution will not be unique, and that with more equations than unknowns, the solution will generally not be exact either.

sanam

### sanam (view profile)

on 9 Jun 2019
I learned alot. thank you for your explanations.