Team Members:

Bilqist Imeilia Az Zahra (2019360019)

Dara Ginanti (2019360025)
M. Alfin Prayogo (2017360017)
 ▪ Vector Loop

Vector Loop:

𝑹𝑨𝑶 − 𝑹𝑨𝑸 − 𝑹𝑸𝑶 = 0

𝒖𝒚 = [0; 1]

Position Constraint

𝐿𝐴𝑂 𝒖𝟏 − 𝐿𝐴𝑄 𝒖𝟑 − 𝑎(−𝒖𝒚 ) = 0

𝐿𝐴𝑂 𝒖𝟏 = 𝐿𝐴𝑂 𝑐𝑜𝑠𝜃1 + 𝐿𝐴𝑂 𝑠𝑖𝑛𝜃1

𝐿𝐴𝑄 𝒖𝟑 = 𝐿𝐴𝑄 𝑐𝑜𝑠𝜃3 + 𝐿𝐴𝑄 𝑠𝑖𝑛𝜃3 = −𝑎
𝑐𝑜𝑠𝜃1 𝑐𝑜𝑠𝜃1 0
𝐿𝐴𝑂 [ ] − 𝐿𝐴𝑄 [ ]−𝑎[ ] = 0
𝑠𝑖𝑛𝜃1 𝑠𝑖𝑛𝜃1 −1
Figure 1. Vector loop of the system

𝐿𝐴𝑂 𝑐𝑜𝑠𝜃1 − 𝐿𝐴𝑄 𝑐𝑜𝑠𝜃3 = 0

𝐿𝐴𝑂 𝑠𝑖𝑛𝜃1 − 𝐿𝐴𝑄 𝑠𝑖𝑛𝜃3 + 𝑎 = 0

Velocity Constraint

−𝐿𝐴𝑂 𝜃̇1 𝑠𝑖𝑛𝜃1 − 𝐿̇𝐴𝑄 𝑐𝑜𝑠𝜃3 + 𝐿𝐴𝑄 𝜃̇3 𝑠𝑖𝑛𝜃3 = 0

𝐿𝐴𝑂 𝜃̇1 𝑐𝑜𝑠𝜃1 − 𝐿̇𝐴𝑄 𝑠𝑖𝑛𝜃3 − 𝐿𝐴𝑄 𝜃̇3 𝑐𝑜𝑠𝜃3 = 0

Acceleration Constraint

−𝐿𝐴𝑂 (𝜃̈1 𝑠𝑖𝑛𝜃1 + 𝜃̇12 𝑐𝑜𝑠𝜃1 ) − 𝐿̈𝐴𝑄 𝑐𝑜𝑠𝜃3 + 𝐿̇𝐴𝑄 𝜃3̇ 𝑠𝑖𝑛𝜃3 + 𝐿̇𝐴𝑄 𝜃̇3 𝑠𝑖𝑛𝜃3 + 𝐿𝐴𝑄 𝜃̈3 𝑠𝑖𝑛𝜃3 + 𝐿𝐴𝑄 𝜃̇32 𝑐𝑜𝑠𝜃 = 0

𝐿𝐴𝑂 (𝜃̈1 𝑐𝑜𝑠𝜃1 − 𝜃̇12 𝑠𝑖𝑛𝜃1 ) − 𝐿̈𝐴𝑄 𝑠𝑖𝑛𝜃3 − 𝐿̇𝐴𝑄 𝜃̇3 𝑐𝑜𝑠𝜃3 − 𝐿̇𝐴𝑄 𝜃̇3 𝑐𝑜𝑠𝜃3 + 𝐿𝐴𝑄 𝜃̈3 𝑐𝑜𝑠𝜃3 + 𝐿𝐴𝑄 𝜃̇32 𝑠𝑖𝑛𝜃 = 0
 ▪ Free-Body Diagram and Equation of Motion

Body 1 Body 2 Body 3

99

Figure 2. Free-body diagram of the system

𝒖𝟏 = [𝑐𝑜𝑠𝜃1 ; 𝑠𝑖𝑛𝜃1 ]
𝒖𝟑 = [𝑐𝑜𝑠𝜃3 ; 𝑠𝑖𝑛𝜃3 ]
Body (1)
1
𝒔𝑶
𝟏 = − 𝐿1 𝒖𝟏
2
1
𝒔𝑨𝟏 = 𝐿1 𝒖𝟏
2
𝑚1 0 0 𝑥̈ 1 0 𝜆1 − 𝜆3
[0 𝑚1 𝑦̈
0 ] [ 1 ] = [−𝑤1 ] + [ 𝜆 2 − 𝜆4 ]
0 0 𝐽1 𝜙1̈ 𝑇1 ̆𝑶 ̆𝑨
𝒔𝟏 ′𝜆1,2 − 𝒔𝟏 ′𝜆2,4

Body (2)

𝑚2 0 0 𝑥̈ 2 0 𝜆3 − 𝜆5 𝑠𝑖𝑛𝜃3
[0 𝑚2 0 ] [ 𝑦̈ 2 ] = [−𝑤2 ] + [𝜆4 + 𝜆5 𝑐𝑜𝑠𝜃3 ]
0 0 𝐽2 𝜙2̈ 0 −𝜆6

Body (3)

𝑸 1
𝒔𝟑 = − 𝐿3 𝒖𝟑
2
1
𝒔𝑩
𝟏 = 𝐿3 𝒖𝟑
2
 𝑚3 0 0 𝑥3̈ 𝐹𝑠𝑥 𝜆7 + 𝜆5 𝑠𝑖𝑛𝜃3
[0 𝑚3 −𝑤
0 ] [ 𝑦̈ 3 ] = [ 3 + 𝐹𝑠𝑦 ] + [ 𝜆 8 − 𝜆5 𝑐𝑜𝑠𝜃3 ]
′
0 0 𝐽3 𝜙3̈ ̆𝑩
𝒔𝟑 ′𝑓𝑠 ̆
𝑶 𝐺 𝐺
𝒔𝟑 𝜆7,8 + 𝜆6 + 𝜆5 (𝑟2 − 𝑟3 )′

▪ Spring Force

Figure 1. Spring mechanism of the system

𝒅 = 𝒓𝑩 − 𝒓𝑹
𝒅
𝒖=
𝐿
𝐿2 = 𝒅′ 𝒅
𝐹𝑠 = 𝑘(𝐿 − 𝐿𝑜 )

• Acceleration of Mass Center

𝐿1
𝑟𝐺1 = 𝑎𝑢𝑦 + 𝑢
2 1
𝑟𝐺2 = 𝑎𝑢𝑦 + 𝐿1 𝑢1
𝐿3
𝑟𝐺3 = 𝑢
2 3

𝑟𝐺1̈ = 𝒓̆𝑮𝟏 𝜃̈1 − 𝑟1𝐺 𝜃̇1

𝑟𝐺1̈ = 𝒓̆𝑮𝟐 𝜃̈1 − 𝑟2𝐺 𝜃̇1

𝑟𝐺1̈ = 𝒓̆𝑮𝟑 𝜃̈3 − 𝑟3𝐺 𝜃̇3

• Array of Reaction Force

I −I 0 0 0
̆
𝑶 ̆𝑨
𝜆1,2
𝒔𝟏 ′ − 𝒔𝟏 ′ 0 0 0
𝜆3,4
0 𝐼 𝑢
̆3 0 0
𝜆5 = 𝐷 ′ 𝜆
0 0 0 −1 0
𝜆6
0 0 −𝑢̆3 0 𝐼
[ 𝜆 ]
[0 0 (𝑟2𝐺 − 𝑟3𝐺 )′ 0 0] 7,8
clear
clc
L1 = 0.2; QB = 0.8; a = 0.4; b = 0.4;
c = 0.5; m1 = 0.1; m2 = 0.2; m3 = 0.3;
J1 = 0.01; J2 = 0.05; J3 = 0.02;
k = 1000; Lo = 0.3;
theta1d = 2*pi;
inc = 5*pi/180;
na = 73;

for n = 1:na;

%position
theta1(n) = (n-1)*inc;
Lcos(n) = L1*cos(theta1(n));
Lsin(n) = L1*sin(theta1(n)) + a;
theta3(n) = atan(Lsin(n)/Lcos(n));
L3(n) = Lcos(n)/cos(theta3(n));

%velocity
u1(n,:) = [cos(theta1(n));sin(theta1(n))];
u3(n,:) = [cos(theta3(n));sin(theta3(n))];
u1r(n,:) = [-u1(n,2);u1(n,1)];
u3r(n,:) = [-u3(n,2);u3(n,1)];

C = [u3(n,:)' L3(n)*u3r(n,:)'];
rhsv = L1*u1r(n,:)'*theta1d;
solVa = C\rhsv;
L3d(n) = solVa(1);
theta3d(n) = solVa(2);

%acceleration
theta1dd = 0;
rhsa = -L1*u1r(n,:)'*theta1dd + L1*u1(n,:)'*theta1d^2 ...
- L3d(n)*u3r(n,:)'*theta3d(n) -
L3d(n)*u3r(n,:)'*theta3d(n) ...
- L3(n)*u3(n,:)'*theta3d(n)^2;
solA = C\rhsa;

L3dd(n) = solA(1);
theta3dd(n) = solA(2);

%spring
rR = [c; b+a];
xB(n) = cos(theta3(n))*QB;
yB(n) = sin(theta3(n))*QB;
rB = [xB(n);yB(n)];
d = [rB - rR];
L = norm(d); u = d/L;
fs = k*(L - Lo);
fsB = -fs*u;
fsR = fs*u;

% EoM
g = 9.81; uy = [0; 1];
w1 = -g*m1*uy; w2 = -g*m2*uy; w3 = -g*m3*uy;
h = [w1; 0;w2; 0; ; (w3 + fsB) ;0];

s1O = -L1/2*u1(n,:)';
s1A = L1/2*u1(n,:)';
s3Q = -L3(n)/2*u3(n,:)';
s3B = L3(n)/2*u3(n,:)';

s1Or = [-s1O(2);s1O(1)];
s1Ar = [-s1A(2);s1A(1)];
s3Qr = [-s3Q(2);s3Q(1)];
s3Br = [-s3B(2);s3B(1)];

rG1 = a*uy + L1/2*u1(n,:)';

rG2 = a*uy + L1*u1(n,:)';
rG3 = L3(n)/2*u3(n,:)';

rG1r = [-rG1(2);rG1(1)];
rG2r = [-rG2(2);rG2(1)];
rG3r = [-rG3(2);rG3(1)];

rG1dd = rG1r*theta1dd - rG1*theta1d;

rG2dd = rG3r*theta1dd - rG2*theta1d;
rG3dd = rG3r*theta3dd(n) - rG3*theta3d(n);

M_diag = [m1 m1 J1 m2 m2 J2 m3 m3 J3]'; M = diag(M_diag);

cdd = [rG1dd; theta1dd; rG2dd; theta3dd(n); rG3dd;
theta3dd(n)];
Mcdd = M_diag.*cdd;

% T3 T1
I2 = eye(2); Z2 = zeros(2);
Z12 = zeros(1,2); Z21 = zeros(2,1);
Dt = [I2 -I2 Z2 Z21 Z21
s1Or' -s1Ar' Z12 0 0
Z2 I2 -I2 Z21 Z21
Z12 s3Qr' -s3Br' 0 0
Z2 Z2 I2 uy Z21
Z12 Z12 Z12 0 1];

column9 = zeros(9,1);
column9(3) = 1.0;
Dt9 = [Dt column9];

sol = Dt9\(Mcdd - h);

T(n) = sol(9)

end

figure(1)
plot(theta1,theta3d)
xlabel('Crank angle (rad)');
ylabel('Angular Velocity (rad/s)');
hold on
figure(2)
plot(theta1,theta3dd)
xlabel('Crank angle (rad)');
ylabel('Torque (Nm)');
From the figure above, we can see that the highest value of the angular velocity is around 6.5 rad/s at
1.5 rad (crank angle). The lowest value is around –9.8 rad/s at 4.8 rad (crank angle). The result shows
a fluctuate relationship. As we can see from the figure, the relation shows a fast and direct change from
peak to the lowest point in a certain small change of crank angle. The link will reach its maximum point
and start to decrease until its lowest point, then, it will increase again. The system will repeat the
same pattern again and again.
From the figure, we can see that the relationship between torque and crank angle is fluctuative. The
highest peak torque is on around 24 Nm and the lowest is at around -5. The torque will continue to
increase until around 3.5 radian and it will decrease until around 4.4 radian. The torque will the increase
again with a lower peak than the previous one, once it reaches its peak, it will start to decrease again until
its lowest point. The pattern will be repeated. The bigger the crank angle does not mean the bigger the
torque will be, in a certain point it will be decreasing and start to increase again.