forked from andybarry/simflight
-
Notifications
You must be signed in to change notification settings - Fork 0
/
BuildTrajectoryLibraryOpenLoop.m
123 lines (78 loc) · 2.7 KB
/
BuildTrajectoryLibraryOpenLoop.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
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
clear
HudBotVisualizer.InitJava();
lib = TrajectoryLibrary();
% add stabilization trajectories
%parameters = {0.820; 2.499; 2.171; 0.697; 0.374; 0.028};
[parameters, gains] = GetDefaultGains();
FindTrimDrake
p = DeltawingPlant(parameters);
throttle_max = 5.33976;
tf = 0.5;
%Q = diag([10 10 10 1 1 1 .1 .1 .1 .1 .1 .1]);
%% straight ahead
t_mark = [0, tf];
elevL = [0.2, 0.2];
elevR = [0.2, 0.2];
throttle = [0.9 * throttle_max, 0.9 * throttle_max];
utraj = PPTrajectory(foh(t_mark, [elevL; elevR; throttle]));
utraj = utraj.setOutputFrame(p.getInputFrame());
xtraj = runInputTape(p, utraj, tf);
lib = AddLqrControllersToLib('straight', lib, xtraj, utraj, parameters, gains);
%% turn left
%
% t_mark = [0, tf];
% elevL = [0.6, 0.6];
% elevR = [-0.2, -0.2];
% throttle = [0.9 * throttle_max, 0.9 * throttle_max];
%
% utraj = PPTrajectory(foh(t_mark, [elevL; elevR; throttle]));
% utraj = utraj.setOutputFrame(p.getInputFrame());
%
% xtraj = runInputTape(p, utraj, tf);
%
%
% disp('Computing TVLQR controller...');
% lqr_controller = tvlqr(p, xtraj, utraj, Q, R, Qf)
% disp('done.');
%
% comments = sprintf('%s\n\n%s', 'Turn left', [prettymat('Parameters', cell2mat(parameters), 3) ...
% prettymat('Q', Q, 5) prettymat('R', R)]);
% lib = lib.AddTrajectory(xtraj, utraj, lqr_controller, comments);
%% turn right
t_mark = [0, .3];
elevL = [-0.1, -0.1];
elevR = [0.2, 0.2];
throttle = [0.9 * throttle_max, 0.9 * throttle_max];
utraj = PPTrajectory(foh(t_mark, [elevL; elevR; throttle]));
utraj = utraj.setOutputFrame(p.getInputFrame());
xtraj = runInputTape(p, utraj, 0.3);
lib = AddLqrControllersToLib('turn-right', lib, xtraj, utraj, parameters, gains);
%% go up
% first pull up, then fly straight
t_mark = [0, .2, .3, 1.5];
elevs = [0.2, 0.2, 0, 0];
throttle = [throttle_max, throttle_max, throttle_max, throttle_max];
utraj = PPTrajectory(foh(t_mark, [elevs; elevs; throttle]));
utraj = utraj.setOutputFrame(p.getInputFrame());
[xtraj, sys] = runInputTape(p, utraj, tf);
lib = AddLqrControllersToLib('climb', lib, xtraj, utraj, parameters, gains);
%% go down
% t_mark = [0, .1, .2, .5];
% elevs = [-0.6, -0.6, 0, 0];
% throttle = [throttle_max, throttle_max, throttle_max, throttle_max];
%
% utraj = PPTrajectory(foh(t_mark, [elevs; elevs; throttle]));
%
% utraj = utraj.setOutputFrame(p.getInputFrame());
%
% [xtraj, sys] = runInputTape(p, utraj, tf);
%
% disp('Computing TVLQR controller...');
% lqr_controller = tvlqr(p, xtraj, utraj, Q, R, Qf)
% disp('done.');
%
% comments = sprintf('%s\n\n%s', 'Dive', [prettymat('Parameters', cell2mat(parameters), 3) ...
% prettymat('Q', Q, 5) prettymat('R', R)]);
% lib = lib.AddTrajectory(xtraj, utraj, lqr_controller, comments);
%% draw
lib.DrawTrajectories();