How would I Solve this System of Trigonometric Equations for Inverse Kinematics?

How would I Solve this System of Trigonometric Equations for Inverse Kinematics?

clc; close all; clear all;

fprintf('cosine of pi /2: \n')
round(cos(pi/2))

fprintf('cosine of 90: \n')
cosd(90)

% Forward Kinematics
syms t1 t2 t3 real
syms r1 r2 r3 real
syms d1 d2 d3 d4 df

% DH Parameters
alpha = [0  sym(pi/2)   sym(pi/2)];
r     = [0  0   0];
d     = [d1  0   d3 ];
theta = [t1 t2+sym(pi/2) 0];

% Initialization of the Transformation from the Base (Frame 0) to the End Effector (Frame F)
T_0F = eye(4);

for i = 1:length(alpha)
    fprintf('T (%d) to (%d) \n',i-1,i)
    T{i} = TF(r(i),alpha(i),d(i),theta(i));
    pretty(simplify(T{i}))
    T_0F = T_0F*T{i}; % Sequential Multiplications
end

fprintf('T_0F: \n')
T_0F;

fprintf('Simplified T_0F: \n')
T_0F = simplify(T_0F,'Steps',60) % Sometimes with 50 or 60 Steps

% double(T_0F) % Numerical Solution
% num2str((double(T_0F)),'%.4f') % 4 Decimal Places

T_13 = simplify(T{2}*T{3}, 'Steps', 60)

% Inverse Kinematics
syms x y z

TRightSide = simplify(T{1}^(-1) * T_0F, 'Steps', 60)
%TRightSide = simplify( (T{1}*T{2})^(-1) * T_0F, 'Steps', 60)

T_0F(1,4)=x;
T_0F(2,4)=y;
T_0F(3,4)=z;
T_0Fx = T_0F

TLeftSide = simplify(T{1}^(-1) * T_0Fx, 'Steps', 60)
%TLeftSide = simplify( (T{1}*T{2})^(-1) * T_0Fx, 'Steps', 60)

eqn1 = TRightSide(1,4) == TLeftSide(1,4)
eqn2 = TRightSide(2,4) == TLeftSide(2,4)
eqn3 = TRightSide(3,4) == TLeftSide(3,4)

% HOW WOULD I CONTINUE FROM HERE TO GET SOLUTIONS for t1, t2 & d1?

%[t1 , t2, d1] = solve(eqn1,eqn2,eqn3,t1,t2,d1)

%t1 = solve(eqn2,t1)



% Radians

function T = TF(r,alpha,d,theta)
T = [cos(theta)            -sin(theta)           0           r
     sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha) -sin(alpha)*d
     sin(theta)*sin(alpha) cos(theta)*sin(alpha)  cos(alpha)  cos(alpha)*d
     0                     0                     0                       1];
end

How would I proceed from eqn 1, 2 and 3 to solve for t1, t2 & d1? Thanks!

Answer 1

[Torsten]

1. Solve eqn2 for t1
2. Insert the result for t1 in eqn1 and solve for t2
3. Insert the result for t2 in eqn3 and solve for d1

Answer 2

[Walter Roberson]

We just have to give up on the idea that the results are real-valued.

% Forward Kinematics
syms t1 t2 t3 %real
syms r1 r2 r3 real
syms d1 d2 d3 d4 df

% DH Parameters
alpha = [0  sym(pi/2)   sym(pi/2)];
r     = [0  0   0];
d     = [d1  0   d3 ];
theta = [t1 t2+sym(pi/2) 0];

% Initialization of the Transformation from the Base (Frame 0) to the End Effector (Frame F)
T_0F = eye(4);

for i = 1:length(alpha)
    fprintf('T (%d) to (%d) \n',i-1,i)
    T{i} = TF(r(i),alpha(i),d(i),theta(i));
    pretty(simplify(T{i}))
    T_0F = T_0F*T{i}; % Sequential Multiplications
end

T (0) to (1) 

/ cos(t1), -sin(t1), 0,  0 \
|                          |
| sin(t1),  cos(t1), 0,  0 |
|                          |
|    0,        0,    1, d1 |
|                          |
\    0,        0,    0,  1 /

T (1) to (2) 

/ -sin(t2), -cos(t2),  0, 0 \
|                           |
|     0,        0,    -1, 0 |
|                           |
|  cos(t2), -sin(t2),  0, 0 |
|                           |
\     0,        0,     0, 1 /

T (2) to (3) 

/ 1, 0,  0,  0  \
|               |
| 0, 0, -1, -d3 |
|               |
| 0, 1,  0,  0  |
|               |
\ 0, 0,  0,  1  /

fprintf('T_0F: \n')

T_0F: 

T_0F;

fprintf('Simplified T_0F: \n')

Simplified T_0F: 

T_0F = simplify(T_0F,'Steps',60) % Sometimes with 50 or 60 Steps

% double(T_0F) % Numerical Solution
% num2str((double(T_0F)),'%.4f') % 4 Decimal Places

T_13 = simplify(T{2}*T{3}, 'Steps', 60)

% Inverse Kinematics
syms x y z

TRightSide = simplify(T{1}^(-1) * T_0F, 'Steps', 60)
%TRightSide = simplify( (T{1}*T{2})^(-1) * T_0F, 'Steps', 60)

T_0F(1,4)=x;
T_0F(2,4)=y;
T_0F(3,4)=z;
T_0Fx = T_0F

TLeftSide = simplify(T{1}^(-1) * T_0Fx, 'Steps', 60)
%TLeftSide = simplify( (T{1}*T{2})^(-1) * T_0Fx, 'Steps', 60)

eqn1 = TRightSide(1,4) == TLeftSide(1,4)
eqn2 = TRightSide(2,4) == TLeftSide(2,4)
eqn3 = TRightSide(3,4) == TLeftSide(3,4)

% HOW WOULD I CONTINUE FROM HERE TO GET SOLUTIONS for t1, t2 & d1?

%[t1 , t2, d1] = solve(eqn1,eqn2,eqn3,t1,t2,d1)

%t1 = solve(eqn2,t1)

sol1 = solve(eqn1, t1)
neweqns_a = subs([eqn2, eqn3], t1, sol1(1))
neweqns_b = subs([eqn2, eqn3], t1, sol1(2)), disp(char(neweqns_b))

[0 == x*sin(log(-((d3^2*cos(t2)^2 - x^2 - y^2)^(1/2) - d3*cos(t2))/(x - y*1i))*1i) + y*cos(log(-((d3^2*cos(t2)^2 - x^2 - y^2)^(1/2) - d3*cos(t2))/(x - y*1i))*1i), d3*sin(t2) == z - d1]

sol2a = solve(neweqns_a(1), t2), disp(char(sol2a))

[acos((x^2 + y^2)^(1/2)/d3); -acos((x^2 + y^2)^(1/2)/d3)]

sol2b = solve(neweqns_b(1), t2), disp(char(sol2b))

[acos((x^2 + y^2)^(1/2)/d3); -acos((x^2 + y^2)^(1/2)/d3)]

neweqns2_aa = subs(neweqns_a(2:end), t2, sol2a(1))
sol3_aa = solve(neweqns2_aa, d1), disp(char(sol3_aa))

z - d3*(1 - (x^2 + y^2)/d3^2)^(1/2)

neweqns2_ab = subs(neweqns_a(2:end), t2, sol2a(2))
sol3_ab = solve(neweqns2_ab, d1), disp(char(sol3_ab))

z + d3*(1 - (x^2 + y^2)/d3^2)^(1/2)

neweqns2_ba = subs(neweqns_b(2:end), t2, sol2a(1))
sol3_ba = solve(neweqns2_ba, d1), disp(char(sol3_ba))

z - d3*(1 - (x^2 + y^2)/d3^2)^(1/2)

neweqns2_bb = subs(neweqns_b(2:end), t2, sol2a(2))
sol3_bb = solve(neweqns2_bb, d1), disp(char(sol3_bb))

z + d3*(1 - (x^2 + y^2)/d3^2)^(1/2)

So we have solved for t1, t2, d1, and all that remains is appropriate back-substitution.

% Radians

function T = TF(r,alpha,d,theta)
T = [cos(theta)            -sin(theta)           0           r
     sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha) -sin(alpha)*d
     sin(theta)*sin(alpha) cos(theta)*sin(alpha)  cos(alpha)  cos(alpha)*d
     0                     0                     0                       1];
end