Fundamental of Robotics (FOR)

Lab # 2
Submitted to:

Sir Nasir Jadoon

Submitted by:

Warda tu Zahra
BSEE (17-21)
Section A

Submission Date:
                                      12-02-2021
         

Pakistan Institute of Engineering and Applied Sciences

(PIEAS)
Code to find joint variables of Rhino XR3 and plot its serial link
diagram

Code:
a=[0 20 20 20 0];
d=[10 0 0 0 10];
w=[40 0 20 0 0 -0.607];
q(1)=atan2d(w(2),w(1));
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);
temp=((b1^2+b2^2)-(a(2)^2)-(a(3)^2))/(2*a(2)*a(3));
q(3)=acosd(temp);
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));
q(4)=q234-q(2)-q(3);
q(5)=180*log(sqrt(w(4)^2+w(5)^2+w(6)^2));
disp('The joint variables of the robot are');
disp (q);
dh=[0 d(1) 0 -90;0 0 a(2) 0;0 0 a(3) 0;0 0 a(4) -90;0 d(5) 0 0];
r=SerialLink(dh);
r.plot([q(1),q(2),q(3),q(4),q(5)]);

output:

q(4)=q234-q(2)-q(3);
q(5)=180*log(sqrt(w(4)^2+w(5)^2+w(6)^2));
disp('The joint variables of the robot are');
disp (q);
dh=[0 d(1) 0 -90;0 0 a(2) 0;0 0 a(3) 0;0 0 a(4) -90;0 d(5) 0 0];
r=SerialLink(dh);
r.plot([q(1),q(2),q(3),q(4),q(5)]);
q(4)=q234-q(2)-q(3);
q(5)=180*log(sqrt(w(4)^2+w(5)^2+w(6)^2));
disp('The joint variables of the robot are');
disp (q);
dh=[0 d(1) 0 -90;0 0 a(2) 0;0 0 a(3) 0;0 0 a(4) -90;0 d(5) 0 0];
r=SerialLink(dh);
r.plot([q(1),q(2),q(3),q(4),q(5)]);
Link coordinate diagram:

Q2: Code to find joint variables of SCARA and plot its serial link
diagram

Code:
a=[425 425 0 0];
d=[877 0 0 200];
w=[];
for i=1:6
w(i)= input ('enter tool configuration vector'),i;
end
temp=-((w(1))^2+(w(2))^2-(a(1))^2-(a(2))^2)/(2*a(1)*a(2));
q2t=acos(temp);
q(2)=pi-q2t;
x1=sin(q(2));
x2=cos(q(2));
temp1=(a(2))*x1*w(1)+(a(1)+a(2)*x2)*w(2);
temp2=(a(1)+a(2)*x2)*w(1)-a(2)*x1*w(2);
q(1)=atan2(temp1,temp2);
q(3)=d(1)-d(4)-w(3);
q(4)=pi*log(abs(w(6)));
disp('The joint variables of the robot are');
disp (q);
dh=[0 d(1) a(1) pi; 0 d(2) a(2) 0; q(3) d(3) a(3) 0; 0 d(4) a(4) 0];
r=SerialLink(dh);
r.plot([q(1),q(2),q(3),q(4)]);

Output:

Link coordinate diagram: