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

2 次查看（过去 30 天）
Valeriy
Valeriy 2024-2-27
编辑： Valeriy 2024-2-28
Ran in:
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
  4 个评论
显示 2更早的评论
John D'Errico
John D'Errico 2024-2-27
编辑：John D'Errico 2024-2-27
@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
Torsten 2024-2-28
编辑：Torsten 2024-2-28
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.

请先登录，再进行评论。

请先登录，再回答此问题。

采纳的回答

Valeriy
Valeriy 2024-2-28
编辑：Valeriy 2024-2-28
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...
>>
  1 个评论
Valeriy
Valeriy 2024-2-28
编辑：Valeriy 2024-2-28
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.

请先登录，再进行评论。

更多回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Solver Outputs and Iterative Display 的更多信息

标签

产品


版本

R2023b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by