Delta Robot 3 DoF: Forward (Direct) Kinematic Problem (DKP)

DKP.m

%Resavanje DKP
function X = DKP(q1,q2,q3)
L1 = 62.2;
L2 = 173.6;
R = 209.25/2;
r = 128.74/2;

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

J1 = [(R-r+L1*cosd(q1))*cos(gama1);(R-r+L1*cosd(q1))*sin(gama1);L1*sind(q1)];
J2 = [(R-r+L1*cosd(q2))*cos(gama2);(R-r+L1*cosd(q2))*sin(gama2);L1*sind(q2)];
J3 = [(R-r+L1*cosd(q3))*cos(gama3);(R-r+L1*cosd(q3))*sin(gama3);L1*sind(q3)];

x1=J1(1); y1=J1(2); z1=J1(3);
x2=J2(1); y2=J2(2); z2=J2(3);
x3=J3(1); y3=J3(2); z3=J3(3);

w1 = x1^2 + y1^2 + z1^2;
w2 = x2^2 + y2^2 + z2^2;
w3 = x3^2 + y3^2 + z3^2;

d = (x3-x1)*(y2-y1)-(x2-x1)*(y3-y1);
a1 = 1/d*((z2-z1)*(y3-y1)-(z3-z1)*(y2-y1));
b1 = -1/(2*d)*((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1));
a2 = -1/d*((z2-z1)*(x3-x1)-(z3-z1)*(x2-x1));
b2 = 1/(2*d)*((w2-w1)*(x3-x1)-(w3-w1)*(x2-x1));

A = a1^2 + a2^2 + 1;
B = 2*(a1*(b1 - x1) + a2*(b2 - y1) - z1);
C = b1^2 + b2^2 - x1*b1 + y1*b2 + x1^2 + y1^2 + z1^2 - L2^2;

z = (-B - sqrt(B^2 - 4*A*C))/(2*A);
x = a1*z + b1;
y = a2*z + b2;

X = round([x,y,z]);
end