HO CHI MINH CITY UNIVERSITY OF TECHNOLOGY AND EDUCATION

FACULTY OF INTERNATIONAL EDUCATION

------

FINAL REPORT

ROBOTICS SIMULATION: MOTOMAN GP250 (YASKAWA)

COURSE: ROBO331129E_23_1_02FIE

SEMESTER 1 – YEAR 2023-2024

GROUP: Group 4, Friday, period 1-3

INSTRUCTORS: Phd. Hà Lê Như Ngọc Thành

Thành phố Hồ Chí Minh, ngày 3 tháng 12 năm 2023

1
 LIST OF GROUPS PARTICIPATING IN WRITING REPORT

SEMESTER I, 2023-2024

Group 4. Friday, period 1-3

No. STUDENT'S FULL STUDENT COMPLETION

NAME CODE RATE %
1 Nguyễn Minh Duy 21146009 100%

2 Khổng Trung Hiệp 21146016 100%

3 Nguyễn Minh Quân 21146409 100%

Note:

- Percentage % = 100%: Percentage level of each student participating.

Teacher comment:

…………………………………………………………………………………….

…………………………………………………………………………………….

…………………………………………………………………………………….

Ho Chi Minh City, month day, year

Teacher grades Teacher Sign
(Sign with full name)

0
Contents
I. INTROD UCTION...................................................................................................2
II. CALCULATION.................................................................................................3
1. The parameter of GP250....................................................................................3
2. Calculating Forward Kinematics.......................................................................5
3. Calculating Inverse Kinematics.........................................................................7
4. Calculating Jacobian.........................................................................................19
5. Controlling and drawing 3D model in Matlab...............................................25

1
 I. INTRODUCTION

Equipped with a wide wrist motion range and high moment and inertia ratings the GP250
robot is ideal for heavy part processing. Fast and precise, the six-axis GP250 robot
enables increased productivity for a variety of applications. While exceptionally fast axis
speeds and acceleration capabilities increase production output, a streamlined upper arm
with a slim wrist profile facilitates an easier reach into tight spaces, optimizing
application flexibility. A reduced interference design enables high-density robot
placement in work cells for the best use of floorspace, making the robust and efficient
GP250 a smart choice for machine tending, material handling and press tending
applications.

2
 II. CALCULATION
1. The parameter of GP250

 According to the parameters of GP250 we can draw the direction of robot GP250
in xyz surface

3
 We can write the Denavit – Hartenberg (DH) table for our robot by using all the
parameters above

a Alpha d Theta
0-1 l2=285 π l1=650 theta1
2
1-2 l3=1150 0 0 +π
theta2
2
2-3 l4=250 π 0 theta3
2
3-4 0 −π l5=1285 theta4
2
4-5 0 π 0 theta5
2
5-6 0 0 0 theta6

4
 6-P 0 0 l6=250 0
2. Calculating Forward Kinematics

We write the code for the Forward Kinematics in Matlab

clc; clear;close all;

syms l1 l2 l3 l4 l5 l6 t1 t2 t3 t4 t5 t6
% l1 = 650;
% l2 = 285;
% l3 = 1150;
% l4 = 250;
% l5 = 1285;
% l6 = 250;
T01 = TransfromMatrix(l2,pi/2,l1,t1);
T12 = TransfromMatrix(l3,0,0,t2+pi/2);
T23 = TransfromMatrix(l4,pi/2,0,t3);
T34 = TransfromMatrix(0,-pi/2,l5,t4);
T45 = TransfromMatrix(0,pi/2,0,t5);
T56 = TransfromMatrix(0,0,0,t6);
T67 = TransfromMatrix(0,0,l6,0);

PH = [0 0 0 1]';
T03 = T01*T12*T23; % Arm
T36 = T34*T45*T56; % Wist
T07 = T03*T36*T67;
T07 = simplify(T07);

5
P = T07*PH;
The result for Forward Kinematics is

( ) ( )
c 1 0 s1 l2 c 1 c 4 0 −s 4 0
s 0 −c l s s 0 c4 0
T01¿ 1 1 1 2 1 T34= 4
0 0 l1 0 −1 0 l 5
00 0 1 0 0 0 1

( ) ( )
π π π c5 0 s5 0
c2 + −s 2 + l3 c 2 + s 0 −c 5 0
2 2 0 2 T45= 5
π π 0 π 01 0 0
T12= s 2+ c 2+ 1
l 3 s 2+
00 0 1
2 2 2
0 0 0 0

( )
0 0 1 c6 −s6 0 0
s c 00
T56= 6 6

( )
c3 0 s3 l ¿ 0 0 10
s 0 −c 3 l ¿ 0 0 01
T23= 3
01 0 0

( )
00 0 1 100 0
010 0
T67= 0 0 1 l
6
000 1

 T07 = T01*T12*T23*T34*T45*T56*T67

6
 3. Calculating Inverse Kinematics
With the initial parameters:

First, separate the robot into 3 main stages: Arm, Wist and the final working stage
(translational stage).

- Arm suture (T03) is the section from point O to W.

- Wist suture (T36) is the wrist joint at the W position.

- The translation link (T67) is the section from point W to E.

 T07 = T03*T36 *T67

To calculate the end point position, we need to know the direction the robot will go.

The orientation of a Robot can be determine by three types of rotaion:

7
 1. Roll – Pitch – Roll.
2. Roll – Pitch – Yaw.
3. Pitch – Yaw – Roll.

With MOTOMAN GP250 (YASKAWA) robot, our team will follow the Roll – Pitch –
Roll principle to calculate inverse kinematics. As follows:

R02 =R z ( ϕ ) × R x ( θ ) × R z ( ψ )
0 1 2

( )( )
cϕcψ −sϕcθsψ −cϕsψ−sϕcθcψ sϕsθ nx ox ax
¿ sϕcψ +cϕcθsψ sϕsψ +cϕcθcψ −cϕsθ = n y o y a y
sθsψ sθcψ cθ nz oz az

In there: ϕ , θ ,ψ is the robot's direction given in advance.

function [ax,ay,az,R0E]= RotateMatrix(r1,p,r2)

r1 = r1*pi/180;
p = p*pi/180;
r2 = r2*pi/180;
R0E= [cos(r1)*cos(r2)-sin(r1)*cos(p)*sin(r2) -cos(r1)*sin(r2)-sin(r1)*cos(p)*cos(r2)
sin(r1)*sin(p)
sin(r1)*cos(r2)+cos(r1)*cos(p)*sin(r2) -sin(r1)*sin(r2)+cos(r1)*cos(p)*cos(r2) -
cos(r1)*sin(p)
sin(p)*sin(r2) sin(p)*cos(r2) cos(p) ];

P0= [0 0 1]';
Re = R0E*P0;
ax = Re(1);
ay = Re(2);

8
az = Re(3);

After knowing the robot's direction through Roll - Pitch - Roll, we will determine the
position of the W point.

We assume the end-effector P(X, Y, Z). We have:

O O 7 O
pW =R 7 × pW × p7 org

( )( )( ) ( ) ( )
xW nx o x ax 0 X X −l 6 a x
¿> y W = n y o y a y 0 + Y = Y −l 6 a y
zW n z o z a z −l 6 Z Z −l 6 a z

clc;clear all;close all;

l1 = 650;
l2 = 285;
l3 = 1150;
l4 = 250;
l5 = 1285;
l6 = 250;

X=1820; Y=0; Z=2050;

roll1=0; pitch=0; roll2=0;

[ax,ay,az,R0E]=RotateMatrix(roll1,pitch,roll2);
xW=X-l6*ax;
yW=Y-l6*ay;
zW=Z-l6*az;

9
We use the geometric method to calculate the angle of theta1, theta2, theta3.We draw
the angle of theta1, theta2, theta3 in the x, y, z plane.

- Calculate theta 1:

atan 2(x w , y w )

=> theta1 =

%% theta 1
if((xW>0)&&(yW~=0))
t1=sign(yW)*(atan(abs(yW/xW)));
else if((xW==0)&&(yW~=0))
t1=sign(yW)*pi/2;
else if ((xW<0)&& (yW~=0))
t1=sign(yW)*(pi-atan(abs(yW/xW)));
else ((xW~=0)&&(yW==0))
t1=pi-pi*sign(xW);
end
end

10
 end
- Calculate theta 2:

r =√ x w ²+ y w ²

OM = r - l2; WM = zw – l1

O1W= √ ( r−l 2 )2 +(z ¿¿ w−l 1)² ¿

O2W = √ l 4 ²+l 5 ²
2 2
l3 +O 1W −O 2W ²
Cos (O2O1W) =
2 l3 (O1 W )

 O2O1W =atan2(± √1−cos ⁡(O 2O 1W )2 , cos ⁡(O2 O 1W ))

2 2
O1 W +O 1 M −MW ²
cos (WO 1 M )=
2 (O1 W )(O 1 M )

 WO1M = atan2((± √1−cos ⁡(WO 1 M )2 ,cos ⁡(WO 1 M ))

 theta2=O2O1W+WO1M

11
%% theta 2
r=sqrt(xW^2+yW^2);
O1W=sqrt((r-l2)^2+(zW-l1)^2);
O2W=sqrt(l4^2+l5^2);
O1O2=l3;
if(O1W<O1O2+O2W)
cos_O2O1W=(O1O2^2+O1W^2-O2W^2)/(2*O1O2*O1W);
cos_O1O2W=(O1W^2-O1O2^2-O2W^2)/(2*O1O2*O2W);
WM=zW-l1;
O1M=r-l2;
if((O1M>0)&&(WM~=0))
WO1M=sign(WM)*atan(abs(WM/O1M));
else if ((O1M==0)&&(WM~=0))
WO1M=sign(WM)*pi/2;
else if((O1M<0)&&(WM~=0))
WO1M=sign(WM)*(pi-atan(abs(WM/O1M)));
else((O1M~=0)&&(WM==0))
WO1M=pi-pi*sign(O1M);
end
end
end
t2=WO1M+acos(cos_O2O1W);

Calculate theta 3:

12
 2 2
O1 W −O 2 W −O 1O 2 ²
cos (x3O2W) = -cos (O1O2W) =
2(O 2W )(O 1 O2)

sin (x3O2W) = ± √1−cos ⁡(x 3 O 2W )2

 x3O2W = atan2((± √ sin ⁡(x 3O 2 W )2 , cos ⁡(x 3 O 2 W ))

 theta3 = atan2(
l5
l4
(
√ )2
)-atan2( 1−cos O 1O 2 W \
cosO 1O 2W
%% theta 3
xO2W=atan(l5/l4);
t3=atan(l5/l4)-atan((sqrt(1-cos_O1O2W^2))/cos_O1O2W)
if(t3>xO2W)
t3=t3-pi;
end

Calculate theta 4, theta 5, theta 6

( )
c1 c 23 s 1 c 1 s23
0
We have R ¿ T R = s 1 c23 −c 1 s1 s 23
0
3
0
3 3

s 23 0 −c23

function [R] = calculate_R03(t1,t2,t3)

t1=t1;
t2=t2;
t3=t3;
R = [cos(t1)*cos(t2+t3) sin(t1) cos(t1)*sin(t2+t3)
sin(t1)*cos(t2+t3) -cos(t1) sin(t1)*sin(t2+t3)
sin(t2+t3) 0 -cos(t2+t3)];
end

13
We have R36 ¿ T 36

( )
c 4 c5 c 6−s 4 s 6−c 4 c5 s 6−s 4 c 6 c 4 s 5
3
R6 = s 4 c 5 c 6 +c 4 s6 −s 4 c 5 s6 + c 4 c6 s 4 s 5
−s5 c 6 s 5 s6 c5

−1
Orhter hand: R36 =R 03 × R06=( R03 ) × R 0E

( )( )( )
c 1 c 23 s 1 c 1 s 23 nx ox ax r 11 r 12 r 13
¿ s1 c 23−c 1 s 1 s23 n y o y a y = r 21 r 22 r 23
s23 0 −c 23 n z o z a z r 31 r 32 r 33

%% R03
R03 = calculate_R03(t1,t2,t3);
%% R36
R36 = R03^-1* R0E;
r11 = R36(1,1); r12 = R36(1,2); r13 = R36(1,3);
r21 = R36(2,1); r22 = R36(2,2); r23 = R36(2,3);
r31 = R36(3,1); r32 = R36(3,2); r33 = R36(3,3);
%% theta 4
if ((r13>0)&&(r23~=0))
t4=sign(r23)*atan(abs(r23/r13));
else if((r13==0)&&(r23~=0))
t4=sign(r23)*pi/2;
else if((r13<0)&&(r23~=0))
t4=sign(r23)*(pi-atan(abs(r23/ax)));
else ((r13~=0)&&(r23==0))
t4=pi-pi*sign(r13);
end
end
end

if(abs(ax)<=0.00001)&&(abs(ay)<=0.000001)
t4=0;
end

%% theta 5
Ay=(r13+r23)/(cos(t4)+sin(t4));
Ax=r33;
14
 if((Ax==0)&&(Ay~=0));
t5=atan(Ay/Ax);
end
if(Ax>0)
t5=atan(Ay/Ax);
else if(Ax<0)
if(Ay>=0);
t5=atan(Ay/Ax)+pi;
else(Ay<=0)
t5=atan(Ay/Ax)+pi;
end
end
end
%% theta 6
t6=atan(r32/(-r31))
if(r13==0)&&(r23==0)
t5=0;
t6=0;
t4=atan(r21/r11);
end
end
end
t2 = t2-pi/2

Test the correction of Inverse Kinematic

1. Assume the end effector position is P (300;800;1000)

15
 The angle is: roll1 : 5, pitch:-60, roll2: 60

2. Assume the end effector position is P (300; 800; 1000)

The angle is roll1: 40 pitch: -60, roll2: -90

16
3. Assume the end effector position is P (1000;1650;700)

The angle is roll1: 90 pitch: -45, roll2: -90

17
Conclusion

The Inverse Kinematic we calculate is satisfy and the end effector corrects with T07
of Kinematic. According to the limits of the rotation angles and operating range of the
robot, our team's inverse kinematics worked well to give the exact position we
wanted.

18
 4. Calculating Jacobian

According to the Geometric expression of the Jacobian, apply to Matlab to calculate

angular velocity and linear velocity

clc; clear;
syms l1 l2 l3 l4 l5 l6 t1 t2 t3 t4 t5 t6 real

T01 = TransfromMatrix(l2,pi/2,l1,t1);
T12 = TransfromMatrix(l3,0,0,t2+pi/2);
T23 = TransfromMatrix(l4,pi/2,0,t3);
T34 = TransfromMatrix(0,-pi/2,l5,t4);
T45 = TransfromMatrix(0,pi/2,0,t5);
T56 = TransfromMatrix(0,0,0,t6);
T67 = TransfromMatrix(0,0,l6,0);

T02 = T01*T12; T05 = T01*T12*T23*T34*T45;

T02 = simplify(T02); T05 = simplify(T05);

T03 = T01*T12*T23; T06 = T01*T12*T23*T34*T45*T56;

T03 = simplify(T03); T06 = simplify(T06);

T04 = T01*T12*T23*T34; T07 = T01*T12*T23*T34*T45*T56*T67;

T04 = simplify(T04); T07 = simplify(T07);

19
z0 = [0 0 1]'; P0 = [0 0 0]';
z0_trans = transZ(z0);

z1 = T01*[0 0 1 0]'; P1 = T01*[0 0 0 1]';

z1 = [z1(1),z1(2),z1(3)]'; P1 = [P1(1) P1(2) P1(3)]';
z1_trans = transZ(z1);

z2 = T02*[0 0 1 0]'; P2 = T02*[0 0 0 1]';

z2 = [z2(1),z2(2),z2(3)]'; P2 = [P2(1) P2(2) P2(3)]';
z2_trans = transZ(z2);

z3 = T03*[0 0 1 0]'; P3 = T03*[0 0 0 1]';

z3 = [z3(1),z3(2),z3(3)]'; P3 = [P3(1) P3(2) P3(3)]';
z3_trans = transZ(z3);

z4 = T04*[0 0 1 0]'; P4 = T04*[0 0 0 1]';

z4 = [z4(1),z4(2),z4(3)]'; P4 = [P4(1) P4(2) P4(3)]';
z4_trans = transZ(z4);

z5 = T05*[0 0 1 0]'; P5 = T05*[0 0 0 1]';

z5 = [z5(1),z5(2),z5(3)]'; P5 = [P5(1) P5(2) P5(3)]';
z5_trans = transZ(z5);

z6 = T06*[0 0 1 0]'; P6 = T06*[0 0 0 1]';

z6 = [z6(1),z6(2),z6(3)]'; P6 = [P6(1) P6(2) P6(3)]';
z6_trans = transZ(z6);
Pe = T07*[0 0 0 1]';
Pe = [Pe(1) Pe(2) Pe(3)]';

Jv1 = [z0_trans*(Pe-P0)]; Jv3 = simplify(Jv3);

Jv1 = simplify(Jv1); Jw3 = z2;
Jw1 = z0;
Jv4 = [z3_trans*(Pe-P3)];
Jv2 = [z1_trans*(Pe-P1)]; Jv4 = simplify(Jv4);
Jv2 = simplify(Jv2); Jw4 = z3;
Jw2 = z1;
Jv5 = [z4_trans*(Pe-P4)];
Jv3 = [z2_trans*(Pe-P2)]; Jv5 = simplify(Jv5);

20
Jw5 = z4; Jv6 = simplify(Jv6);
Jw6 = z5;
Jv6 = [z5_trans*(Pe-P5)];

Jac= [Jv1 Jv2 Jv3 Jv4 Jv5 Jv6

Jw1 Jw2 Jw3 Jw4 Jw5 Jw6];
Jac = simplify(Jac)

%% Check
J= jacobian([Pe(1),Pe(2),Pe(3)],[t1,t2,t3,t4,t5,t6]);
J = simplify(J)

The result for Jacobian

 Revolute joint

() ()
0 0
z0= 0 p0= 0
1 0

 z0.(p7-p1)= Jv1
Jv1= ¿

z1 p1

( )
c10 s1 l 2c1

( )
0 0 −c 1
s 1 0 −c 1 l2 s 1
T01¿ 0 1 0 l1 ¿
z1_trans 0 0 −s 1
c1s 1 0
0 0 0 1

 z1.(p7-p1)=Jv2

21
Jv2=

(
−c 1(l 3 c 2+l 4 c 2 c 3+l5 c 2 s 3+l 5 c 3 s 2−l 4 s 2 s 3−l 6 c 2c 5 s 3−l 6 c 3 c 5 s 2+l 6 c 2 c 3 c 4 s 5−l 6 c 4 s 2 s 3
−s 1(l 3 c 2+l 4 c 2 c 3+ l5 c 2 s 3+l 5 c 2 s 3+l5 c 3 s 2−l 4 s 2 s 3−l6 c 2 c 5 s 3−l 6 c 3 c 5 s 2+l 6 c 2 c 3 c 4 s 5−l 6 c 4
l5 c 2 c 3−l 3 s 2−l 4 c 2 s 3−l 4 c 3 s 2−l5 s 2 s 3−l6 c 2 c 3 c 5+l6 c 5 s 2 s 3−l6 c 2 c 4 s 3 s 5−l6 c 3 c 4 s 2 s

z2 p2

( )
0 0 −c 1
T02=¿z2_trans¿ 0 0 −s 1
c1s 1 0

 z2.(p7-p2)=Jv3
Jv3 =

( )
−c 1(l 4 c 23+l 5 c 2 s 3+l5 c 3 s 2−l 4 s 2 s 3+l 6 c 2 c 5 s 3+l 6 c 3 c 5 s 2+l6 c 2 c 3 c 4 s 5−l6 c 4 s 2 s 3 s 5)
−s 2(l 4 c 23+l5 c 2 s 3+l 5 c 3 s 2−l 4 s 2 s 3+l6 c 2 c 5 s 3+l 6 c 3 c 5 s 2+l 6 c 2 c 3 c 4 s 5−l 6 c 4 s 2 s 3 s 5)
l5 c 2 c 3−l 4 c 2 s 3−l 4 c 3 s 2+l 5 s 2 s 3+l6 c 2 c 5 s 3−l 6 c 5 s 3 s 2−l 6 c 2 c 4 s 3 s 5−l 6 c 3 c 42 s 2 s 5

z3 p3

T03=¿

( )
0 −s 23 c 23 s 1
z3_trans¿ s 23 0 −c 23 c 1
−c 23 s 1 c 23 c 1 0

 z3.(p7-p3)=Jv4

( )
l 6 s 5 (c 4 s 1+ c 1 c 2 s 3 s 4+ c 1 c 3 s 2 s 4)
Jv4 = l 6 s 5(c 2 s 1 s 3 s 4−c 1 c 4+ c 3 s 1 s 2 s 4)
−l 6 c 23 s 4 s 5

22
 z4 p4

T04=

( )
s 1 s 4−c 4 c 1 c 2 s 3−c 1 c 3 c 4 s 2 −c 23 c 1 c 4 s 1+ c 1 c 2 s 3 s 4+ c 1 c 3 s 2 s 4 c 1(l 2+l5 c 23−l 4 s 23−l 3 s 2)
−c 1 s 4−c 4 c 2 s 1 s 3−c 3 c 4 s 1 s 2 −c 23 s 1 c 2 s 1 s 3 s 4−c 1c 4 +c 3 s 1 s 2 s 4 s 1(l2+l 5 c 23−l 4 s 23−l3 s 2)
c 23 c 4 −s 23 −c 23 s 4 l1+l 4 c 23+l 5 s 23+ l3 c 2
0 0 0 1

z4_trans

( )
0 c 23 s 4 c 2 s 1 s 3 s 4−c 1 c 4+ c 3 s 1 s 2 s 4
¿ −c 23 s 4 0 −c 4 s 1−c 1 c 2 s 3 s 4−c 1 c 3 s 2 s 4
c 1 c 4−c 2 s 1 s 3 s 4−c 3 s 1 s 2 s 4 c 4 s 1+c 1 c 2 s 3 s 4 +c 1 c 3 s 2 s 4 0

 z4.(p7-p4)=Jv5
Jv5 =

( )
l 6 c 5 s 1 s 4−l 6 c 1 c 2 c 3 s 5+l 6 c 1 s 2 s 3 s 5+l6 c 1 c 3 c 2 c 5 c 4−l6 c 1 c 2 c 4 sc 5
l 6 s 1 s 2 s 3 s 5−l 6 c 2 c 3 s 1 s 5−l 6 c 1 c 5 s 4−l6 c 2 c 4 c 5 s 1 s 3−l 6 c 3 c 4 c 5 s 1 s 2
l 6 c 2 c 3 c 5 c 4−l 6 c 3 s 2 s 5−l6 c 2 s 3 s 5−l 6 c 4 c 5 s 2 s 3

T05= z5 p5

(
−c5 ( c 1 c 2 c 4 s3−s 1 s4 + c 1 c 3 c 4 s 2 )−c 23 c 1 s5 c 4 s 1+ c1 c 2 s3 s4 + c 1 c 3 s 2 s 4 c 23 c1 c 5−s5 (c1 c 2 c 4 s 3−s 1 s 4 +c 1 c3 c 4 s2 ¿−c
c 23 c 4 c5 −s 23 s5 −c 24 s 4

23
z5_trans

(
0 −s 23 c 5−c 23 c 4 s 5 c 23 c 5 s 1−
¿ s 23 s 5+c 23 c 4 s 5 0 s 5 c (c 1 c 2
( )
s 5 c 1 s 4+c 2 c 4 s 1 s 3+c 3 c 4 s 1 s 2 −c 23 c 5 s 1 c 23 c 1 c 5−s 5 (c 1 c 2 c 4 s 3−s 1 s 4+c 1 c 3 c 4 s 2)

 z5.(p7-p5)=Jv6

()
0
Jv6 = 0
0

 Prismatic joint

() ( )
0 s1
Jw1= 0 Jw3= −c1
1 0

( ) ( )
s1 c23 c 1
Jw2= −c1 Jw4= c 23 s1
0 s 23

Jw6=¿

( )
c 4 s1 +c 1 c2 s 3 s 4 +c 1 c 3 s2 s 4
Jw5= c2 s 1 s3 s4 −c 1 c 4 +c 3 s1 s 2 s 4
−c 23 s4

24
5. Controlling and drawing 3D model in Matlab

+To control the robot arm we create the Simulink of GP250 in Matlab

25
+We draw the model of GP250 3D plane by using SolidWorks.

 We separate it into each of block that we Simulink into Matlab.

26
 The result of the Simulink we have 3D model in Matlab

27
28