-
Notifications
You must be signed in to change notification settings - Fork 5
/
ex10_timeOpt_backandforth.m
97 lines (83 loc) · 2.71 KB
/
ex10_timeOpt_backandforth.m
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
clear; close all;
% constraints
pmax = 1; % final position
vmax = 0.5; % max velocity
amax = 0.75; % max acceleration
% jmax = 4; % max jerk
tfin_init = 5; % final time
d = 5; % spline degree, minimum 3
n = 32; % number of knots
tfin_feasible = tfin_init;
tfin_infeasible = 0;
tfin_diff_stop = 0.01;
itr = 1;
maxitr = 10;
% tfin_opt minimization by bisection method
while (abs(tfin_feasible-tfin_infeasible)) > tfin_diff_stop && itr <= maxitr
tfin = (tfin_feasible+tfin_infeasible)/2;
% B-spline basis of degree d with n knots
basis = BSplineBasis([0, tfin], d, n);
% scalar (1x1) spline variable
pos = BSpline.sdpvar(basis, [1, 1]); % position trajectory
vel = pos.derivative(1); % velocity trajectory
acc = pos.derivative(2); % acceleration trajectory
jrk = pos.derivative(3); % jerk trajectory
% equality constraints
con = [pos.f(0) == 0, pos.f(tfin) == pmax, ...
vel.f(0) == 0, vel.f(tfin) == 0, ...
acc.f(0) == 0, acc.f(tfin) == 0];
% jrk.f(0) == 0, jrk.f(tfin) == 0];
% inequality constraints
con = [con, -vmax <= vel, vel <= vmax, ...
-amax <= acc, acc <= amax];
if exist('jmax','var'), con = [con, -jmax <= jrk, jrk <= jmax]; end
% Solve convex semi-Definite program (SDP)
options = sdpsettings('verbose',0);
sol = optimize(con, [], options);
if sol.problem == 0 % feasible
tfin_feasible = tfin;
fprintf('iteration %d (feasible): tfin_feasible=%.2f [s], tfin_infeasible =%.2f [s]\n',itr,tfin_feasible,tfin_infeasible);
t = 0:tfin_feasible/1000:tfin_feasible;
y1 = value(pos); y1 = y1.f(t);
y2 = value(vel); y2 = y2.f(t);
y3 = value(acc); y3 = y3.f(t);
y4 = value(jrk); y4 = y4.f(t);
else
tfin_infeasible = tfin;
fprintf('iteration %d (infeasible): tfin_feasible=%.2f [s], tfin_infeasible =%.2f [s]\n',itr,tfin_feasible,tfin_infeasible);
end
itr = itr + 1;
end
%% Plot
hfig = figure;
subplot(2,2,1);
plot(t,y1,'b-'); hold on
xlabel('time [s]');
title('position');
subplot(2,2,2);
plot(t,y2,'b-'); hold on
plot([t(1),t(end)],[vmax,vmax],'r--');
plot([t(1),t(end)],[-vmax,-vmax],'r--');
ylim([0,0.6]);
xlabel('time [s]');
title('velocity');
subplot(2,2,3);
plot(t,y3,'b-'); hold on
plot([t(1),t(end)],[amax,amax],'r--');
plot([t(1),t(end)],[-amax,-amax],'r--');
xlabel('time [s]');
title('acceleration');
subplot(2,2,4);
plot(t,y4,'b-'); hold on
if exist('jmax','var')
plot([t(1),t(end)],[jmax,jmax],'r--');
plot([t(1),t(end)],[-jmax,-jmax],'r--');
end
xlabel('time [s]');
title('jerk');
% FigTools required
% https://github.com/ThomasBeauduin/FigTools
if exist('pubfig','file')
pfig = pubfig(hfig);
expfig('ex10','-png');
end