(a)

clc;
clear all
close all
%% Similation settings
I = eye(2); % Identity matrix
x_hat = [1;0];
% Unit vector in x-direction
g = [0; -9.81];
% Gravity vector
z = [0;0];
% Zero vector
S = [0 1;-1 0];
% Cross product conversion matrix
% Lengths [m]
l2 = 0.5;
l2c = 0.5*l2;
l3 = 0.5;
l3c = 0.5*l2;
% Segment vectors in a neutral posture
r2c = l2c*x_hat;
r2e = l2*x_hat;
r3c = l3c*x_hat;
r3e = l3*x_hat;
% Masses and moments of inertias
m1 = 3;
m2 = 3;
m3 = 1;
I2 = 1/12*m2*l2^2;
I3 = 1/12*m3*l2^2;

% If any specific value is available, use it.

% Simulation setting
T = 0.001; % Sampling period
ET = 10;
% Total simulation time
t = [0:T:ET]; % Time vector
L = length(t); % Number of samples to be simulated
x = zeros(8,L);
% Unkowns to be identified
rT = zeros(2,L);
% Location of the tiptoe
% Initial values
% x(:,1) = [0; 1; -pi/2; -pi/2; 0; 0; 0; 0];
x(:,1) = [0; 1; 0; 0; 0; 0; 0; 0];
x(:,2) = x(:,1);
% Control variable setting
kG = 100000;
dGx = 1000;
dGy = 1000;
r1y0 = 1;
kS = 100000;
dS = 1000;
kP = 200; % P control gain
kD = 10; % D control gain
theta2 = 0;
theta3 = 0;
dtheta2 = 0;
dtheta3 = 0;
thetaD2 = zeros(1,L);

thetaD3 = zeros(1,L);
%% Simulation
for k=2:L-1,
k
theta2 = x(3,k);
theta3 = x(4,k);
% Calculation of derivatives
dtheta2 = (x(3,k) - x(3,k-1))/T;
dtheta3 = (x(4,k) - x(4,k-1))/T;
% Location of the tiptoe
rT(:,k) = x(1:2,k) + R(theta2)*r2e + R(theta3)*r3e;
% Desired motions
thetaD2(k) = 1.2*sin(pi*t(k));
thetaD3(k) = thetaD2(k) + max(0, (0.5-pi/0.6*cos(pi*t(k)-0.3*pi)));
tauA1(k) = kP*(thetaD2(k) - theta2) + kD*((thetaD2(k)-thetaD2(k-1))/2 - dtheta2);
tauA2(k) = kP*(thetaD3(k) - theta3) + kD*((thetaD3(k)-thetaD3(k-1))/2 - dtheta3);
% Ground reaction force
if rT(2,k) < 0;
fG = [-dGx*(rT(1,k)-rT(1,k-1))/T;
-kG*rT(2,k)-dGy*(rT(2,k)-rT(2,k-1))/T];
else
fG = [0;
0];
end
% Fictitious guide
if x(2,k) < r1y0,
fs1 = [0;
-kS*(x(2,k)-r1y0)-dS*(x(2,k)-x(2,k-1))/T];
else
fs1 = [0;
0];
End
% tau = 50*(0.5 - t0(k)) + 50*(-dt0); %PD Control
A = [m1*I z
z;
m2*I m2*dR(theta2)*r2c z;
m3*I m3*dR(theta2)*r2e m3*dR(theta3)*r3c;
z'
I2
0;
z'
0
I3];
B = [-I 0*I;
I
-I;
0*I
I;
-r2c'*R(theta2)'*S
-(r2e-r2c)'*R(theta2)'*S;
z'
-r3c'*R(theta3)'*S;];
C = [-m1*g-fs1;
m2*ddR(theta2)*dtheta2^2*r2c-m2*g;
m3*ddR(theta2)*dtheta2^2*r2e+m3*ddR(theta3)*dtheta3^2*r3c-m3*g-fG;
-tauA1(k) + tauA2(k);
-(r3e - r3c)'*R(theta3)'*S*fG - tauA2(k)];

x(:,k+1) = [A/T^2 B]^-1*(-C - [A/T^2 B*0]*(-2*x(:,k) + x(:,k-1)));

end
figure;
tauA11 = [tauA1 0];
tauA22 = [tauA2 0];
plot(tauA11);
title('Required actuation torque1 by time ');xlabel('Time [seconds]'); ylabel('tauA1');
figure;
plot(tauA22);
title('Required actuation torque2 by time ');xlabel('Time [seconds]'); ylabel('tauA1');
figure;
plot(thetaD2);
title('Required actuation torque1 by time ');xlabel('time'); ylabel('theta Link 1');
figure;
plot(thetaD3);
title('Required actuation torque2 by time ');xlabel('time'); ylabel('theta Link 2');
figure;
plot(t,thetaD2.*tauA11);
title('Required power by time ');xlabel('Time [seconds]'); ylabel('power');
figure;
plot(t,thetaD3.*tauA22);
title('Required power by time ');xlabel('Time [seconds]'); ylabel('power');
figure;
plot(thetaD2,tauA11);
title('Required power ');xlabel(' theta Link 1'); ylabel('tau Link 1');
figure;
plot(thetaD3,tauA22);
title('Required power ');xlabel('thetaLink 2'); ylabel('tau Link 2');
%Visualize3D;
figure;
plot(t,x(3,:));
title('Required actuation torque1 by time ');xlabel('time'); ylabel('theta Link 1');
figure;
plot(t,x(4,:));
title('Required actuation torque2 by time ');xlabel('time'); ylabel('theta Link 2');

Required power by time

25
20

X: 2.82
Y: 21.41

15

power

10
5
0
-5
-10
-15
-20

4
5
6
Time/T [seconds]

10

(b)
Link1 : maximum torque : 51.87 , maximum thete = 1.19
Link2 : maximum torque : 16.38 , maximum thete = 4.786
(c)

Required power by time

20

10

power

-10

-20
X: 3.105
Y: -32.98

-30

-40

20

4
5
6
Required power by time
Time/T [seconds]

10

10

power

-10

-20
X: 3.105
Y: -32.98

-30

-40

4
5
6
Time/T [seconds]

10

link1 : required power = 60W

Required power

20

15

tau Link 2

10

-5

-10
-1

2
thetaLink 2

link2 : required power = 40W

(d)
Link 1 : RE 30 30 mm, Graphite Brushes, 60 Watt part number 268216
Nominal speed 7750 rpm
Nominal torque (max. continuous torque) 88.2 mNm
Max efficiency 88%
Weight 260g
Gear ratio > 51.81/0.0882 = 587.415
Spindle Drive GP 32 S 32 mm, Ball Srew, 10 x 2 Gear ratio :762
7750/587.4 = 13.1937 1.19*9.6 RPM
Gear max efficiency 56%
(e)
Gear & motor efficiency = 88%*56%=49%
30W power .

(f)
50%
2 power .
RE 65 65 mm, Graphite Brushes, 250 Watt
Part number 353299
Nominal speed

3130 rpm

Nominal torque (max. continuous torque)

Weight

796 mNm

2100 g

Max. efficiency

89 %

gear ratio :51.82 / 0.796 = 65.1005

gear
Planetary Gearhead GP 62 A 62 mm, 8 - 50 Nm
Part number 110505 , gear ratio :100
Weight

1500 g

Max. efficiency

70 %

66% .
Required power

70
60
50

tau Link 1

40
30
20
10
0
-10
-1.5

-1

-0.5

0
theta Link 1

0.5

1.5

70w power 250w*^^% = 166W

(g)
.
RE 65 65 mm, Graphite Brushes, 250 Watt
Part number 353299
Nominal speed

3130 rpm

Nominal torque (max. continuous torque)

Weight

2100 g

Max. efficiency

89 %

796 mNm

gear ratio :16.36 / 0.796 = 20.5528

gear
Planetary Gearhead GP 62 A 62 mm, 8 - 50 Nm
Part number 110503 , gear ratio :35
Weight

1200 g

Max. efficiency

75 %
Required power

50
40
30

tau Link 2

20
10
0
-10
-20
-30
-1

2
thetaLink 2

50w power .
250*0.89*0.7 = 155W power .