FoR Lab 2

Introduction
The inverse kinematic equation computes what is known in the forward kinematic equation – Theeta the
desired angular position and orientation of the tooltip with the base. Its input is w the tool configuration
vector, the first 3 entries tell us the desired position and the last three entries tell us the orientation of
the tooltip.

Code:
robot=input( 'what is the name of the robot: ');
if(robot==1)
display('The robot is SCARA');    
syms d_3;
d=[877 0 d_3 200];
a=[425 425 0 0];
w_t=[0 0 0 0 0 -1];               %Minimum Reach
w=transpose(w_t);
q=double.empty(4,0);

q(2) = acosd( (w(1)^2 + w(2)^2 - a(1)^2 - a(2)^2) / (2*a(1)*a(2)) );
q(1) = atan2d( a(2)*sin(q(2))*w(1) + (a(1)+a(2)*cos(q(2)))*w(2) , (a(1)+a(2)*cos
(q(2)))*w(1) - a(2)*sin(q(2))*w(2));
q(3) = d(1) - d(4) - w(3);
q(4) =180*log(abs(w(6)));

elseif(robot==2)
    display('The robot is Intelledex');
    d   = [373.4 0 0 0 0 228.6];
a   = [0 0 304.8 304.8 0 0];
w_t = [a(3)+a(4)+d(6) 0 d(1) 1 0 0];
w   = transpose(w_t);
R   = [0 0 1; 0 -1 0; 1 0 0];
r_3 = R(:,3);       %This is the last column of R matrix
p   = w(1:3);       %This are the 1st 3 entries of w(q)
i_3 = R(:,1);       %Since this is [0; 0; 1] which is the 1st column of R matr
ix
q=double.empty(6,0);
b=double.empty(3,0);

q(6) = 180 * log( (w(4)^2+w(5)^2+w(6)^2)^0.5 );
q(2) = -acosd( - R(3,1)*sin(q(6)) - R(3,2)*cos(q(6)) );
q(1) = atan2d( R(2,1)*sin(q(6)) + R(2,2)*cos(q(6)) , R(1,1)*sin(q(6)) + R(1,2)*c
os(q(6)) );

b    = p - d(6)*r_3 - d(1)*i_3;
b_sq = b(1)^2 + b(2)^2 + b(3)^2;
q(4) = acosd( ((abs(b_sq)) - a(3)^2 - a(4)^2) / (2*a(3)*a(4)) );

q(3) = atan2d( (a(3)+cos(q(4))*a(4))*(sin(q(1))*b(1)-cos(q(1))*b(2)) - (
(sin(q(4))*a(4)*b(3))/sin(q(2))) ,  sin(q(4))*a(4)*(sin(q(1))*b(1)-
cos(q(1))*b(2))+((a(3)+cos(q(4)*a(4))*b(3))/sin(q(2))) );

q345 = atan2d( R(3,3), R(3,1)*cos(q(6)) - R(3,2)*sin(q(6)) );
q(5) = q345 - q(3) - q(4);

q = round (q);
elseif(robot==3)
    d=[26.04 0 0 0 16.83];
a=[0 22.86 22.86 0.95 0];
w_t=[a(3)+a(4) 0 d(1)+a(2)-d(5) 0 0 -0.607];
w=transpose(w_t);
q=double.empty(5,0);
%--------------------------q1-------------------------------------------
q(1)=atan2d(w(2),w(1));
%--------------------------q3
q234=atan2d( -((cosd(q(1))*w(4))+(sind(q(1))*w(5))),-w(6));
b1=(cosd(q(1))*w(1))+(sind(q(1))*w(2))-(a(4)*cosd(q234))+(d(5)*sind(q234));
b2= d(1)-a(4)*sind(q234)-(d(5)*cosd(q234))-w(3);
b_sq = power(b1,2) + power(b2,2);
temp = (b_sq - a(2)^2 - a(3)^2) / (2 * a(2) * a(3));
q(3) = acosd(temp);
%--------------------------q2
q(2) = atan2d( (a(2)+a(3)*cosd(q(3)))*b2 - a(3)*sind(q(3))*b1, (a(2)+a(3)*cosd(q
(3)))*b1 + a(3)*sind(q(3))*b2);
%--------------------------q4
q(4) = q234 - q(2) - q(3);
%--------------------------q5
q(5)=180*log( (w(4)^2 + w(5)^2 + w(6)^2)^0.5 );

q = round (q);
end

display (q)
Results:
The results are as folllows

1)Scara

2)Intelledex

3) Alpha 2