AE4803Quad/run_quad.m at main · David-Rey/AE4803Quad · GitHub

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
129
130
131
132
133
134
135
136
close all;
clear;
clc;

%% Dynamics Model

mass = 1;
inertia = 2.5e-4;
radius = 1;
gravity = 9.8;
dt = 0.05;
tf = 100;

ic = [0; 0; 0; 0; 0; 0];
goal = [4; 2; 0; 0; 0; 0];

dyn = planar_quadrotor(mass, inertia, radius, gravity, dt);

%% Tunable Parameters

% Tune here!

Q = eye(6);
R = eye(2);
Qf = 500*Q;

regularizer = 20;  % 0 has no regularizing
initial_controls = zeros(tf, 2);
mode = 'ilqr';  % ilqr, ddp


%% Enable / Disable Animation

animate = true;

%%

[costfn, term_costfn] = quad_cost(Q, R, Qf, goal);

iters = 10;
initial_controls = (mass * gravity / 2) * ones(tf, 2);

[controller, total_costs] = ddp(ic, initial_controls, iters, regularizer, dyn, costfn, term_costfn, mode);


%%

num_quads = 15;

rng(1);
cov = diag([0.01, 0.01, 0.01, 0.01, 0.01, 0.01]);
sampled_ics = mvnrnd(ic, cov, num_quads);

trajs = {};

for i = 1:num_quads
    [states, ~, ~] = fwd_pass(sampled_ics(i, :)', controller, dyn, costfn, term_costfn);
    trajs{i} = states;
end

[nom_states, nom_controls, costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn);

disp(['Nominal Cost: ' num2str(sum(costs))]);


%%

figure(Position=[10 10 1200 600])

layout = tiledlayout(2, 5);
nexttile(layout, [2, 3]);
hold on;

lines = {};
bodies = {};
wings = {};

for i = 1:num_quads
    lines{i} = plot([0 0], [0 0], LineWidth=1.5, Color=[1 0 0 0.25]);
    body{i} = scatter(0, 0, 'filled', 'o', MarkerFaceColor='k');
    wings{i} = plot([0 0], [0 0], 'k', LineWidth=2);
end

plot(states(:, 1), states(:, 2), '--', LineWidth=2, Color='k');
%scatter(goal(1), goal(2), 1000, 'k', 'o', LineWidth=3);
viscircles(goal(1:2)', 0.5, Color='b');

xlim([-5, 4.5])
ylim([-5, 3.5])
axis equal;

title('Planar Quadrotor Trajectories', FontSize=16);

nexttile(layout, [1 2]);

hold on;
max_nom_controls = max(nom_controls(:, 1), nom_controls(:, 2));
plot(max_nom_controls, LineWidth=2);
title('Largest Control Value', FontSize=16);

nexttile(layout, [1 2]);

hold on;
plot(total_costs, LineWidth=2);
title('Objective Cost', FontSize=16);

if animate
    for t = 1:tf
        for i = 1:num_quads
            lines{i}.XData = trajs{i}(1:t, 1);
            lines{i}.YData = trajs{i}(1:t, 2);
            bodies{i}.XData = trajs{i}(t, 1);
            bodies{i}.YData = trajs{i}(t, 2);

            wings{i}.XData = [trajs{i}(t, 1) - 0.1 * cos(trajs{i}(t, 3)); trajs{i}(t, 1) + 0.1 * cos(trajs{i}(t, 3))];
            wings{i}.YData = [trajs{i}(t, 2) + 0.1 * sin(trajs{i}(t, 3)); trajs{i}(t, 2) - 0.1 * sin(trajs{i}(t, 3))];
        end

        drawnow;
        pause(0.02)
    end
else
    for i = 1:num_quads
        lines{i}.XData = trajs{i}(1:end, 1);
        lines{i}.YData = trajs{i}(1:end, 2);
        bodies{i}.XData = trajs{i}(end, 1);
        bodies{i}.YData = trajs{i}(end, 2);

        wings{i}.XData = [trajs{i}(end, 1) - 0.1 * cos(trajs{i}(end, 3)); trajs{i}(end, 1) + 0.1 * cos(trajs{i}(end, 3))];
        wings{i}.YData = [trajs{i}(end, 2) + 0.1 * sin(trajs{i}(end, 3)); trajs{i}(end, 2) - 0.1 * sin(trajs{i}(end, 3))];
    end

    drawnow;
end