solve() fails to solve inverse kinematics with 4-DOF

I would like to solve for 4 joint variables given a constant (4x4) goal pose, but solve cannot find a solution.

% main .m file
% Analytical Inverse Kinematics with Symbols in 4-DOF (Matlab)

% Define Joint Variables as Symbols
% If you don't declare them as "real", you may get imaginary numbers
syms theta1 theta2 theta3 theta4 real

% Define Goal Pose (set from Forward Kinematics)
EE = ...
  zrotate(-0.528585) * ... % base
  translate(0, 0, 0.670) * ...
  yrotate(-0.585409) * ... % shoulder
  translate(0.7, 0, 0) * ...
  yrotate(1.23597) * ...   % elbow
  translate(0.7, 0.05, 0) * ...
  xrotate(0.917576) * ...  % wrist
  translate(0.18, 0, 0)

EE = 4×4

0.6871 0.7218 -0.0827 1.1337 -0.4013 0.2823 -0.8714 -0.6042 -0.6056 0.6319 0.4836 0.5238 0 0 0 1.0000

% Define Joint Frames
Base = zrotate(theta1);
Shoulder = ...
  translate(0, 0, 0.670) * ...
  yrotate(theta2);
Elbow = ...
  translate(0.7, 0, 0) * ...
  yrotate(theta3);
Wrist = translate(0.7, 0.05, 0) * ...
  xrotate(theta4);
Tool = translate(0.18, 0, 0);

% Define IK Equation
IK = Base * Shoulder == EE * inv(Tool) * inv(Wrist) * inv(Elbow);

IK = 

% Solve a 4x4 system of nonlinear equations with 4 unknowns
solve(IK, [theta1, theta2, theta3, theta4])

ans = struct with fields:
    theta1: [0×1 sym]
    theta2: [0×1 sym]
    theta3: [0×1 sym]
    theta4: [0×1 sym]

% Dependency functions in separate .m files
function frame = translate(x, y, z)
  frame = [
    1 0 0 x;
    0 1 0 y;
    0 0 1 z;
    0 0 0 1
  ];
end

function frame = xrotate(theta)
  frame = [
    1 0 0 0;
    0 cos(theta) -sin(theta) 0;
    0 sin(theta) cos(theta) 0;
    0 0 0 1
  ];
end


function frame = yrotate(theta)
  frame = [
    cos(theta) 0 sin(theta) 0;
    0 1 0 0;
    -sin(theta) 0 cos(theta) 0;
    0 0 0 1
  ];
end

function frame = zrotate(theta)
  frame = [ ...
    [cos(theta), -sin(theta), 0, 0];
    [sin(theta), cos(theta), 0, 0];
    [0, 0, 1, 0];
    [0, 0, 0, 1]
  ];
end

[Torsten]

Seems you have 12 equations for 4 unknowns. What do you expect as a solution ?

[Valeriy]

I expect the 4 unknowns as the solution.

If you are telling me that I need to pre-analyze the matrix and choose 4 equations out of 12 then pass those in to solve instead of the entire matrix?

Could you demonstrate how to do that?

[John D'Errico]

@Valeriy

You don't understand. Solve will not solve over-determined problems, unless the extra equations are simply duplicate information. An over-determined problem is one where you have more equations than unknowns. 12, versus 4, so this is over-determined.

Typically over-determined problems might be treated to minimize the errors over all, since no set of parameters will be expected to solve all 12 equations exactly. However, solve does not solve that class of problem. And, since the coefficients in those equations are generated from only approximate floating point numbers, it will be almost certain that there is no set of 4 parameters that could solve the entire set exactly.

This means your best approach will be to turn each of those 12 equations into expressions nominally equal to zero. Then use a tool like lsqnonlin, which is explicitly designed to solver over determined problems. It will return a purely numeric solution.

[Torsten]

If you are telling me that I need to pre-analyze the matrix and choose 4 equations out of 12 then pass those in to solve instead of the entire matrix?

You can try this, but I doubt you will get an analytical solution and - if you got one - that it would satisfy the other 8 equations. Use lsqnonlin as @John D'Errico said as an attempt to generate a least-squares solution.

Answer 1

[Valeriy]

I found what was wrong! The transform functions that I used to build joint transform matrices were flattening the output into basic double values instead of keeping everything in variable precision.

If the joint matrices are built like this (using Z rotation as an example):

function frame = zrotate(theta)
  arguments
    theta sym
  end
  frame = [
    cos(theta), -sin(theta), 0, 0;
    sin(theta), cos(theta), 0, 0;
    0, 0, 1, 0;
    0, 0, 0, 1
  ];
end

Then the output has a lot of precision. The way I had it before was:

function frame = zrotate(theta)
  frame = [ ...
    [cos(theta), -sin(theta), 0, 0];
    [sin(theta), cos(theta), 0, 0];
    [0, 0, 1, 0];
    [0, 0, 0, 1]
  ];
end

If the input argument type is not specified, you get double. Anything returned by the function is a double.

I had to declare it as a symbol to get the precision needed to calculate IK. Now the call to solve() started working as expected, changing nothing else about the way I was doing it.

>> solve(IK, [base,shoulder,elbow,wrist])

ans = 

  struct with fields:

        base: -2*atan(sin(4761070418067267/9007199...
    shoulder: -2*atan((cos(5272895508518669/900719...
       elbow: 2*atan((sin(695789253930139/56294995...
       wrist: 2*atan((sin(2066197465842055/2251799...

>> 

[Valeriy]

I think what happened is that the loss of precision from using doubles instead of precise fractions made the solution over-determined, and the solver gave up. With original precision restored, the extra equations are properly seen as duplicates as the solution is no longer over-determined.