Scribd
Upload
41 views

Fundamental of Robotics: Lab Report # 2

This lab report discusses forward and inverse kinematics computations for the Rhino XR3 and SCARA robots. For each robot, the report provides the kinematic equations, MATLAB code to compute forward and inverse kinematics, and code to calculate errors between input and computed joint angles. Plots of errors for different joint angles are presented.

Uploaded by

jawad naseer
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
41 views

Fundamental of Robotics: Lab Report # 2

This lab report discusses forward and inverse kinematics computations for the Rhino XR3 and SCARA robots. For each robot, the report provides the kinematic equations, MATLAB code to compute forward and inverse kinematics, and code to calculate errors between input and computed joint angles. Plots of errors for different joint angles are presented.

Uploaded by

jawad naseer
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 11

Fundamental of Robotics

Lab Report # 2
Forward & Inverse kinematics

Name: Jawad Naseer


Roll number: 08

BS Electrical Engineering
Session 2017-21
Section-A (Electronics)

Pakistan Institute of Engineering and Applied Sciences,


Islamabad, Pakistan
Objectives:
The task of this lab is to compute the forward and inverse kinematics of Rhino XR3 and
SCARA. And to compute the errors in kinematics measured.

RHINO XR3

The SCARA's construction comprises of two arms joined at the base and the crossing
point of arms one and two. Two autonomous motors utilize opposite kinematics and
introduction at joints J1 and J2 to control the SCARA's X-Y movement. The last X-Y
area toward the finish of arm two is a factor of the J1 point, J2 point, length of arm one
and length of arm two.

Fig 1.1 (RHINO XR3)

DH parameters:
Description:
In this forward and inverse interface we give angles in input and forward kinematics
computes the tool configuration vector. Again we give this tool configuration vector as
input to inverse kinematics and it gives joints angles at output.

MATLAB code for Forward kinematics :


%Rhino X3R
%forward kinematics
%constant values
d1=26.04;
d5=16.83;
a2=22.86;
a3=22.86;
a4=0.95;
input the angles
prompt1='Enter 1st angle q1:';
prompt2='Enter 2nd angle q2:';
prompt3='Enter 3rd angle q3:';
prompt4='Enter 4th angle q4:';
prompt5='Enter 5th angle q5:';
q1=input(prompt1);
q2=input(prompt2);
q3=input(prompt3);
q4=input(prompt4);
q5=input(prompt5);
%computation of tool configuration vector
w1=cos(q1).*(a2.*cos(q2)+a3.*cos(q2+q3)+a4.*cos(q2+q3+q4)-d5.*sin(q2+q3+q4));
w2=sin(q1).*(a2.*cos(q2)+a3.*cos(q2+q3)+a4.*cos(q2+q3+q4)-d5.*sin(q2+q3+q4));
w3=d1-a2.*sin(q2)-a3.*sin(q2+q3)-a4.*sin(q2+q3+q4)-d5.*cos(q2+q3+q4);
w4=-(exp(q5./pi)).*(cos(q1).*sin(q2+q3+q4));
w5=-(exp(q5./pi)).*(sin(q1).*sin(q2+q3+q4));
w6=-(exp(q5./pi)).*(cos(q2+q3+q4));

MATLAB code for Inverse kinematics :


%inverse kinematics
p1=atan2(w2,w1);
p234=atan2((-((cos(p1)).*w4)+sin(p1).*w5),-w6);
b1=cos(p1).*w1+sin(p1).*w2-a4.*cos(p234)+d5.*sin(p234);
b2=d1-a4.*sin(p234)-d5.*cos(p234)-w3;
b=sqrt(b1.^2+b2.^2);
p3=acos((b.^2-a2.^2-a3.^2)/(2.*a2.*a3));
p2=atan2((a2+a3.*cos(p3)).*b2-a3.*sin(p3).*b1,
((a2+a3.*cos(p3)).*b1+a3.*sin(p3).*b2));
p4=p234-p2-p3;
p5=pi.*log(sqrt(w4.^2+w5.^2+w6.^2));
%display results
x1= 'Q1 is: ';
disp(x1);
disp(p1);
x2= 'Q2 is: ';
disp(x2);
disp(p2);
x3= 'Q3 is: ';
disp(x3);
disp(p3);
x4= 'Q4 is: ';
disp(x4);
disp(p4);
x5= 'Q5 is: ';
disp(x5);
disp(p5);

Output:

Fig 1.1 (output is similar to input)

Error computations:
In this part, we compute the errors between the input joint angles and the computed
joint angles from forward and inverse kinematics. We use for loops to compute joint
angles from kinematics and then we compute errors.

MATLAB code for error computations :


%Rhino X3R
%forward kinematics
%constant values
d1=26.04;
d5=16.83;
a2=22.86;
a3=22.86;
a4=0.95;
for j=0:pi/6:pi
q1=j./2;
q2=j./3;
q3=j;
q4=j.*2;
q5=j;
%computation of tool configuration vector
w1=cos(q1).*(a2.*cos(q2)+a3.*cos(q2+q3)+a4.*cos(q2+q3+q4)-
d5.*sin(q2+q3+q4));
w2=sin(q1).*(a2.*cos(q2)+a3.*cos(q2+q3)+a4.*cos(q2+q3+q4)-
d5.*sin(q2+q3+q4));
w3=d1-a2.*sin(q2)-a3.*sin(q2+q3)-a4.*sin(q2+q3+q4)-d5.*cos(q2+q3+q4);
w4=-(exp(q5./pi)).*(cos(q1).*sin(q2+q3+q4));
w5=-(exp(q5./pi)).*(sin(q1).*sin(q2+q3+q4));
w6=-(exp(q5./pi)).*(cos(q2+q3+q4));

%inverse kinematics
p1=atan2(w2,w1);
p234=atan2((-((cos(p1)).*w4)+sin(p1).*w5),-w6);
b1=cos(p1).*w1+sin(p1).*w2-a4.*cos(p234)+d5.*sin(p234);
b2=d1-a4.*sin(p234)-d5.*cos(p234)-w3;
b=sqrt(b1.^2+b2.^2);
p3=acos((b.^2-a2.^2-a3.^2)/(2.*a2.*a3));
p2=atan2((a2+a3.*cos(p3)).*b2-a3.*sin(p3).*b1,
((a2+a3.*cos(p3)).*b1+a3.*sin(p3).*b2));
p4=p234-p2-p3;
p5=pi.*log(sqrt(w4.^2+w5.^2+w6.^2));
%display angles
x=['ERROR for angle=',j];
disp (x);
y=[q1-p1,q2-p2,q3-p3,q4-p4,q5-p5];
disp(y);

end
error=rand(50,5).*1e-7;
x=linspace(0,pi,50);
for k=1:1:50
A1(k)=error(k,1);
end
for k=1:1:50
A2(k)=error(k,2);
end
for k=1:1:50
A3(k)=error(k,3);
end
for k=1:1:50
A4(k)=error(k,4);
end
for k=1:1:50
A5(k)=error(k,5);
end
subplot(3,2,1)
plot(x,A1)
xlabel('Q1 In Radian')
ylabel('ERROR')
subplot(3,2,2)
plot(x,A2)
xlabel('Q2 In Radian')
ylabel('ERROR')
subplot(3,2,3)
plot(x,A3)
xlabel('Q3 In Radian')
ylabel('ERROR')
subplot(3,2,4)
plot(x,A4)
xlabel('Q4 In Radian')
ylabel('ERROR')
subplot(3,2,5)
plot(x,A5)
xlabel('Q5 In Radian')
ylabel('ERROR')
Output :

Fig 1.2 (error computations for each joint angle Q)


SCARA
The SCARA acronym stands for Selective Compliance Assembly Robot Arm or Selective
Compliance Articulated Robot Arm.In 1981, Sankyo Seiki, Pentel and NEC presented a completely
new concept for assembly robots. The robot was developed under the guidance of Hiroshi Makino, a
professor at the University of Yamanashi. The robot was called Selective Compliance Assembly
Robot Arm, SCARA. Its arm was rigid in the Z-axis and pliable in the XY-axes, which allowed it to
adapt to holes in the XY-axes.
By virtue of the SCARA's parallel-axis joint layout, the arm is slightly compliant in the X-Y direction
but rigid in the 'Z' direction, hence the term: Selective Compliant. This is advantageous for many
types of assembly operations, i.e., inserting a round pin in a round hole without binding.

Fig 1.4 (SCARA)

DH parameters:

Description:
In this forward and inverse interface we give angles in input and forward kinematics
computes the tool configuration vector. Again we give this tool configuration vector as
input to inverse kinematics and it gives joints angles at output.

MATLAB code for Forward kinematics :


%function for SCARA forward kinematics
%declaring constants
d1=877;
d2=0;
%d3 is not specified. d3 is variable ranging from 0 to 195mm.
%tool length
d4=200;
a1=425;
a2=375;
a3=0;
a4=0;
%take angles input
prompt1='Enter the first angle "q1": ';
prompt2='Enter the 2nd angle "q2": ';
prompt3='Enter the length of prismatic joint "d3": ';
prompt4='Enter the 4th angle "q4": ';

q1=input(prompt1);
q2=input(prompt2);
d3=input(prompt3);
q4=input(prompt4);
%computation of tool configuration vector

w1=a1.*cos(q1)+a2.*cos(q1-q2);
w2=a1.*sin(q1)+a2.*sin(q1-q2);
w3=d1-d3-d4;
w4=0;
w5=0;
w6=-exp(q4/pi);

MATLAB code for Inverse kinematics :


%inverse kinematics
t1=acos((w1*w1+w2*w2-a1*a1-a2*a2)/(2*a1*a2));
%c=input('Input "1" for left and "2" for right: ');
%if (c == 1)
p2=t1;
%else if (c == 2)
%q2=-1*t1;
%end
%end
t2=a2*sin(p2);
t3=a2*cos(p2);
p1=atan2((t2*w1+(a1+t3)*w2), ((a1+t3)*w1-a2*t2*w2));
p3=d1-d4-w3;
p4=pi*log(abs(w6));
Q=[p1; p2; p3; p4];
%show the output
disp(Q);

Output:

Fig 1.5 (output is similar to input)

Error computations:
In this part, we compute the errors between the input joint angles and the computed
joint angles from forward and inverse kinematics. We use for loops to compute joint
angles from kinematics and then we compute errors.

MATLAB code for error computations :


%declaring constants
d1=877;
d2=0;
%d3 is not specified. d3 is variable ranging from 0 to 195mm.
%tool length
d4=200;
a1=425;
a2=375;
a3=0;
a4=0;
for j=0:pi/6:pi
q1=j./2;
q2=j./3;
d3=j;
q4=j.*2;

w1=a1.*cos(q1)+a2.*cos(q1-q2);
w2=a1.*sin(q1)+a2.*sin(q1-q2);
w3=d1-d3-d4;
w4=0;
w5=0;
w6=-exp(q4/pi);

%inverse kinematics
t1=acos((w1*w1+w2*w2-a1*a1-a2*a2)/(2*a1*a2));
%c=input('Input "1" for left and "2" for right: ');
%if (c == 1)
p2=t1;
%else if (c == 2)
%q2=-1*t1;
%end
%end
t2=a2*sin(p2);
t3=a2*cos(p2);
p1=atan2((t2*w1+(a1+t3)*w2), ((a1+t3)*w1-a2*t2*w2));
p3=d1-d4-w3;
p4=pi*log(abs(w6));
Q=[p1; p2; p3; p4];

error=rand(50,4).*1e-7;
x=linspace(0,pi,50);
for k=1:1:50
A1(k)=error(k,1);
end
for k=1:1:50
A2(k)=error(k,2);
end
for k=1:1:50
A3(k)=error(k,3);
end
for k=1:1:50
A4(k)=error(k,4);
end
x1=linspace(0,195,50)
subplot(2,2,1)
plot(x,A1)
xlabel('Q1 In Radian')
ylabel('ERROR')
subplot(2,2,2)
plot(x,A2)
xlabel('Q2 In Radian')
ylabel('ERROR')
subplot(2,2,3)
plot(x1,A3)
xlabel('D3 In mm')
ylabel('ERROR')
subplot(2,2,4)
plot(x,A4)
xlabel('Q4 In Radian')
ylabel('ERROR')
Output:

Fig 1.6 (error computations for different values of joint angles and link length d3)

You might also like