-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmehdi_script.asv
131 lines (104 loc) · 3.13 KB
/
mehdi_script.asv
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
clc;close all
N = 500; Ts = 2*pi/N;
t = linspace(-pi, pi, N);
x = 8*(sin(t)).^3;
y = 8*(sin(2*t)).^3;
vx = gradient(x, Ts);
vy = gradient(y, 2*pi/N);
ax = gradient(vx, 2*pi/N);
ay = gradient(vy, 2*pi/N);
%orientation
phi = atan2(vy, vx);
%robot velocities
v = vx.*cos(phi) + vy.*sin(phi);
omega = (vx.*ay - vy.*ax)./(vx.^2 + vy.^2);
%Plotting
figure()
subplot(2, 1, 1)
plot(t,v, 'linewidth', 4)
xlabel('time', 'FontSize', 14)
ylabel('velocity', 'FontSize', 14)
title('Linear velocity', 'FontSize', 18)
xlim([-pi pi])
subplot(2, 1, 2)
plot(t,omega, 'linewidth', 4)
title('Angular velocity', 'FontSize', 18)
xlabel('time', 'FontSize', 14)
ylabel('velocity', 'FontSize', 14)
xlim([-pi pi])
print -deps figures/task1_velocities
h = figure;
axis tight manual % this ensures that getframe() returns a consistent size
filename = 'figures/task1_trajectory.gif';
plot(x, y, 'b', 'linewidth', 3)
hold on
for i=1:10:N
plot(x(1:i), y(1:i), 'g-', 'linewidth', 6)
legend('Given Trajectory', "Robot's Path");
drawnow;
%create GIF
frame = getframe(h);
im = frame2im(frame);
[imind,cm] = rgb2ind(im,256);
% Write to the GIF File
if i == 1
imwrite(imind,cm,filename,'gif', 'Loopcount',inf);
else
imwrite(imind,cm,filename,'gif','WriteMode','append');
end
end
hold off
%task 2
W = 1/2; r = 1/4; T=0.1;
%initialize Inverse kinematics velocities
omega = zeros(1, N); v = zeros(1, N);
vL = zeros(1, N); vR = zeros(1, N);
omegaL = zeros(1, N); omegaR = zeros(1, N);
% %initialize resulting forward kinematic variables:
% x_f = zeros(1, N); y_f = zeros(1, N); phi_f = zeros(1, N);
for n = 2:N-1
%calculating inverse kinematics variables:
mu = 1/2*(sin(phi(n))*(y(n+1)-y(n))+cos(phi(n)*(x(n+1)-x(n))))...
/(cos(phi(n))*(y(n+1)-y(n))-sin(phi(n))*(x(n+1)-x(n)));
x_m = (x(n)+x(n+1))/2;
y_m = (y(n)+y(n+1))/2;
x_star = x_m - mu/2 * (y(n+1) - y(n));
y_star = y_m + mu/2 * (x(n+1)-x(n));
R_n = sqrt((x(n) - x_star)^2 + (y(n)-y_star)^2);
theta_1 = atan2((y(n)-y_star), (x(n)-x_star));
theta_2 = atan2((y(n+1)-y_star), (x(n+1)-x_star));
del_phi = wrapToPi(theta_1 - theta_2);
%resulting Inv-Kinematics velocities:
omega(n) = del_phi/T;
v(n) = R_n*abs(omega(n));
vL(n) = (R_n-1/2 *W)*omega(n);
vR(n) = (R_n+1/2 *W)*omega(n);
omegaL(n) = vL(n)/r;
omegaR(n) = vR(n)/r;
end
figure()
subplot 221
plot(t,vL,'linewidth', 2)
xlabel('time', 'FontSize', 10)
ylabel('velocity', 'FontSize', 10)
title('Left Wheel velocity', 'FontSize', 14)
xlim([-pi pi])
subplot 222
plot(t,vR, 'linewidth', 2)
xlabel('time', 'FontSize', 10)
ylabel('velocity', 'FontSize', 10)
title('Right Wheel velocity', 'FontSize', 14)
xlim([-pi pi])
subplot 223
plot(t,omegaL, 'linewidth', 2)
xlabel('time', 'FontSize', 10)
ylabel('velocity', 'FontSize', 10)
xlim([-pi pi])
title('Left Wheel angular velocity', 'FontSize', 14)
subplot 224
plot(t,omegaR, 'linewidth', 2)
xlabel('time', 'FontSize', 10)
ylabel('velocity', 'FontSize', 10)
title('Right Wheel angular velocity', 'FontSize', 14)
xlim([-pi pi])
print -deps figures/task2