-
Notifications
You must be signed in to change notification settings - Fork 5
/
ex8_jerkOpt_minInfNorm_backandforth.m
85 lines (73 loc) · 2.19 KB
/
ex8_jerkOpt_minInfNorm_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
clear; close all;
% constraints
pmax = 1; % final position
vmax = 0.5; % max velocity
amax = 0.75; % max acceleration
% jmax = 4; % max jerk
flag_jrk0 = false;
tfin = 3.5; % final time
d = 3; % spline degree, minimum 3
n = 8; % number of knots, typical 8
% 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];
if flag_jrk0, con = [con, jrk.f(0) == 0, jrk.f(tfin) == 0]; end
% inequality constraints
con = [con, -vmax <= vel, vel <= vmax, ...
-amax <= acc, acc <= amax];
if exist('jmax','var'), con = [con, -jmax <= jrk, jrk <= jmax]; end
% set objective function: minimal deflection
n_sample = 200; % number of sampled point
t_sample = linspace(0,tfin,n_sample);
obj = norm(jrk.f(t_sample),inf);
% Solve convex semi-Definite program (SDP)
options = sdpsettings('verbose',1);
sol = optimize(con, obj, options);
if sol.problem == 1, error('infeasible! increase d or n, otherwise relax tfin!'); end
%% Plot
Ts = 1e-3;
t = 0:Ts:tfin;
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);
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('ex8','-png');
end