Delta Robot 3 DoF: Inverse Kinematic Problem (DKP)

IKP.m

% Resavanje IKP
function [q1,q2,q3] = IKP(x,y,z)
L1 = 62.2;
L2 = 173.6;
R = 209.25/2;
r = 128.74/2;

gama1 = 0;
gama2 = 2/3*pi;
gama3 = 4/3*pi;


kw1 = [x; y; z] + [cos(gama1); sin(gama1); 0]*r - [cos(gama1); sin(gama1); 0]*R;
kwx1 = kw1(1);
kwy1 = kw1(2);
kwz1 = kw1(3);

n1 = cos(gama1)*kwx1 + sin(gama1)*kwy1;
m1 = kwz1;
c1 = (L1^2-L2^2+kwx1^2+kwy1^2+kwz1^2)/(2*L1);

d1 = m1^2 + n1^2 - c1^2;

t1 = (m1+sqrt(d1))/(n1+c1);
q1 = 2*atan(t1)*180/pi;

kw2 = [x; y; z] + [cos(gama2); sin(gama2); 0]*r - [cos(gama2); sin(gama2); 0]*R;
kwx2 = kw2(1);
kwy2 = kw2(2);
kwz2 = kw2(3);

n2 = cos(gama2)*kwx2 + sin(gama2)*kwy2;
m2 = kwz2;
c2 = (L1^2-L2^2+kwx2^2+kwy2^2+kwz2^2)/(2*L1);

d2 = m2^2 + n2^2 - c2^2;

t2 = (m2+sqrt(d2))/(n2+c2);
q2 = 2*atan(t2)*180/pi;

kw3 = [x; y; z] + [cos(gama3); sin(gama3); 0]*r - [cos(gama3); sin(gama3); 0]*R;
kwx3 = kw3(1);
kwy3 = kw3(2);
kwz3 = kw3(3);

n3 = cos(gama3)*kwx3 + sin(gama3)*kwy3;
m3 = kwz3;
c3 = (L1^2-L2^2+kwx3^2+kwy3^2+kwz3^2)/(2*L1);

d3 = m3^2 + n3^2 - c3^2;

t3 = (m3+sqrt(d3))/(n3+c3);
q3 = 2*atan(t3)*180/pi;
end