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

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?

[dpb]

There doesn't appear to be a solution -- certainly your claimed "exact initial guess" isn't--

>> IK(x1)
ans =
    0.8284
    0.4291
   -0.3599
   -0.2564
    0.8619
    0.4375
    0.4980
   -0.2702
    0.8240
   -0.0000
    0.0000
    0.0000
>> 

[sanam]

I'm sure that it has at least 2 sets of solutions. I don't know why fsolve doesn't work properly in this case

[dpb]

What makes you think a) necessarily is so and that fsolve isn't working correctly?

Granted, I've not tried to do any reduction or evaluation of the system, so it may be so, but...looks like a mess and with all the mixed-effects terms, not particularly surprised doesn't find such a solution.

[sanam]

because these equations belong to an industrial robot configuration and when you give 3 of 4 DH parameters to form the equations and the forth parameter as an initial guess, u expect to get close answers (:angles(here x )). in other cases presented in robotics and modeling of Spong, I have tried and given close iniatial guess for angles I got the right solution but here I don't get my ans and I don't know where the problem of my work is.

[Walter Roberson]

12 equations in 6 variables is difficult

[dpb]

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]

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.

Answer 1

[John D'Errico]

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]

I learned alot. thank you for your explanations.