NATIONAL CHENG KUNG UNIVERSITY

Department of Mechanical Engineering

DYNAMICS OF MACHINERY

HOMEWORK 1

Professor name: Chiou, Shen-Tarng

Student name: Nguyen Van Thanh
Student ID: P96007019
Class: N1-047 Dynamics of Machinery
Time: [3] 5  7
 1

Contents
The Problem No.1..............................................................................2
The Solution.......................................................................................5
1.Position solution..................................................................6
2.Kinematic coefficient..........................................................7
3.Velocity................................................................................ 8
4.Acceleration.........................................................................8
5.Motion of points..................................................................9
Conclusion and suggestion................................................................ 15
References..........................................................................................16
Appendix A1 The result with theta2 = 2*pi/3................................... 16
Appendix A2 The result with theta2 = 2*pi/3 + 5*pi/180.................19
Appendix A3 The result with theta2 = 2*pi/3 + 2*5*pi/180.............23
Appendix A4 The result with theta2 = 2*pi/3 + 3*5*pi/180.............26
Appendix A5 The result with theta2 = 2*pi/3 + 4*5*pi/180.............30
Appendix C1 Program for the first case, theta2 = 2*pi/3..................34
 2

The Problem No.1

Dynamics of Machinery

Kinematic Analysis of the Stephenson III Six-bar Linkage with a Spring and a Damper

Homework #1 (10 %) Due to 03/23/2011

Please develop programs on a PC and using the package for the kinematic analysis of the Stephenson III
six-bar linkage with a spring and a damper, whose skeleton is shown in Fig. 1 and the dimensions are
given in Table 1. Regarding the dimensions of the spring, the stiffness K  20 N mm , the initial
length L0  91.52 mm, and X s  50 mm, Ys  40 mm. The damping coefficient, dc, of the
damper is 220 N.s/mm, and X d  50 mm, Yd  0 mm. Please use the analytical method for the
displacement analysis. Link 2 is driven with constant speeds of 10, 30, 60, 90, and 120 rad/sec,
respectively. The results, with 5o interval, of displacement, velocity and acceleration of each moving
link and its mass center are required; furthermore the spring length and the rate of change of the
damper length are also needed. Please check the results, by choosing 2 (not particular) positions, with
graphical methods. Please investigate the relations between the input velocity and the maximum
magnitudes of the velocity and acceleration, respectively, of the mass center of link 5. (Hints: The
derivation and programs should be able to analyze the cases with non-zero input acceleration, and
kinematic coefficients should be gotten before the velocity and acceleration analysis.)

項目 配分 得分
結果與討論 40%
結論與建議 10%
報告寫作 40%
報告美觀 10%
總得分 100%
 3

I ( X d , Yd )

rd
H
J ( X s , Ys ) m5
F 6
r5   5
l6
5
l5 6 m6
rs C 4
r5
5
l4 r6
E
m3 4
m4
r3 l3 3 r3 G
3 r4
3

B D r7
m2 r2 1
2 2
A 1 r1
l2 X
r1 r7

Fig. 1 Skeleton of the mechanism

Table 1 Dimensions of the mechanism

Ii
ri ri  mi li i i i
i (kg- m
2
(mm) (mm) (kg) (mm) (deg) (deg) (deg)
)
1 87.01 15.01 --- --- --- --- 0.00 ---
2 30.43 --- 37.49 25.12 0.00 0.0307 --- ---
3 78.61 51.62 62.78 38.81 12.32 1.0941 --- 30.75
4 55.92 --- 50.89 40.33 -8.78 0.6700 --- ---
5 88.45 44.91 21.19 37.71 10.25 0.4420 --- 30.82
6 98.91 --- 32.90 52.62 5.42 1.1271 --- ---
7 130.03 45.01 --- --- --- --- 0.00 ---
 4

Introduction

The main objective of this report is to provide the students with the opportunity to learn and improve
their programming skills using the Matlab environment to solve the problem in Kinematic analysis, of
course including do the Kinematic Analysis. With this basic Matlab in Kinematic analysis problem
solving, the report provides opportunities to explore the application of Matlab in Mechanical
Engineering. The problem we want to solve is kinematic analysis of the Stephenson III sex-bar linkage
with a Spring and a Damper. Talk specifically to the problem, given the state of motion (position,
velocity and acceleration) of one link (in this case is link 2, the “input” link), determine the state of
motion (position, velocity, and acceleration) of all other links and particular points of interest (mass
centers). Before using Matlab to solve the problem, we need to choose a method first to do the kinematic
analysis; the used method is the vector loop that is briefed in the content. The report is a practice for
everyone to learn about Kinematic Analysis, programming, and the application of Matlab in Mechanical
Engineering. This is the first time I do, so I hope that it is useful.
 5

The Solution

I would like to develop a program on PC by using Matlab software for the kinematic analysis of the
Stephenson III six-bar linkage with a Spring and a Damper as shown in Fig. 1. The data is provided in
Table 1.

To understand clearly and deeply the solving, the vector loop approach is briefed first. Briefly, the vector
loop approach consists of the following steps.

1. Attach to the links of the mechanism vectors forming a closed loop (or loops).
2. The vectors should be chosen and defined such that the variables (vector lengths or angles) are
variables of interest in the mechanism.
3. Write the vector loop equation. This is simply the statement that the sum of the vectors in the loop
is zero.
4. Choose an X, Y coordinate system. Break the vector loop position equation into two scalar
component equations. Solve these for the position unknowns.
5. Differentiate the position equation with respect to time. Solve these for the velocity unknowns.
6. Differentiate again with respect to time. Solve for the acceleration unknowns.

For more detail, refer to the reference book number one.

We use the vector loop approach to solve this problem. All of vectors are chosen and defined as shown in
Fig. 1. Take a look the Fig. 1, there have two vector loops and we have two vector loop position
equations are:

r´2 + r´3 + r´4−r´1− r´11=0(1)

r´2 + r´33 + r´5 + r´6 −r´7−r 77 =0 ( 2 )

The input link is link 2, and we have position and state of motion: θ2 , θ˙2 , θ¨2 . The given constants is in the
table 1 below,

Table 1 Dimensions of the mechanism

Ii
ri ri  mi li i i i
i (kg- m
2
(mm) (mm) (kg) (mm) (deg) (deg) (deg)
)
1 87.01 15.01 --- --- --- --- 0.00 ---
2 30.43 --- 37.49 25.12 0.00 0.0307 --- ---
3 78.61 51.62 62.78 38.81 12.32 1.0941 --- 30.75
4 55.92 --- 50.89 40.33 -8.78 0.6700 --- ---
5 88.45 44.91 21.19 37.71 10.25 0.4420 --- 30.82
6 98.91 --- 32.90 52.62 5.42 1.1271 --- ---
7 130.03 45.01 --- --- --- --- 0.00 ---
 6

And the others are in the problem paragraph. We choose the coordinates X, Y, and A is origin
coordinates, AX is parallel to r1. For writing easier in Matlab we use var 'i=var ii.

1. Position solution

We write again the vector loop equation (1),

r´2 + r´3 + r´4−r´1− r´11=0(1)

Break that equation into scalar component equation, we have:

r2*cos(theta2) + r3*cos(theta3) + r4*cos(theta4) - r1 = 0

r2*sin(theta2) + r3*sin(theta3) + r4*sin(theta4) - r11 = 0

The variables in these equations are theta3 and theta4, the problem now is to solve for theta3 and theta4.
One way to do this is by an iterative procedure known as Newton – Raphson’ method, based on the
following reasoning which involves first estimating values for the unknowns, then systematically
correcting those values until desired precision is obtaind.
For the first solution, we choose theta2 = 2*pi/3.
After calculating by using Matlab, the result is shown below,
k theta3(k) theta4(k) f1 f2
0 1.0472 5.2360 -34.9600 30.9933
1 0.3962 5.0427 -11.5671 -11.2195
2 0.4836 5.3115 -1.0960 1.7174
3 0.4576 5.3146 -0.0239 -0.0120
4 0.4575 5.3151 -0.0000 0.0000
Where k is the number of iterations, theta3 (0) and theta4 (0) are the estimate of theta3 and theta4 or the
initial inputs, and
f1 = r2*cos (theta2) + r3*cos (theta3) + r4*cos (theta4) - r1
f2 = r2*sin (theta2) + r3*sin (theta3) + r4*sin (theta4) - r11
In 4 iterations, the computation process converged, and the root is,
theta3 = 0.4575 , theta4 = 5.3151
Move on the vector loop equation (2): r´2 + r´33 + r´5 + r´6 −r´7−r 77 =0
Similarly, we have the result for this equation,
k theta5(k) theta6(k) f1 f2
0 0.5236 5.4978 29.4365 -1.0974
1 0.7763 5.2367 -4.4779 0.9669
2 0.7404 5.2630 -0.0586 -0.0101
3 0.7401 5.2635 -0.0000 0.0000
The root is theta5 = 0.7401 , theta6 = 5.2635.
For the Spring and the Damper, we use the coordinate’s method. We can calculate the coordinate of point
E and H:
 7

XE = r2*cos(theta2)+r33*cos(theta3+theta33)
YE = r2*sin(theta2)+r33*sin(theta3+theta33)
XH = r2*cos(theta2)+r33*cos(theta3+theta33)+ r55*cos(theta5+theta55)
YH = r2*cos(theta2)+r33*cos(theta3+theta33)+ r55*cos(theta5+theta55)
And rs = sqrt((XE-Xs).^2+(YE-Ys).^2)
rd = sqrt((Xd-XH).^2+(Yd-YH).^2)
thetas = atan((YE-Ys)/(XE-Xs))
thetad = atan((YH-Yd)/(XH-Xd))
After calculate, the result is below,
rs = 126.4040 thetas = 1.0497 rd = 80.1821 thetad = 0.3288.

2. Kinematic coefficient

We use the following symbolism, d(thetak)/d(theta2) = hk and d2(thetak)/(d(theta2))2 = hkk.

For h3 and h4:
Write the scalar component equations for calculating theta3 and theta4 again:
r2*cos(theta2) + r3*cos(theta3) + r4*cos(theta4) - r1 = 0
r2*sin(theta2) + r3*sin(theta3) + r4*sin(theta4) - r11 = 0
After differentiating with respect to the input variable theta2, we have
-r2*sin(theta2)-r3*sin(theta3)*h3-r4*sin(theta4)*h4 = 0
r2*cos(theta2)+r3*cos(theta3)*h3+r4*cos(theta4)*h4 = 0
After calculate, the result is below,
h3 = 0.0626 h4 = -0.6193.
For h5 and h6:
Write the scalar component equations for calculating theta5 and theta6 again:
r2*cos(theta2)+ r33*cos(theta3+theta33)+ r5*cos(theta5)+r6*cos(theta6)- r7 = 0
r2*sin(theta2)+r33*cos(theta3+theta33)+ r5*sin(theta5)+r6*sin(theta6)-r77 = 0
After differentiating with respect to the input variable theta2, we have
-r2*sin(theta2)-r33*sin(theta3+theta33)*h3-r5*sin(theta5)*h5-r6*sin(theta6)*h6 = 0.
r2*cos(theta2)+ r33*cos(theta3+theta33)*h3+r5*cos(theta5)*h5+r6*cos(theta6)*h6 = 0.
After calculate, the result is below,
h5 = -0.0432 h6 = 0.3143.
For h33 and h44:
Write the scalar component equations for calculating h3 and h4 again:
-r2*sin(theta2)-r3*sin(theta3)*h3-r4*sin(theta4)*h4 = 0
r2*cos(theta2)+r3*cos(theta3)*h3+r4*cos(theta4)*h4 = 0
After differentiating with respect to the input variable theta2, we have
 8

-r2*cos(theta2)-r3*cos(theta3)*h3*h3-r3*sin(theta3)*h33-r4*cos(theta4)*h4*h4 -r4*sin(theta4)*h44 = 0
-r2*sin(theta2)-r3*sin(theta3)*h3*h3+r3*cos(theta3)*h33 -r4*sin(theta4)*h4*h4 + r4*cos(theta4)*h44 =
0
After calculate, the result is below,
h33 = 0.1137 h44 = 0.0253.
For h55 and h66:
Write the scalar component equations for calculating h5 and h6 again:
-r2*sin(theta2)-r33*sin(theta3+theta33)*h3-r5*sin(theta5)*h5-r6*sin(theta6)*h6 = 0.
r2*cos(theta2)+ r33*cos(theta3+theta33)*h3+r5*cos(theta5)*h5+r6*cos(theta6)*h6 = 0
After differentiating with respect to the input variable theta2, we have
-r2*cos(theta2)-r33*cos(theta3+theta33)*h3*h3-r33*sin(theta3+theta33)*h33-r5*cos(theta5)*h5*h5 -
r5*sin(theta5)*h55-r6*cos(theta6)*h6*h6-r6*sin(theta6)*h66 = 0
-r2*sin(theta2)-r33*sin(theta3+theta33)*h3*h3+r33*cos(theta3+theta33)*h33-r5*sin(theta5)*h5*h5 +
r5*cos(theta5)*h55-r6*sin(theta6)*h6*h6 +r6*cos(theta6)*h66
After calculate, the result is below,
h55 = 0.1780 h66 = 0.0673

3. Velocity

After calculating kinematic coefficients, h3, h4, h5,h6, we have

omegak = hk * omega2, ( k = 3, 4, 5, 6)
Hence, the result is shown below,
omega2 omega3 omega4 omega5 omega6
10.0000 0.6260 -6.1925 -0.4324 3.1428
30.0000 1.8781 -18.5775 -1.2973 9.4284
60.0000 3.7563 -37.1551 -2.5945 18.8568
90.0000 5.6344 -55.7326 -3.8918 28.2852
120.0000 7.5126 -74.3102 -5.1891 37.7136

4. Acceleration

The acceleration of link 2, aa2 is equal to 0 due to this link is driven with constant angular speed.
After calculating kinematic coefficients, h33, h44, h55, h66, we have
aak = hkk * (omega2)2, ( k = 3, 4, 5, 6)
Hence, the result is shown below,
aa2 aa3 aa4 aa5 aa6
0 0.0114 0.0025 0.0178 0.0067
0 0.1024 0.0228 0.1602 0.0606
1.0e+003 * 0 0.4094 0.0912 0.6408 0.2423
0 0.9212 0.2051 1.4417 0.5452
 9

0 1.6378 0.3647 2.5631 0.9692

5. Motion of points

Calculate for mass center of each link.

Link 2, we have,
Xg2 = l2*cos(theta2);
Xg22 = -l2*sin(theta2)*omega2;
Xg222 = -l2*cos(theta2)*omega2.*omega2;
Yg2 = l2*sin(theta2);
Yg22 = l2*cos(theta2)*omega2;
Yg222 = -l2*sin(theta2)*omega2.*omega2;
magvg2 = sqrt(Xg22.^2 + Yg22.^2);
magag2 = sqrt(Xg222.^2 + Yg222.^2);
After calculate, we have

Xg2 Yg2 Xg22 Yg22 magvg2 Xg222 Yg222 magag2

-0.0001 0.0002 -0.0022 -0.0013 0.0025 0.0126 -0.0218 0.0251

-0.0001 0.0002 -0.0065 -0.0038 0.0075 0.1130 -0.1958 0.2261
1.0e+005 * -0.0001 0.0002 -0.0131 -0.0075 0.0151 0.4522 -0.7832 0.9043
-0.0001 0.0002 -0.0196 -0.0113 0.0226 1.0174 -1.7621 2.0347
-0.0001 0.0002 -0.0261 -0.0151 0.0301 1.8086 -3.1327 3.6173

Link 3, we have,
Xg3 = r2*cos(theta2)+l3*cos(theta3+phi3);
Xg33 = -r2*sin(theta2)*omega2 - l3*sin(theta3+phi3)*h3*omega2;
Xg333 = -r2*cos(theta2)*omega2.*omega2 - l3*cos(theta3+phi3)*h3 ...
*h3*omega2.*omega2 - l3*sin(theta3+phi3)*h33*omega2;
Yg3 = r2*sin(theta2)+l3*sin(theta3+phi3);
Yg33 = r2*cos(theta2)*omega2 + l3*cos(theta3+phi3)*h3*omega2;
Yg333 = - r2*sin(theta2)*omega2.*omega2 - l3*sin(theta3+phi3)*h3*h3 ...
*omega2.*omega2 + l3*cos(theta3+phi3)*h33*omega2;
magvg3 = sqrt(Xg33.^2 + Yg33.^2);
magag3 = sqrt(Xg333.^2 + Yg333.^2);
After calculate, we have
Xg3 Yg3 Xg33 Yg33 magvg3 Xg333 Yg333 magag3

0.0002 0.0003 -0.0027 -0.0013 0.0030 0.0150 -0.0260 0.0300

0.0002 0.0003 -0.0081 -0.0039 0.0089 0.1353 -0.2362 0.2722
 10

1.0e+005 * 0.0002 0.0003 -0.0161 -0.0077 0.0179 0.5418 -0.9473 1.0913

0.0002 0.0003 -0.0242 -0.0116 0.0268 1.2195 -2.1333 2.4573
0.0002 0.0003 -0.0322 -0.0154 0.0357 2.1684 -3.7943 4.3702
Link 4, we have,
Xg4 = r2*cos(theta2) + r3*cos(theta3) + l4*cos(theta4+phi4);
Xg44 = -r2*sin(theta2)*omega2 - r3*sin(theta3)*h3*omega2- l4*sin(theta4+phi4)*h4*omega2;
Xg444 = - r2*sin(theta2)*omega2.*omega2 - r3*cos(theta3)*h3 *h3*omega2.*omega2 -
r3*sin(theta3)*h33*omega2- l4*cos(theta4+phi4)*h4*h4*omega2.*omega2-
l4*sin(theta4+phi4)*h44*omega2;
Yg4 = r2*sin(theta2) + r3*sin(theta3) + l4*sin(theta4+phi4);
Yg44 = r2*cos(theta2)*omega2 + r3*cos(theta3)*h3*omega2+ l4*cos(theta4+phi4)*h4*omega2;
Yg444 = - r2*sin(theta2)*omega2.*omega2 - r3*sin(theta3)*h3*h3*omega2.*omega2 +
l3*cos(theta3)*h33*omega2- l4*sin(theta4+phi4)*h4*h4*omega2.*omega2 +
l4*cos(theta4+phi4)*h44*omega2;
magvg4 = sqrt(Xg44.^2 + Yg44.^2);
magag4 = sqrt(Xg444.^2 + Yg444.^2);
After calculate, we have
Xg4 Yg4 Xg44 Yg44 magvg4 Xg444 Yg444 magag4

0.0002 0.0007 -0.0021 0.0013 0.0024 -0.0124 -0.0311 0.0335

0.0002 0.0007 -0.0062 0.0039 0.0073 -0.1090 -0.2817 0.3021
1.0e+005 * 0.0002 0.0007 -0.0124 0.0077 0.0146 -0.4333 -1.1287 1.2090
0.0002 0.0007 -0.0185 0.0116 0.0219 -0.9730 -2.5409 2.7209
0.0002 0.0007 -0.0247 0.0155 0.0291 -1.7281 -4.5184 4.8376

Link 5, we have,
Xg5 = r2*cos(theta2) + r33*cos(theta3+theta33) + l5*cos(theta5+phi5);
Xg55 = -r2*sin(theta2)*omega2 - r33*sin(theta3+theta33)*h3*omega2-
l5*sin(theta5+phi5)*h5*omega2;
Xg555 = - r2*sin(theta2)*omega2.*omega2 - r33*cos(theta3+theta33)*h3*h3*omega2.*omega2 -
r33*sin(theta3+theta33)*h33*omega2- l5*cos(theta5+phi5)*h5*h5*omega2.*omega2 -
l5*sin(theta5+phi5)*h55*omega2;
Yg5 = r2*sin(theta2) + r33*sin(theta3+theta33) + l5*sin(theta5+phi5);
Yg55 = r2*cos(theta2)*omega2 + r33*cos(theta3+theta33)*h3*omega2+
l5*cos(theta5+phi5)*h5*omega2;
Yg555 = - r2*sin(theta2)*omega2.*omega2 - r33*sin(theta3+theta33)*h3*h3*omega2.*omega2 +
l3*cos(theta3+theta33)*h33*omega2- l5*sin(theta5+phi5)*h5*h5*omega2.*omega2+
l5*cos(theta5+phi5)*h55*omega2;
magvg5 = sqrt(Xg55.^2 + Yg55.^2);
magag5 = sqrt(Xg555.^2 + Yg555.^2);
 11

After calculate, we have

Xg5 Yg5 Xg55 Yg55 magvg5 Xg555 Yg555 magag5

0.0001 0.0003 -0.0031 -0.0013 0.0034 -0.0263 -0.0262 0.0371

0.0001 0.0003 -0.0092 -0.0040 0.0101 -0.2376 -0.2374 0.3359
1.0e+005 * 0.0001 0.0003 -0.0184 -0.0081 0.0201 -0.9516 -0.9509 1.3452
0.0001 0.0003 -0.0276 -0.0121 0.0302 -2.1419 -2.1405 3.0281
0.0001 0.0003 -0.0368 -0.0161 0.0402 -3.8085 -3.8063 5.3845

The relation between omega2 and magvg5:

We have:
magvg5 = abs(k*omega2),(k is a real number). Hence, magvg5 = magvg5max when omega2 =
omega2max.
And magag5 = abs(k*omega2.^2),(k is a real number. Hence, magag5 = magag5max
when omega2 = omega2max;

Link 6, we have,
Xg6 = r2*cos(theta2) + r33*cos(theta3+theta33) + r5*cos(theta5) +l6*cos(theta6+phi6);
Xg66 = -r2*sin(theta2)*omega2 - r33*sin(theta3+theta33)*h3*omega2 - r5*sin(theta5)*h5*omega2 -
l6*sin(theta6+phi6)*h6*omega2 ;
Xg666 = - r2*sin(theta2)*omega2.*omega2 - r33*cos(theta3+theta33)*h3*h3*omega2.*omega2 -
r33*sin(theta3+theta33)*h33*omega2- r5*cos(theta5)*h5*h5*omega2.*omega2-
r5*sin(theta5)*h55*omega2 - l6*cos(theta6+phi6)*h6*h6 *omega2.*omega2 -
l6*sin(theta6+phi6)*h66*omega2;
Yg6 = r2*sin(theta2) + r33*sin(theta3+theta33) + r5*sin(theta5)+l6*sin(theta6+phi6);
Yg66 = r2*cos(theta2)*omega2 + r33*cos(theta3+theta33)*h3*omega2 + r5*cos(theta5)*h5*omega2 +
+l6*cos(theta6+phi6);
Yg666 = -r2*sin(theta2)*omega2.*omega2 - r33*sin(theta3+theta33)*h3*h3*omega2.*omega2 +
l3*cos(theta3+theta33)*h33*omega2 -r5*sin(theta5)*h5*h5*omega2.*omega2
+r5*cos(theta5)*h55*omega2 -r6*sin(theta6+phi6)*h6*h6*omega2.*omega2+r6*cos(theta6+phi6)
*h66*omega2;
magvg6 = sqrt(Xg66.^2 + Yg66.^2);
magag6 = sqrt(Xg666.^2 + Yg666.^2);
After calculate, we have
Xg6 Yg6 Xg66 Yg66 magvg6 Xg666 Yg666 magag6

0.0006 0.0008 -0.0011 -0.0018 0.0021 -0.0262 -0.0161 0.0308

0.0006 0.0008 -0.0032 -0.0050 0.0060 -0.2286 -0.1524 0.2747
1.0e+005 * 0.0006 0.0008 -0.0064 -0.0099 0.0118 -0.9069 -0.6169 1.0969
0.0006 0.0008 -0.0097 -0.0148 0.0177 -2.0351 -1.3935 2.4665
0.0006 0.0008 -0.0129 -0.0197 0.0235 -3.6131 -2.4821 4.3836
12
We want to show some important data in the table below,
Table 2 The result with theta2 = 2*pi/3
theta(i) omega(i) aa(i)
1.0e+003 *
0.0021 0.0100 0.0300 0.0600 0.0900 0.1200 0 0 0 0 0
0.0005 0.0006 0.0019 0.0038 0.0056 0.0075 0.0114 0.1024 0.4094 0.9212 1.6378
0.0053 -0.0062 -0.0186 -0.0372 -0.0557 -0.0743 0.0025 0.0228 0.0912 0.2051 0.3647
0.0007 -0.0004 -0.0013 -0.0026 -0.0039 -0.0052 0.0178 0.1602 0.6408 1.4417 2.5631
0.0053 0.0031 0.0094 0.0189 0.0283 0.0377 0.0067 0.0606 0.2423 0.5452 0.9692
Table 3 The result with theta2 = 2*pi/3 + 5*pi/180
theta(i) omega(i) aa(i)
1.0e+003 *
0.0022 0.0100 0.0300 0.0600 0.0900 0.1200 0 0 0 0 0
0.0005 0.0003 0.0010 0.0021 0.0031 0.0041 0.0144 0.1299 0.5196 1.1690 2.0782
0.0054 -0.0059 -0.0176 -0.0352 -0.0528 -0.0704 -0.0016 -0.0145 -0.0580 -0.1304 -0.2318
0.0007 -0.0001 -0.0002 -0.0004 -0.0006 -0.0008 0.0159 0.1432 0.5729 1.2890 2.2916
0.0053 0.0031 0.0094 0.0188 0.0282 0.0376 0.0044 0.0395 0.1581 0.3557 0.6323

Table 4 The result with theta2 = 2*pi/3 + 2*5*pi/180

theta(i) omega(i) aa(i)
1.0e+003 *
0.0023 0.0100 0.0300 0.0600 0.0900 0.1200 0 0 0 0 0
0.0005 0.0000 0.0000 0.0000 0.0000 0.0000 0.0182 0.1635 0.6540 1.4716 2.6162
0.0054 -0.0054 -0.0163 -0.0327 -0.0490 -0.0653 -0.0061 -0.0553 -0.2214 -0.4981 -0.8855
0.0007 0.0003 0.0010 0.0019 0.0029 0.0039 0.0138 0.1243 0.4972 1.1187 1.9888
0.0053 0.0031 0.0093 0.0186 0.0279 0.0372 0.0025 0.0226 0.0903 0.2032 0.3612
 13

Table 5 The result with theta2 = 2*pi/3 + 3*5*pi/180

theta(i) omega(i) aa(i)
1.0e+003 *
0.0024 0.0100 0.0300 0.0600 0.0900 0.1200 0 0 0 0 0
0.0005 -0.0004 -0.0013 -0.0026 -0.0038 -0.0051 0.0227 0.2039 0.8155 1.8348 3.2619
0.0055 -0.0049 -0.0146 -0.0293 -0.0439 -0.0585 -0.0110 -0.0993 -0.3971 -0.8935 -1.5884
0.0007 0.0008 0.0023 0.0046 0.0069 0.0092 0.0115 0.1033 0.4130 0.9293 1.6521
0.0053 0.0030 0.0091 0.0182 0.0274 0.0365 0.0012 0.0112 0.0450 0.1012 0.1799

Table 6 The result with theta2 = 2*pi/3 + 4*5*pi/180

theta(i) omega(i) aa(i)
1.0e+003 *
0.0024 0.0100 0.0300 0.0600 0.0900 0.1200 0 0 0 0 0
0.0005 -0.0010 -0.0030 -0.0059 -0.0089 -0.0119 0.0278 0.2503 1.0013 2.2529 4.0052
0.0055 -0.0041 -0.0123 -0.0246 -0.0369 -0.0493 -0.0159 -0.1433 -0.5731 -1.2895 -2.2924
0.0008 0.0013 0.0038 0.0077 0.0115 0.0154 0.0090 0.0809 0.3236 0.7281 1.2944
0.0054 0.0029 0.0088 0.0177 0.0265 0.0353 0.0009 0.0084 0.0336 0.0756 0.1345
Program for solving this problem is provided in the following files:
For the first case, theta2 = 2*pi/3, the file’s name is cal_var_theta2_1.m
For the second case, theta2 = 2*pi/3 + 5*pi/180, the file’s name is cal_var_theta2_2.m
For the third case, theta2 = 2*pi/3 + 2*5*pi/180, the file’s name is cal_var_theta2_3.m
For the first case, theta2 = 2*pi/3 + 3*5*pi/180, the file’s name is cal_var_theta2_4.m
For the first case, theta2 = 2*pi/3 + 4*5*pi/180, the file’s name is cal_var_theta2_5.m
 15

Conclusion and suggestion

As you know, in the introduction, I said that this is the first time I do. I have never used the Matlab
software before and I understand a little about mechanical, so it is not ease for me to give a conclusion
and suggestion. Tell you the truth, I had to try very hard with my best. I hope to receipt many responses
and contribution.
 16

References

1. Allen S. Hall, Jr., 1986, Notes on mechanism analysis, Waveland Press, Prospect Heights,
Illinois, USA.
2. Norton, Robert L., 1999, Design of machinery: an introduction to the synthesis and analysis
of mechanisms and machines, 2nd ed, McGraw-Hill, Boston, New York, USA.
3. Otto, S. R. (Stephen Robert)/Denier, J. P./Springer, 2005, An introduction to programming
and numerical methods in MATLAB, Springer, London.
4. Rao V. Dukkipati, 2009, MATLAB for mechanical engineers, Tunbridge Wells, Kent TN1
1YS, UK
5. Roger F. Gans, 1991, Analytical kinematics :analysis and synthesis of planar mechanisms,
Butterworth-Heinemann, USA
Appendix A1 The result with theta2 = 2*pi/3

k theta3(k) theta4(k) f1 f2

0 1.0472 5.2360 -34.9600 30.9933

1 0.3962 5.0427 -11.5671 -11.2195

2 0.4836 5.3115 -1.0960 1.7174

3 0.4576 5.3146 -0.0239 -0.0120

4 0.4575 5.3151 -0.0000 0.0000

The Root is: theta3 = 0.4575 , theta4 = 5.3151

-----------------------------------------------------------------------------------------------------------------------------

k theta5(k) theta6(k) f1 f2

0 0.5236 5.4978 29.4365 -1.0974

1 0.7763 5.2367 -4.4779 0.9669

2 0.7404 5.2630 -0.0586 -0.0101

3 0.7401 5.2635 -0.0000 0.0000

The Root is: theta5 = 0.7401 , theta6 = 5.2635

----------------------------------------------------------------------------------------------------------------------------

rs = 126.4040 thetas = 1.0497 rd = 80.1821 thetad = 0.3288

----------------------------------------------------------------------------------------------------------------------------

h3 = 0.0626 h4 = -0.6193

----------------------------------------------------------------------------------------------------------------------------

h5 = -0.0432 h6 = 0.3143

----------------------------------------------------------------------------------------------------------------------------

h33 = 0.1137 h44 = 0.0253

 17

----------------------------------------------------------------------------------------------------------------------------

h55 = 0.1780 h66 = 0.0673

----------------------------------------------------------------------------------------------------------------------------

omega2 omega3 omega4 omega5 omega6

10.0000 0.6260 -6.1925 -0.4324 3.1428

30.0000 1.8781 -18.5775 -1.2973 9.4284

60.0000 3.7563 -37.1551 -2.5945 18.8568

90.0000 5.6344 -55.7326 -3.8918 28.2852

120.0000 7.5126 -74.3102 -5.1891 37.7136

----------------------------------------------------------------------------------------------------------------------------

aa2 aa3 aa4 aa5 aa6

1.0e+003 *

0 0.0114 0.0025 0.0178 0.0067

0 0.1024 0.0228 0.1602 0.0606

0 0.4094 0.0912 0.6408 0.2423

0 0.9212 0.2051 1.4417 0.5452

0 1.6378 0.3647 2.5631 0.9692

----------------------------------------------------------------------------------------------------------------------------

Xg2 Yg2 Xg22 Yg22 magvg2 Xg222 Yg222 magag2

1.0e+005 *

-0.0001 0.0002 -0.0022 -0.0013 0.0025 0.0126 -0.0218 0.0251

-0.0001 0.0002 -0.0065 -0.0038 0.0075 0.1130 -0.1958 0.2261

-0.0001 0.0002 -0.0131 -0.0075 0.0151 0.4522 -0.7832 0.9043

 18

-0.0001 0.0002 -0.0196 -0.0113 0.0226 1.0174 -1.7621 2.0347

-0.0001 0.0002 -0.0261 -0.0151 0.0301 1.8086 -3.1327 3.6173

----------------------------------------------------------------------------------------------------------------------------

Xg3 Yg3 Xg33 Yg33 magvg3 Xg333 Yg333 magag3

1.0e+005 *

0.0002 0.0003 -0.0027 -0.0013 0.0030 0.0150 -0.0260 0.0300

0.0002 0.0003 -0.0081 -0.0039 0.0089 0.1353 -0.2362 0.2722

0.0002 0.0003 -0.0161 -0.0077 0.0179 0.5418 -0.9473 1.0913

0.0002 0.0003 -0.0242 -0.0116 0.0268 1.2195 -2.1333 2.4573

0.0002 0.0003 -0.0322 -0.0154 0.0357 2.1684 -3.7943 4.3702

----------------------------------------------------------------------------------------------------------------------------

Xg4 Yg4 Xg44 Yg44 magvg4 Xg444 Yg444 magag4

1.0e+005 *

0.0002 0.0007 -0.0021 0.0013 0.0024 -0.0124 -0.0311 0.0335

0.0002 0.0007 -0.0062 0.0039 0.0073 -0.1090 -0.2817 0.3021

0.0002 0.0007 -0.0124 0.0077 0.0146 -0.4333 -1.1287 1.2090

0.0002 0.0007 -0.0185 0.0116 0.0219 -0.9730 -2.5409 2.7209

0.0002 0.0007 -0.0247 0.0155 0.0291 -1.7281 -4.5184 4.8376

----------------------------------------------------------------------------------------------------------------------------

Xg5 Yg5 Xg55 Yg55 magvg5 Xg555 Yg555 magag5

1.0e+005 *

0.0001 0.0003 -0.0031 -0.0013 0.0034 -0.0263 -0.0262 0.0371

0.0001 0.0003 -0.0092 -0.0040 0.0101 -0.2376 -0.2374 0.3359

 19

0.0001 0.0003 -0.0184 -0.0081 0.0201 -0.9516 -0.9509 1.3452

0.0001 0.0003 -0.0276 -0.0121 0.0302 -2.1419 -2.1405 3.0281

0.0001 0.0003 -0.0368 -0.0161 0.0402 -3.8085 -3.8063 5.3845

----------------------------------------------------------------------------------------------------------------------------

Xg6 Yg6 Xg66 Yg66 magvg6 Xg666 Yg666 magag6

1.0e+005 *

0.0006 0.0008 -0.0011 -0.0018 0.0021 -0.0262 -0.0161 0.0308

0.0006 0.0008 -0.0032 -0.0050 0.0060 -0.2286 -0.1524 0.2747

0.0006 0.0008 -0.0064 -0.0099 0.0118 -0.9069 -0.6169 1.0969

0.0006 0.0008 -0.0097 -0.0148 0.0177 -2.0351 -1.3935 2.4665

0.0006 0.0008 -0.0129 -0.0197 0.0235 -3.6131 -2.4821 4.3836

Appendix A2 The result with theta2 = 2*pi/3 + 5*pi/180

k theta3(k) theta4(k) f1 f2

0 1.0472 5.2360 -37.1989 29.5669

1 0.3979 5.0913 -11.3082 -11.5786

2 0.4815 5.3580 -1.1454 1.6580

3 0.4556 5.3625 -0.0239 -0.0116

4 0.4555 5.3630 -0.0000 0.0000

The Root is: theta3 = 0.4555 , theta4 = 5.3630

----------------------------------------------------------------------------------------------------------------------------

k theta5(k) theta6(k) f1 f2

0 0.5236 5.4978 27.2855 -2.5811

1 0.7708 5.2640 -3.9720 0.5153

2 0.7416 5.2898 -0.0448 0.0019

 20

3 0.7413 5.2901 -0.0000 0.0000

The Root is: theta5 = 0.7413 , theta6 = 5.2901

----------------------------------------------------------------------------------------------------------------------------

rs = 124.0515 thetas = 1.0588 rd = 77.3995 thetad = 0.3110

----------------------------------------------------------------------------------------------------------------------------

h3 = 0.0343 h4 = -0.5868

----------------------------------------------------------------------------------------------------------------------------

h5 = -0.0070 h6 = 0.3137

----------------------------------------------------------------------------------------------------------------------------

h33 = 0.1443 h44 = -0.0161

----------------------------------------------------------------------------------------------------------------------------

h55 = 0.1591 h66 = 0.0439

----------------------------------------------------------------------------------------------------------------------------

omega2 omega3 omega4 omega5 omega6

10.0000 0.3430 -5.8685 -0.0701 3.1367

30.0000 1.0291 -17.6055 -0.2102 9.4102

60.0000 2.0582 -35.2109 -0.4205 18.8204

90.0000 3.0873 -52.8164 -0.6307 28.2306

120.0000 4.1163 -70.4218 -0.8410 37.6408

----------------------------------------------------------------------------------------------------------------------------

aa2 aa3 aa4 aa5 aa6

1.0e+003 *

0 0.0144 -0.0016 0.0159 0.0044

 21

0 0.1299 -0.0145 0.1432 0.0395

0 0.5196 -0.0580 0.5729 0.1581

0 1.1690 -0.1304 1.2890 0.3557

0 2.0782 -0.2318 2.2916 0.6323

----------------------------------------------------------------------------------------------------------------------------

Xg2 Yg2 Xg22 Yg22 magvg2 Xg222 Yg222 magag2

1.0e+005 *

-0.0001 0.0002 -0.0021 -0.0014 0.0025 0.0144 -0.0206 0.0251

-0.0001 0.0002 -0.0062 -0.0043 0.0075 0.1297 -0.1852 0.2261

-0.0001 0.0002 -0.0123 -0.0086 0.0151 0.5187 -0.7408 0.9043

-0.0001 0.0002 -0.0185 -0.0130 0.0226 1.1671 -1.6667 2.0347

-0.0001 0.0002 -0.0247 -0.0173 0.0301 2.0748 -2.9631 3.6173

----------------------------------------------------------------------------------------------------------------------------

Xg3 Yg3 Xg33 Yg33 magvg3 Xg333 Yg333 magag3

1.0e+005 *

0.0002 0.0003 -0.0025 -0.0016 0.0030 0.0173 -0.0244 0.0299

0.0002 0.0003 -0.0076 -0.0048 0.0090 0.1563 -0.2228 0.2722

0.0002 0.0003 -0.0151 -0.0097 0.0180 0.6260 -0.8944 1.0917

0.0002 0.0003 -0.0227 -0.0145 0.0269 1.4091 -2.0149 2.4587

0.0002 0.0003 -0.0302 -0.0194 0.0359 2.5055 -3.5842 4.3732

----------------------------------------------------------------------------------------------------------------------------

Xg4 Yg4 Xg44 Yg44 magvg4 Xg444 Yg444 magag4

 22

1.0e+005 *

0.0001 0.0007 -0.0020 0.0008 0.0021 -0.0121 -0.0282 0.0307

0.0001 0.0007 -0.0059 0.0023 0.0063 -0.1062 -0.2570 0.2781

0.0001 0.0007 -0.0118 0.0046 0.0127 -0.4221 -1.0314 1.1144

0.0001 0.0007 -0.0177 0.0070 0.0190 -0.9475 -2.3232 2.5090

0.0001 0.0007 -0.0236 0.0093 0.0254 -1.6825 -4.1325 4.4619

----------------------------------------------------------------------------------------------------------------------------

Xg5 Yg5 Xg55 Yg55 magvg5 Xg555 Yg555 magag5

1.0e+005 *

0.0001 0.0003 -0.0027 -0.0016 0.0031 -0.0250 -0.0247 0.0351

0.0001 0.0003 -0.0080 -0.0049 0.0094 -0.2247 -0.2239 0.3172

0.0001 0.0003 -0.0160 -0.0099 0.0188 -0.8987 -0.8973 1.2700

0.0001 0.0003 -0.0240 -0.0148 0.0282 -2.0220 -2.0203 2.8583

0.0001 0.0003 -0.0320 -0.0198 0.0376 -3.5945 -3.5929 5.0823

----------------------------------------------------------------------------------------------------------------------------

Xg6 Yg6 Xg66 Yg66 magvg6 Xg666 Yg666 magag6

1.0e+005 *

0.0006 0.0008 -0.0010 -0.0018 0.0021 -0.0249 -0.0144 0.0287

0.0006 0.0008 -0.0030 -0.0052 0.0061 -0.2156 -0.1371 0.2555

0.0006 0.0008 -0.0061 -0.0103 0.0120 -0.8543 -0.5558 1.0192

0.0006 0.0008 -0.0091 -0.0154 0.0179 -1.9161 -1.2561 2.2911

0.0006 0.0008 -0.0122 -0.0205 0.0238 -3.4010 -2.2379 4.0712

Appendix A3 The result with theta2 = 2*pi/3 + 2*5*pi/180

k theta3(k) theta4(k) f1 f2
 23

0 1.0472 5.2360 -39.3050 27.9508

1 0.4030 5.1420 -10.9667 -11.7124

2 0.4803 5.4046 -1.1656 1.5754

3 0.4549 5.4102 -0.0233 -0.0109

4 0.4548 5.4107 -0.0000 0.0000

The Root is: theta3 = 0.4548 , theta4 = 5.4107

----------------------------------------------------------------------------------------------------------------------------

k theta5(k) theta6(k) f1 f2

0 0.5236 5.4978 25.2092 -4.2166

1 0.7671 5.2913 -3.5370 0.0985

2 0.7439 5.3168 -0.0351 0.0104

3 0.7436 5.3170 -0.0000 -0.0000

The Root is: theta5 = 0.7436 , theta6 = 5.3170

----------------------------------------------------------------------------------------------------------------------------

rs = 121.6127 thetas = 1.0671 rd = 74.6763 thetad = 0.2922

----------------------------------------------------------------------------------------------------------------------------

h3 = 0.0001 h4 = -0.5443

----------------------------------------------------------------------------------------------------------------------------

h5 = 0.0324 h6 = 0.3103

----------------------------------------------------------------------------------------------------------------------------

h33 = 0.1817 h44 = -0.0615

----------------------------------------------------------------------------------------------------------------------------

h55 = 0.1381 h66 = 0.0251

 24

----------------------------------------------------------------------------------------------------------------------------

omega2 omega3 omega4 omega5 omega6

10.0000 0.0013 -5.4434 0.3236 3.1034

30.0000 0.0039 -16.3302 0.9707 9.3103

60.0000 0.0079 -32.6603 1.9413 18.6206

90.0000 0.0118 -48.9905 2.9120 27.9309

120.0000 0.0158 -65.3207 3.8826 37.2412

----------------------------------------------------------------------------------------------------------------------------

aa2 aa3 aa4 aa5 aa6

1.0e+003 *

0 0.0182 -0.0061 0.0138 0.0025

0 0.1635 -0.0553 0.1243 0.0226

0 0.6540 -0.2214 0.4972 0.0903

0 1.4716 -0.4981 1.1187 0.2032

0 2.6162 -0.8855 1.9888 0.3612

----------------------------------------------------------------------------------------------------------------------------

Xg2 Yg2 Xg22 Yg22 magvg2 Xg222 Yg222 magag2

1.0e+005 *

-0.0002 0.0002 -0.0019 -0.0016 0.0025 0.0161 -0.0192 0.0251

-0.0002 0.0002 -0.0058 -0.0048 0.0075 0.1453 -0.1732 0.2261

-0.0002 0.0002 -0.0115 -0.0097 0.0151 0.5813 -0.6927 0.9043

-0.0002 0.0002 -0.0173 -0.0145 0.0226 1.3079 -1.5587 2.0347

-0.0002 0.0002 -0.0231 -0.0194 0.0301 2.3251 -2.7710 3.6173

 25

----------------------------------------------------------------------------------------------------------------------------

Xg3 Yg3 Xg33 Yg33 magvg3 Xg333 Yg333 magag3

1.0e+005 *

0.0002 0.0003 -0.0023 -0.0020 0.0030 0.0194 -0.0226 0.0298

0.0002 0.0003 -0.0070 -0.0059 0.0091 0.1756 -0.2077 0.2720

0.0002 0.0003 -0.0140 -0.0117 0.0183 0.7033 -0.8350 1.0917

0.0002 0.0003 -0.0210 -0.0176 0.0274 1.5830 -1.8820 2.4592

0.0002 0.0003 -0.0280 -0.0235 0.0365 2.8149 -3.3485 4.3745

----------------------------------------------------------------------------------------------------------------------------

Xg4 Yg4 Xg44 Yg44 magvg4 Xg444 Yg444 magag4

1.0e+005 *

0.0001 0.0007 -0.0018 0.0002 0.0018 -0.0122 -0.0251 0.0280

0.0001 0.0007 -0.0055 0.0006 0.0055 -0.1067 -0.2315 0.2549

0.0001 0.0007 -0.0110 0.0011 0.0111 -0.4235 -0.9311 1.0229

0.0001 0.0007 -0.0165 0.0017 0.0166 -0.9504 -2.0988 2.3040

0.0001 0.0007 -0.0220 0.0022 0.0221 -1.6872 -3.7348 4.0982

----------------------------------------------------------------------------------------------------------------------------

Xg5 Yg5 Xg55 Yg55 magvg5 Xg555 Yg555 magag5

1.0e+005 *

0.0001 0.0003 -0.0022 -0.0020 0.0030 -0.0236 -0.0229 0.0329

0.0001 0.0003 -0.0066 -0.0059 0.0089 -0.2106 -0.2083 0.2962

0.0001 0.0003 -0.0133 -0.0117 0.0177 -0.8408 -0.8355 1.1853

0.0001 0.0003 -0.0199 -0.0176 0.0266 -1.8905 -1.8815 2.6672

 26

0.0001 0.0003 -0.0265 -0.0235 0.0354 -3.3599 -3.3464 4.7421

----------------------------------------------------------------------------------------------------------------------------

Xg6 Yg6 Xg66 Yg66 magvg6 Xg666 Yg666 magag6

1.0e+005 *

0.0006 0.0008 -0.0009 -0.0019 0.0021 -0.0236 -0.0129 0.0269

0.0006 0.0008 -0.0028 -0.0054 0.0061 -0.2032 -0.1238 0.2379

0.0006 0.0008 -0.0057 -0.0106 0.0120 -0.8039 -0.5026 0.9481

0.0006 0.0008 -0.0085 -0.0158 0.0180 -1.8021 -1.1363 2.1304

0.0006 0.0008 -0.0114 -0.0211 0.0239 -3.1978 -2.0249 3.7850

Appendix A4 The result with theta2 = 2*pi/3 + 3*5*pi/180

k theta3(k) theta4(k) f1 f2

0 1.0472 5.2360 -41.2623 26.1574

1 0.4114 5.1942 -10.5619 -11.6102

2 0.4802 5.4510 -1.1577 1.4736

3 0.4556 5.4574 -0.0219 -0.0100

4 0.4555 5.4579 -0.0000 0.0000

The Root is: theta3 = 0.4555 , theta4 = 5.4579

----------------------------------------------------------------------------------------------------------------------------

k theta5(k) theta6(k) f1 f2

0 0.5236 5.4978 23.2219 -5.9904

1 0.7654 5.3186 -3.1765 -0.2800

2 0.7473 5.3441 -0.0290 0.0162

3 0.7469 5.3442 -0.0000 -0.0000

The Root is: theta5 = 0.7469 , theta6 = 5.3442

 27

----------------------------------------------------------------------------------------------------------------------------

rs = 119.1032 thetas = 1.0745 rd = 72.0354 thetad = 0.2724

----------------------------------------------------------------------------------------------------------------------------

h3 = -0.0427 h4 = -0.4877

----------------------------------------------------------------------------------------------------------------------------

h5 = 0.0765 h6 = 0.3040

----------------------------------------------------------------------------------------------------------------------------

h33 = 0.2265 h44 = -0.1103

----------------------------------------------------------------------------------------------------------------------------

h55 = 0.1147 h66 = 0.0125

----------------------------------------------------------------------------------------------------------------------------

omega2 omega3 omega4 omega5 omega6

10.0000 -0.4274 -4.8772 0.7648 3.0402

30.0000 -1.2822 -14.6315 2.2945 9.1207

60.0000 -2.5644 -29.2630 4.5890 18.2414

90.0000 -3.8466 -43.8945 6.8836 27.3621

120.0000 -5.1288 -58.5260 9.1781 36.4828

----------------------------------------------------------------------------------------------------------------------------

aa2 aa3 aa4 aa5 aa6

1.0e+003 *

0 0.0227 -0.0110 0.0115 0.0012

0 0.2039 -0.0993 0.1033 0.0112

0 0.8155 -0.3971 0.4130 0.0450

 28

0 1.8348 -0.8935 0.9293 0.1012

0 3.2619 -1.5884 1.6521 0.1799

----------------------------------------------------------------------------------------------------------------------------

Xg2 Yg2 Xg22 Yg22 magvg2 Xg222 Yg222 magag2

1.0e+005 *

-0.0002 0.0002 -0.0018 -0.0018 0.0025 0.0178 -0.0178 0.0251

-0.0002 0.0002 -0.0053 -0.0053 0.0075 0.1599 -0.1599 0.2261

-0.0002 0.0002 -0.0107 -0.0107 0.0151 0.6395 -0.6395 0.9043

-0.0002 0.0002 -0.0160 -0.0160 0.0226 1.4388 -1.4388 2.0347

-0.0002 0.0002 -0.0213 -0.0213 0.0301 2.5578 -2.5578 3.6173

----------------------------------------------------------------------------------------------------------------------------

Xg3 Yg3 Xg33 Yg33 magvg3 Xg333 Yg333 magag3

1.0e+005 *

0.0002 0.0003 -0.0021 -0.0023 0.0031 0.0213 -0.0207 0.0297

0.0002 0.0003 -0.0064 -0.0069 0.0094 0.1925 -0.1912 0.2713

0.0002 0.0003 -0.0127 -0.0139 0.0188 0.7710 -0.7700 1.0897

0.0002 0.0003 -0.0191 -0.0208 0.0282 1.7356 -1.7364 2.4551

0.0002 0.0003 -0.0254 -0.0278 0.0376 3.0863 -3.0903 4.3675

----------------------------------------------------------------------------------------------------------------------------

Xg4 Yg4 Xg44 Yg44 magvg4 Xg444 Yg444 magag4

1.0e+005 *

0.0001 0.0006 -0.0017 -0.0005 0.0017 -0.0129 -0.0221 0.0256

0.0001 0.0006 -0.0050 -0.0016 0.0052 -0.1120 -0.2060 0.2345

 29

0.0001 0.0006 -0.0099 -0.0031 0.0104 -0.4437 -0.8315 0.9425

0.0001 0.0006 -0.0149 -0.0047 0.0156 -0.9953 -1.8765 2.1241

0.0001 0.0006 -0.0198 -0.0062 0.0208 -1.7665 -3.3409 3.7792

----------------------------------------------------------------------------------------------------------------------------

Xg5 Yg5 Xg55 Yg55 magvg5 Xg555 Yg555 magag5

1.0e+005 *

0.0001 0.0003 -0.0017 -0.0023 0.0028 -0.0221 -0.0209 0.0304

0.0001 0.0003 -0.0050 -0.0068 0.0085 -0.1958 -0.1909 0.2735

0.0001 0.0003 -0.0101 -0.0136 0.0169 -0.7798 -0.7666 1.0935

0.0001 0.0003 -0.0151 -0.0204 0.0254 -1.7520 -1.7271 2.4602

0.0001 0.0003 -0.0201 -0.0273 0.0339 -3.1125 -3.0723 4.3734

----------------------------------------------------------------------------------------------------------------------------

Xg6 Yg6 Xg66 Yg66 magvg6 Xg666 Yg666 magag6

1.0e+005 *

0.0006 0.0007 -0.0009 -0.0019 0.0021 -0.0224 -0.0119 0.0254

0.0006 0.0007 -0.0026 -0.0054 0.0060 -0.1923 -0.1139 0.2235

0.0006 0.0007 -0.0052 -0.0108 0.0120 -0.7596 -0.4626 0.8894

0.0006 0.0007 -0.0078 -0.0161 0.0179 -1.7019 -1.0462 1.9978

0.0006 0.0007 -0.0104 -0.0214 0.0238 -3.0192 -1.8647 3.5486

Appendix A5 The result with theta2 = 2*pi/3 + 4*5*pi/180

k theta3(k) theta4(k) f1 f2

0 1.0472 5.2360 -43.0557 24.2001

1 0.4231 5.2478 -10.1153 -11.2676

 30

2 0.4811 5.4970 -1.1247 1.3562

3 0.4577 5.5039 -0.0201 -0.0089

4 0.4576 5.5043 -0.0000 0.0000

The Root is: theta3 = 0.4576 , theta4 = 5.5043

----------------------------------------------------------------------------------------------------------------------------

k theta5(k) theta6(k) f1 f2

0 0.5236 5.4978 21.3359 -7.8874

1 0.7655 5.3457 -2.8923 -0.6187

2 0.7516 5.3713 -0.0256 0.0202

3 0.7513 5.3714 -0.0000 -0.0000

The Root is: theta5 = 0.7513 , theta6 = 5.3714

----------------------------------------------------------------------------------------------------------------------------

rs = 116.5395 thetas = 1.0810 rd = 69.4964 thetad = 0.2517

----------------------------------------------------------------------------------------------------------------------------

h3 = -0.0989 h4 = -0.4105

----------------------------------------------------------------------------------------------------------------------------

h5 = 0.1280 h6 = 0.2942

----------------------------------------------------------------------------------------------------------------------------

h33 = 0.2781 h44 = -0.1592

----------------------------------------------------------------------------------------------------------------------------

h55 = 0.0899 h66 = 0.0093

----------------------------------------------------------------------------------------------------------------------------

omega2 omega3 omega4 omega5 omega6

 31

10.0000 -0.9886 -4.1054 1.2799 2.9423

30.0000 -2.9658 -12.3161 3.8396 8.8268

60.0000 -5.9315 -24.6321 7.6792 17.6535

90.0000 -8.8973 -36.9482 11.5188 26.4803

120.0000 -11.8630 -49.2642 15.3584 35.3070

----------------------------------------------------------------------------------------------------------------------------

aa2 aa3 aa4 aa5 aa6

1.0e+003 *

0 0.0278 -0.0159 0.0090 0.0009

0 0.2503 -0.1433 0.0809 0.0084

0 1.0013 -0.5731 0.3236 0.0336

0 2.2529 -1.2895 0.7281 0.0756

0 4.0052 -2.2924 1.2944 0.1345

----------------------------------------------------------------------------------------------------------------------------

Xg2 Yg2 Xg22 Yg22 magvg2 Xg222 Yg222 magag2

1.0e+005 *

-0.0002 0.0002 -0.0016 -0.0019 0.0025 0.0192 -0.0161 0.0251

-0.0002 0.0002 -0.0048 -0.0058 0.0075 0.1732 -0.1453 0.2261

-0.0002 0.0002 -0.0097 -0.0115 0.0151 0.6927 -0.5813 0.9043

-0.0002 0.0002 -0.0145 -0.0173 0.0226 1.5587 -1.3079 2.0347

-0.0002 0.0002 -0.0194 -0.0231 0.0301 2.7710 -2.3251 3.6173

----------------------------------------------------------------------------------------------------------------------------

Xg3 Yg3 Xg33 Yg33 magvg Xg333 Yg333 magag3

 32

1.0e+005 *

0.0001 0.0003 -0.0019 -0.0027 0.0033 0.0227 -0.0186 0.0293

0.0001 0.0003 -0.0056 -0.0081 0.0099 0.2058 -0.1736 0.2692

0.0001 0.0003 -0.0113 -0.0162 0.0198 0.8245 -0.7007 1.0820

0.0001 0.0003 -0.0169 -0.0244 0.0296 1.8561 -1.5813 2.4384

0.0001 0.0003 -0.0225 -0.0325 0.0395 3.3006 -2.8154 4.3383

----------------------------------------------------------------------------------------------------------------------------

Xg4 Yg4 Xg44 Yg44 magvg4 Xg444 Yg444 magag4

1.0e+005 *

0.0001 0.0006 -0.0014 -0.0014 0.0020 -0.0144 -0.0192 0.0240

0.0001 0.0006 -0.0042 -0.0042 0.0059 -0.1243 -0.1825 0.2208

0.0001 0.0006 -0.0083 -0.0083 0.0118 -0.4918 -0.7395 0.8880

0.0001 0.0006 -0.0125 -0.0125 0.0177 -1.1025 -1.6710 2.0019

0.0001 0.0006 -0.0167 -0.0166 0.0236 -1.9564 -2.9771 3.5624

----------------------------------------------------------------------------------------------------------------------------

Xg5 Yg5 Xg55 Yg55 magvg5 Xg555 Yg555 magag5

1.0e+005 *

0.0001 0.0003 -0.0010 -0.0026 0.0028 -0.0207 -0.0188 0.0279

0.0001 0.0003 -0.0031 -0.0078 0.0084 -0.1811 -0.1725 0.2501

0.0001 0.0003 -0.0063 -0.0156 0.0169 -0.7194 -0.6936 0.9993

0.0001 0.0003 -0.0094 -0.0235 0.0253 -1.6147 -1.5633 2.2475

0.0001 0.0003 -0.0125 -0.0313 0.0337 -2.8671 -2.7815 3.9947

----------------------------------------------------------------------------------------------------------------------------
 33

Xg6 Yg6 Xg66 Yg66 magvg6 Xg666 Yg666 magag6

1.0e+005 *

0.0006 0.0007 -0.0008 -0.0019 0.0020 -0.0217 -0.0114 0.0245

0.0006 0.0007 -0.0024 -0.0055 0.0059 -0.1848 -0.1098 0.2150

0.0006 0.0007 -0.0047 -0.0108 0.0118 -0.7291 -0.4462 0.8548

0.0006 0.0007 -0.0071 -0.0161 0.0176 -1.6329 -1.0092 1.9196

0.0006 0.0007 -0.0094 -0.0215 0.0235 -2.8961 -1.7988 3.4092

 34

Appendix C1 Program for the first case, theta2 = 2*pi/3

function cal_var_theta2_1
theta2 = 2*pi/3;

r1 = 87.01; r11 = 15.01; r2 = 30.43; r3 = 78.61; r33 = 51.62; r4 = 55.92;

r5 = 88.45; r55 = 44.91; r6 = 98.91; r7 = 130.03; r77 = 45.01;
l2 = 25.12; l3 = 38.81; l4 = 40.33; l5 = 37.71; l6 = 52.62;
phi2 = 0; phi3 = 12.32; phi4 = -8.78; phi5 = 10.25; phi6 = 5.42;
theta33 = 30.75*pi/180; theta55 = 30.82*pi/180;
omega2 = [ 10 30 60 90 120];
aa2 = [0 0 0 0 0];

% calculate theta3 and theta4.

theta3 = sym('theta3'); theta4 = sym('theta4');

f1 = r2*cos(theta2)+r3*cos(theta3)+r4*cos(theta4)-r1;
f2 = r2*sin(theta2)+r3*sin(theta3)+r4*sin(theta4)-r11;

gradf1 = [diff(f1,'theta3') diff(f1,'theta4')];

% gradf1 = vector string that names the gradient of
% the function f1(theta3,theta4) of two variables.
gradf2 = [diff(f2,'theta3') diff(f2,'theta4')];
% gradf2 = vector string that names the gradient of
% the function f2(theta3,theta4) of two variables.

w = [pi/3 5*pi/3]; % Initial values [theta3 theta4].

v = w';
tol = 0.01; % tol = tolerance to stop calculations (non-negative real).
nitermax = 10; % nitermax = maximun number of iteration to be performed
%(non-negative integer).
disp(' k theta3(k) theta4(k) f1 f2')

theta3 = v(1);
theta4 = v(2);
fv1 = eval(f1);
fv2 = eval(f2);
fxo = [fv1;fv2];
f1pv = eval(gradf1);
f2pv = eval(gradf2);
fpxo =[f1pv;f2pv];
if det(fpxo)==0
disp('Err: determinant of Jacobian is zero, try another initial point ')
else
iter = 0;
s1 = sprintf('%3.0f %- 10.4f %- 10.4f',iter,v(1),v(2));
s2 = sprintf(' %- 10.4f %- 10.4f',fv1,fv2);
disp([s1 s2])
while (norm(fxo)>tol) & (det(fpxo)~=0) & (iter<nitermax)
iter = iter + 1;
vn = v - inv(fpxo)*fxo;
v = vn;
theta3 = v(1);
theta4 = v(2);
fv1 = eval(f1);
fv2 = eval(f2);
fxo = [fv1;fv2];
f1pv = eval(gradf1);
f2pv = eval(gradf2);
fpxo =[f1pv;f2pv];
if det(fpxo)==0
 35
disp('Err:determinant of Jacobian is zero,try another initial point ')
end
s1 = sprintf('%3.0f %- 10.4f %- 10.4f',iter,v(1),v(2));
s2 = sprintf(' %- 10.4f %- 10.4f',fv1,fv2);
disp([s1 s2])
end
root = v;
disp(sprintf('\n The Root is: theta3 = %- 8.4f, theta4 = %- 8.4f',v(1),v(2)))
end;
disp('
--------------------------------------------------------------------------------')
% calculate theta5 and theta6.

theta5 = sym('theta5'); theta6 = sym('theta6');

f1 = r2*cos(theta2)+ r33*cos(theta3+theta33)+ r5*cos(theta5)+r6*cos(theta6)- r7;
f2 = r2*sin(theta2)+r33*sin(theta3+theta33)+ r5*sin(theta5)+r6*sin(theta6)-r77;

gradf1 = [diff(f1,'theta5') diff(f1,'theta6')];

% gradf1 = vector string that names the gradient of
% the function f1(theta5,theta6) of two variables.
gradf2 = [diff(f2,'theta5') diff(f2,'theta6')];
% gradf2 = vector string that names the gradient of
% the function f2(theta5,theta6) of two variables.

w = [30*pi/180 315*pi/180]; % Initial values [theta5 theta6].

v = w';
tol = 0.01; % tol = tolerance to stop calculations (non-negative real).
nitermax = 10; % nitermax = maximun number of iteration to be performed
%(non-negative integer).
disp(' k theta5(k) theta6(k) f1 f2')

theta5 = v(1);
theta6 = v(2);
fv1 = eval(f1);
fv2 = eval(f2);
fxo = [fv1;fv2];
f1pv = eval(gradf1);
f2pv = eval(gradf2);
fpxo =[f1pv;f2pv];
if det(fpxo)==0
disp('Err: determinant of Jacobian is zero, try another initial point ')
else
iter = 0;
s1 = sprintf('%3.0f %- 10.4f %- 10.4f',iter,v(1),v(2));
s2 = sprintf(' %- 10.4f %- 10.4f',fv1,fv2);
disp([s1 s2])
while (norm(fxo)>tol) & (det(fpxo)~=0) & (iter<nitermax)
iter = iter + 1;
vn = v - inv(fpxo)*fxo;
v = vn;
theta5 = v(1);
theta6 = v(2);
fv1 = eval(f1);
fv2 = eval(f2);
fxo = [fv1;fv2];
f1pv = eval(gradf1);
f2pv = eval(gradf2);
fpxo =[f1pv;f2pv];
if det(fpxo)==0
disp('Err:determinant of Jacobian is zero, try another initial point ')
end
s1 = sprintf('%3.0f %- 10.4f %- 10.4f',iter,v(1),v(2));
s2 = sprintf(' %- 10.4f %- 10.4f',fv1,fv2);
 36
disp([s1 s2])
end
root = v;
disp(sprintf('\n The Root is: theta5 = %- 8.4f, theta6 = %- 8.4f',v(1),v(2)))
end;
disp('
--------------------------------------------------------------------------------');
% calculate rs, thetas and rd, thetad
% to calculate rs and rd, we have to caculate coordinate of point E and
% point H (these point are shown in Fig. 1);
Xs = -50; Ys = -40;
Xd = -50; Yd = 0;
XE = r2*cos(theta2)+r33*cos(theta3+theta33);
YE = r2*sin(theta2)+r33*sin(theta3+theta33);
XH = r2*cos(theta2)+r33*cos(theta3+theta33)+ r55*cos(theta5+theta55);
YH = r2*cos(theta2)+r33*cos(theta3+theta33)+ r55*cos(theta5+theta55);
rs = sqrt((XE-Xs).^2+(YE-Ys).^2);
rd = sqrt((Xd-XH).^2+(Yd-YH).^2);
thetas = atan((YE-Ys)/(XE-Xs));
thetad = atan((YH-Yd)/(XH-Xd));
disp('');
fprintf('rs = %8.4f thetas = %8.4f rd = %8.4f thetad = %8.4f\n', rs,...
thetas, rd, thetad);
disp('
--------------------------------------------------------------------------------');
% calculate kinematic coefficients;

% calculate h3, h4;

h3 = sym('h3'); h4 = sym('h4'); h33 = sym('h33'); h44 = sym('h44');

f1 = r2*cos(theta2)+r3*cos(theta3)+r4*cos(theta4)-r1;
f11theta2 = -r2*sin(theta2)-r3*sin(theta3)*h3-r4*sin(theta4)*h4;

f2 = r2*sin(theta2)+r3*sin(theta3)+r4*sin(theta4)-r11;
f22theta2 = r2*cos(theta2)+r3*cos(theta3)*h3+r4*cos(theta4)*h4;

A = [-r3*sin(theta3) r4*sin(theta4); r3*cos(theta3) r4*cos(theta4)];

B = [r2*sin(theta2); r2*cos(theta2)];
C = inv(A)*B; D = C';
h3 = D(1); h4 = D(2);
fprintf('h3 = %8.4f h4 = %8.4f\n', h3, h4);
disp('
--------------------------------------------------------------------------------')
% calculate h5, h6;

h5 = sym('h5'); h6 = sym('h6'); h55 = sym('h55'); h66 = sym('h66');

g1 = r2*cos(theta2)+ r33*cos(theta3+theta33)+ r5*cos(theta5)...

+r6*cos(theta6)- r7;
g11theta2 = -r2*sin(theta2)-r33*sin(theta3+theta33)*h3...
-r5*sin(theta5)*h5-r6*sin(theta6)*h6;

g2 = r2*sin(theta2)+r33*cos(theta3+theta33)+ r5*sin(theta5)...
+r6*sin(theta6)-r77;
g22theta2 = r2*cos(theta2)+ r33*cos(theta3+theta33)*h3...
+r5*cos(theta5)*h5+r6*cos(theta6)*h6;

E = [-r5*sin(theta5) -r6*sin(theta6); r5*cos(theta5) r6*cos(theta6)];

F = [r2*sin(theta2)+r33*sin(theta3+theta33)*h3;...
-r2*cos(theta2)-r33*cos(theta3+theta33)*h3];
 37
G = inv(E)*F; H = G';
h5 = H(1); h6 = H(2);
fprintf('h5 = %8.4f h6 = %8.4f\n', h5, h6);
disp('
--------------------------------------------------------------------------------')
% calculate h33, h44;

f111theta2 = -r2*cos(theta2)-r3*cos(theta3)*h3*h3-r3*sin(theta3)*h33...
-r4*cos(theta4)*h4*h4-r4*sin(theta4)*h44;
f222theta2 = -r2*sin(theta2)-r3*sin(theta3)*h3*h3+r3*cos(theta3)*h33...
-r4*sin(theta4)*h4*h4+r4*cos(theta4)*h44;

I = [-r3*sin(theta3) -r4*sin(theta4); r3*cos(theta3) r4*cos(theta4)];

J = [ r2*cos(theta2)+r3*cos(theta3)*h3*h3+r4*cos(theta4)*h4*h4;...
+r2*sin(theta2)+r3*sin(theta3)*h3*h3+r4*sin(theta4)*h4*h4];
K = inv(I)*J; L = K';
h33 = L(1); h44 = L(2);
fprintf('h33 = %8.4f h44 = %8.4f\n', h33, h44);
disp('
--------------------------------------------------------------------------------')
% calculate h55, h66;

g111theta2 = -r2*cos(theta2)-r33*cos(theta3+theta33)*h3*h3...
-r33*sin(theta3+theta33)*h33-r5*cos(theta5)*h5*h5...
-r5*sin(theta5)*h55-r6*cos(theta6)*h6*h6...
-r6*sin(theta6)*h66;
g222theta2 = -r2*sin(theta2)-r33*sin(theta3+theta33)*h3*h3...
+r33*cos(theta3+theta33)*h33-r5*sin(theta5)*h5*h5...
+r5*cos(theta5)*h55-r6*sin(theta6)*h6*h6...
+r6*cos(theta6)*h66;

M = [-r5*sin(theta5) -r6*sin(theta6); r5*cos(theta5) r6*cos(theta6)];

N = [r2*cos(theta2)+r33*cos(theta3+theta33)*h3*h3+r33...
*sin(theta3+theta33)*h33+r5*cos(theta5)*h5*h5+r6*cos(theta6)*h6*h6;...
r2*sin(theta2)+r33*sin(theta3+theta33)*h3*h3-r33...
*cos(theta3+theta33)*h33+r5*sin(theta5)*h5*h5+r6*sin(theta6)*h6*h6];
O = inv(M)*N; P = O';
h55 = P(1); h66 = P(2);
fprintf('h55 = %8.4f h66 = %8.4f\n', h55, h66);
disp('
--------------------------------------------------------------------------------')
% calculate velocity;
omega3 = h3*omega2;
omega4 = h4*omega2;
omega5 = h5*omega2;
omega6 = h6*omega2;
disp(' omega2 omega3 omega4 omega5 omega6');
omega =[omega2; omega3; omega4; omega5; omega6];
Ome = omega';
disp(Ome);
disp('
--------------------------------------------------------------------------------')
% calculate acceleration;

aa3 = h33*omega2.*omega2;
aa4 = h44*omega2.*omega2;
aa5 = h55*omega2.*omega2;
aa6 = h66*omega2.*omega2;
disp(' aa2 aa3 aa4 aa5 aa6');
aa = [aa2; aa3; aa4; aa5; aa6];
aaa = aa';
disp(aaa);
 38
disp('
--------------------------------------------------------------------------------')
% calculate for mass center of each link;
% link 2;
Xg2 = l2*cos(theta2);
Xg22 = -l2*sin(theta2)*omega2;
Xg222 = -l2*cos(theta2)*omega2.*omega2;
Yg2 = l2*sin(theta2);
Yg22 = l2*cos(theta2)*omega2;
Yg222 = -l2*sin(theta2)*omega2.*omega2;
magvg2 = sqrt(Xg22.^2 + Yg22.^2);
magag2 = sqrt(Xg222.^2 + Yg222.^2);
Xg2 = [Xg2 Xg2 Xg2 Xg2 Xg2];
Yg2 = [Yg2 Yg2 Yg2 Yg2 Yg2];
disp(' Xg2 Yg2 Xg22 Yg22 magvg2 Xg222 Yg222
magag2');
G2 = [Xg2; Yg2; Xg22; Yg22; magvg2; Xg222; Yg222; magag2];
G22 = G2';
disp(G22);
disp('
--------------------------------------------------------------------------------');
% link 3;
Xg3 = r2*cos(theta2)+l3*cos(theta3+phi3);
Xg33 = -r2*sin(theta2)*omega2 - l3*sin(theta3+phi3)*h3*omega2;
Xg333 = -r2*cos(theta2)*omega2.*omega2 - l3*cos(theta3+phi3)*h3 ...
*h3*omega2.*omega2 - l3*sin(theta3+phi3)*h33*omega2;
Yg3 = r2*sin(theta2)+l3*sin(theta3+phi3);
Yg33 = r2*cos(theta2)*omega2 + l3*cos(theta3+phi3)*h3*omega2;
Yg333 = - r2*sin(theta2)*omega2.*omega2 - l3*sin(theta3+phi3)*h3*h3 ...
*omega2.*omega2 + l3*cos(theta3+phi3)*h33*omega2;
magvg3 = sqrt(Xg33.^2 + Yg33.^2);
magag3 = sqrt(Xg333.^2 + Yg333.^2);
Xg3 = [Xg3 Xg3 Xg3 Xg3 Xg3];
Yg3 = [Yg3 Yg3 Yg3 Yg3 Yg3];
disp(' Xg3 Yg3 Xg33 Yg33 magvg3 Xg333 Yg333
magag3');
G3 = [Xg3; Yg3; Xg33; Yg33; magvg3; Xg333; Yg333; magag3];
G33 = G3';
disp(G33);
disp('
--------------------------------------------------------------------------------');
% link 4;
Xg4 = r2*cos(theta2) + r3*cos(theta3) + l4*cos(theta4+phi4);
Xg44 = -r2*sin(theta2)*omega2 - r3*sin(theta3)*h3*omega2 ...
- l4*sin(theta4+phi4)*h4*omega2;
Xg444 = - r2*sin(theta2)*omega2.*omega2 - r3*cos(theta3)*h3 ...
*h3*omega2.*omega2 - r3*sin(theta3)*h33*omega2 ...
- l4*cos(theta4+phi4)*h4*h4*omega2.*omega2 ...
- l4*sin(theta4+phi4)*h44*omega2;
Yg4 = r2*sin(theta2) + r3*sin(theta3) + l4*sin(theta4+phi4);
Yg44 = r2*cos(theta2)*omega2 + r3*cos(theta3)*h3*omega2 ...
+ l4*cos(theta4+phi4)*h4*omega2;
Yg444 = - r2*sin(theta2)*omega2.*omega2 - r3*sin(theta3)*h3*h3 ...
*omega2.*omega2 + l3*cos(theta3)*h33*omega2 ...
- l4*sin(theta4+phi4)*h4*h4*omega2.*omega2 ...
+ l4*cos(theta4+phi4)*h44*omega2;
magvg4 = sqrt(Xg44.^2 + Yg44.^2);
magag4 = sqrt(Xg444.^2 + Yg444.^2);
Xg4 = [Xg4 Xg4 Xg4 Xg4 Xg4];
Yg4 = [Yg4 Yg4 Yg4 Yg4 Yg4];
disp(' Xg4 Yg4 Xg44 Yg44 magvg4 Xg444 Yg444
magag4');
G4 = [Xg4; Yg4; Xg44; Yg44; magvg4; Xg444; Yg444; magag4];
G44 = G4';
 39
disp(G44);
disp('
--------------------------------------------------------------------------------');
% link 5;
Xg5 = r2*cos(theta2) + r33*cos(theta3+theta33) + l5*cos(theta5+phi5);
Xg55 = -r2*sin(theta2)*omega2 - r33*sin(theta3+theta33)*h3*omega2 ...
- l5*sin(theta5+phi5)*h5*omega2;
Xg555 = - r2*sin(theta2)*omega2.*omega2 - r33*cos(theta3+theta33)*h3 ...
*h3*omega2.*omega2 - r33*sin(theta3+theta33)*h33*omega2 ...
- l5*cos(theta5+phi5)*h5*h5*omega2.*omega2 ...
- l5*sin(theta5+phi5)*h55*omega2;
Yg5 = r2*sin(theta2) + r33*sin(theta3+theta33) + l5*sin(theta5+phi5);
Yg55 = r2*cos(theta2)*omega2 + r33*cos(theta3+theta33)*h3*omega2 ...
+ l5*cos(theta5+phi5)*h5*omega2;
Yg555 = - r2*sin(theta2)*omega2.*omega2 - r33*sin(theta3+theta33)*h3*h3 ...
*omega2.*omega2 + l3*cos(theta3+theta33)*h33*omega2 ...
- l5*sin(theta5+phi5)*h5*h5*omega2.*omega2 ...
+ l5*cos(theta5+phi5)*h55*omega2;
magvg5 = sqrt(Xg55.^2 + Yg55.^2);
magag5 = sqrt(Xg555.^2 + Yg555.^2);
Xg5 = [Xg5 Xg5 Xg5 Xg5 Xg5];
Yg5 = [Yg5 Yg5 Yg5 Yg5 Yg5];
disp(' Xg5 Yg5 Xg55 Yg55 magvg5 Xg555 Yg555
magag5');
G5 = [Xg5; Yg5; Xg55; Yg55; magvg5; Xg555; Yg555; magag5];
G55 = G5';
disp(G55);
% The relation between omega2 and magvg5: magvg5 = abs(k*omega2),(k is a real
% number. Hence, magvg5 = magvg5max when omega2 = omega2max. And we have:
% magag5 = abs(k*omega2.^2),(k is a real number. Hence, magag5 = magag5max
% when omega2 = omega2max;
disp('
--------------------------------------------------------------------------------');
% link 6;
Xg6 = r2*cos(theta2) + r33*cos(theta3+theta33) + r5*cos(theta5) ...
+l6*cos(theta6+phi6);
Xg66 = -r2*sin(theta2)*omega2 - r33*sin(theta3+theta33)*h3*omega2 ...
- r5*sin(theta5)*h5*omega2 - l6*sin(theta6+phi6)*h6*omega2 ;
Xg666 = - r2*sin(theta2)*omega2.*omega2 - r33*cos(theta3+theta33)*h3 ...
*h3*omega2.*omega2 - r33*sin(theta3+theta33)*h33*omega2 ...
- r5*cos(theta5)*h5*h5*omega2.*omega2 ...
- r5*sin(theta5)*h55*omega2 - l6*cos(theta6+phi6)*h6*h6 ...
*omega2.*omega2 - l6*sin(theta6+phi6)*h66*omega2;
Yg6 = r2*sin(theta2) + r33*sin(theta3+theta33) + r5*sin(theta5) ...
+l6*sin(theta6+phi6);
Yg66 = r2*cos(theta2)*omega2 + r33*cos(theta3+theta33)*h3*omega2 ...
+ r5*cos(theta5)*h5*omega2 + +l6*cos(theta6+phi6);
Yg666 = -r2*sin(theta2)*omega2.*omega2 - r33*sin(theta3+theta33)*h3*h3 ...
*omega2.*omega2 + l3*cos(theta3+theta33)*h33*omega2 ...
-r5*sin(theta5)*h5*h5*omega2.*omega2+r5*cos(theta5)*h55*omega2 ...
-r6*sin(theta6+phi6)*h6*h6*omega2.*omega2+r6*cos(theta6+phi6) ...
*h66*omega2;
magvg6 = sqrt(Xg66.^2 + Yg66.^2);
magag6 = sqrt(Xg666.^2 + Yg666.^2);
Xg6 = [Xg6 Xg6 Xg6 Xg6 Xg6];
Yg6 = [Yg6 Yg6 Yg6 Yg6 Yg6];
disp(' Xg6 Yg6 Xg66 Yg66 magvg6 Xg666 Yg666
magag6');
G6 = [Xg6; Yg6; Xg66; Yg66; magvg6; Xg666; Yg666; magag6];
G66 = G6';
disp(G66);
disp('
--------------------------------------------------------------------------------');
theta = [theta2;theta3;theta4;theta5;theta6];
 40
table2 = [theta omega aa];
disp(' Table 2 the result with theta2 = 2*pi/3');
disp(' theta(i) omega(i)
aa(i)');
disp(table2);
end;