Commit d592e39e authored by mmroma's avatar mmroma
Browse files

Tried to accelerate code, however, it appears things are funky now, need to...

Tried to accelerate code, however, it appears things are funky now, need to look at my changes and evaluate.
parent 322cee41
......@@ -2,6 +2,6 @@ function [coarse_WPs] = WPs_set1_hover()
%WPs_set1
h = 1.5;
coarse_WPs = [0, 0, h, 0;
0, 0, h, 1];
0, 0, h, 10];
end
function [X_struct] = X_struct_readable(X)
%X_struct_readable
X_struct.x = X(1);
X_struct.y = X(2);
X_struct.z = X(3);
X_struct.u = X(4);
X_struct.v = X(5);
X_struct.w = X(6);
X_struct.phi = X(7);
X_struct.theta = X(8);
X_struct.psi = X(9);
X_struct.p = X(10);
X_struct.q = X(11);
X_struct.r = X(12);
% X_struct.R_B_G = rpy_to_R(X_struct.phi, X_struct.theta, X_struct.psi);
X_struct.R_B_G = ...
[cos(theta)*cos(psi) sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);
cos(theta)*sin(psi) sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
-sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)];
end
......@@ -13,11 +13,9 @@ function [X_struct] = X_struct_readable(X)
X_struct.q = X(11);
X_struct.r = X(12);
X_struct.R_B_G = eul2rotm([X_struct.phi, X_struct.theta, X_struct.psi]);
% X_struct.R_B_G = rpy_to_R(X_struct.phi, X_struct.theta, X_struct.psi);
% X_struct.R_B_G = ...
% [cos(theta)*cos(psi) sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);
% cos(theta)*sin(psi) sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
% -sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)];
end
% X_struct.R_B_G = rpy_to_R(X_struct.X(7), X_struct.X(8), X_struct.X(9));
X_struct.R_B_G = [cos(X(8))*cos(X(9)) sin(X(7))*sin(X(8))*cos(X(9))-cos(X(7))*sin(X(9)) cos(X(7))*sin(X(8))*cos(X(9))+sin(X(7))*sin(X(9));
cos(X(8))*sin(X(9)) sin(X(7))*sin(X(8))*sin(X(9))+cos(X(7))*cos(X(9)) cos(X(7))*sin(X(8))*sin(X(9))-sin(X(7))*cos(X(9));
-sin(X(8)) sin(X(7))*cos(X(8)) cos(X(7))*cos(X(8))];
end
function [simData] = runSimulation(params)
%runSimulaion
% Runs the simulation by iteratively calling the controller and
% updating the state with Euler's Method at the specified rates.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% 1) Initialization %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 1.1) Calculate relevant sizes based on parameters
num_tsteps = length(0:params.ctlDt:params.tFinal);
num_states = 12 * (params.num_vehicles + 1);
num_inputs = 4 * params.num_vehicles;
%% 1.2) Initialize Variables with proper sizes
T = zeros(num_tsteps,1);
X_curr = zeros(num_states,1);
X = zeros(num_tsteps, num_states);
X_des = zeros(num_tsteps, (params.num_vehicles +1) * 12);
U_curr = zeros(num_inputs,1);
U = zeros(num_tsteps, num_inputs);
F_nets = zeros(num_tsteps, (params.num_vehicles+1) * 3);
Tao_nets = zeros(num_tsteps, (params.num_vehicles+1) * 3);
F_tethers = zeros(num_tsteps, (params.num_vehicles) * 3);
F_drags = zeros(num_tsteps, (params.num_vehicles+1) * 3);
Tether_states = zeros(num_tsteps, (params.num_vehicles) * 2);
%% 1.3) Initialize Current State based on parameters files
for i = 1:params.num_vehicles
X_curr(i*12-11:i*12) = params.multirotors{i}.x0;
end
X_curr(params.num_vehicles*12+1:end) = params.payload.x0;
%% 1.4) Make sensor noise weighting vector
noiseSTDvec = zeros(size(X_curr));
for i=1:params.num_vehicles
noiseSTDvec(i*12-11:i*12) = params.noiseSTD;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% 2) Run the Simulation %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
tstep_count = 1;
for t_curr = 0:params.multirotors{1}.ctlDt:params.tFinal
%% 2.1) Add sensor noise before control input
% X_curr_w_noise = X_curr + dot(noiseSTDvec, randn(size(X_curr)));
X_curr_w_noise = X_curr;
%% 2.2) Initialize State and Desired Variables for this loop
X_des_curr = [];
%% 2.3) Compute the Payload Augmented Desired State Control
params.tstep_count = tstep_count;
params = payload_controller(X_curr, params);
%% 2.4) Compute Control Input for the vehicles
for i = 1:params.num_vehicles
X_inds = ((i-1)*12 + 1 ) : (i*12);
params.multirotors{i}.tstep_count = tstep_count;
[U_curr_,X_des_curr_] = params.multirotors{i}.controller(X_curr_w_noise(X_inds),params.multirotors{i});
U_curr = [U_curr; U_curr_];
X_des_curr = [X_des_curr; X_des_curr_];
end
%% 2.5) Constrain Control Input
U_curr = min(U_curr,1);
U_curr = max(U_curr,0);
%% 2.6) Record payload desired state
X_des_curr_ = params.payload.xd(tstep_count,:)';
X_des_curr = [X_des_curr; X_des_curr_];
%% 2.7) Record Essential Simulation Data
T(tstep_count) = t_curr;
X(tstep_count,:) = X_curr';
U(tstep_count,:) = U_curr';
X_des(tstep_count,:) = X_des_curr';
%% 2.8) Record Additional Force/Torque Data
[~,F_nets_curr,Tao_nets_curr,F_tethers_curr,F_drags_curr,T_states_curr] = systemModel(X_curr,U_curr,params);
F_nets(tstep_count,:) = F_nets_curr';
Tao_nets(tstep_count,:) = Tao_nets_curr';
F_tethers(tstep_count,:) = F_tethers_curr';
F_drags(tstep_count,:) = F_drags_curr';
Tether_states(tstep_count,:) = T_states_curr';
% %% 2.9) Simulate with Euler's Method
% for t=t_curr:params.tStep:(t_curr+params.multirotors{1}.ctlDt-params.tStep)
% X_curr = X_curr + params.tStep * systemModel(X_curr,U_curr,params);
% % X_curr = constrain_euler_angles(X_curr);
% end
%% 2.9) Simulate with ode45
[~,Xi] = ode45( @(t,x) systemModel(x,U_curr,params),...
t_curr+params.tStep:params.tStep:t_curr+params.multirotors{1}.ctlDt,...
X_curr);
X_curr = Xi(end,:)';
%% 2.10) Increment the time step count
tstep_count = tstep_count + 1;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% 3) Clean Up %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 3.1) Create Variables to Hold Everything
simData.T = T;
simData.X = X;
simData.U = U;
simData.X_des = X_des;
simData.F_nets = F_nets;
simData.Tao_nets = Tao_nets;
simData.F_tethers = F_tethers;
simData.F_drags = F_drags;
simData.Tether_states = Tether_states;
end
......@@ -8,8 +8,8 @@ function [] = animate(simData,params)
% Animation Plot
%% Process Input into simpler names
T0 = simData.T;
X0 = simData.X;
T0 = simData.T';
X0 = simData.X';
anim_FPS = params.anim_FPS;
......
......@@ -9,7 +9,7 @@ function [fig_num] = plotData_drag(simData,params)
% F_drags = zeros(length(simData.T),params.num_vehicles*3);
F_drag_mags = zeros(length(simData.T),params.num_vehicles);
for i=1:params.num_vehicles
F_drag_mags(:,i) = vecnorm(simData.F_drags(:,i*3-2:i*3)')';
F_drag_mags(:,i) = vecnorm(simData.F_drags(i*3-2:i*3,:)')';
end
figure(fig_num);
......@@ -32,7 +32,7 @@ function [fig_num] = plotData_drag(simData,params)
movegui(gcf,'onscreen');
%% 2) Plot Drag Force on Payload
F_drag_P = simData.F_drags(:,end-2:end);
F_drag_P = simData.F_drags(end-2:end,:);
figure(fig_num);
plot(simData.T, F_drag_P(:,1));
......
......@@ -9,9 +9,9 @@ function [fig_num] = plotData_payload(simData,params)
figure(fig_num);
subplot(3,1,1);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 1));
plot(simData.T,simData.X(params.num_vehicles*12 + 1,:));
hold on;
plot(simData.T,simData.X_des(:,params.num_vehicles*12 + 1),'r--');
plot(simData.T,simData.X_des(params.num_vehicles*12 + 1,:),'r--');
hold off;
legend('Actual','Desired');
ylabel('x (m)');
......@@ -19,24 +19,24 @@ function [fig_num] = plotData_payload(simData,params)
title('Load Position');
subplot(3,1,2);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 2));
plot(simData.T,simData.X(params.num_vehicles*12 + 2,:));
hold on;
plot(simData.T,simData.X_des(:,params.num_vehicles*12 + 2),'r--');
plot(simData.T,simData.X_des(params.num_vehicles*12 + 2,:),'r--');
hold off;
legend('Actual','Desired');
ylabel('y (m)');
% minYLims(ylim,-1,1);
subplot(3,1,3);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 3));
plot(simData.T,simData.X(params.num_vehicles*12 + 3,:));
hold on;
plot(simData.T,simData.X_des(:,params.num_vehicles*12 + 3),'r--');
plot(simData.T,simData.X_des(params.num_vehicles*12 + 3,:),'r--');
hold off;
legend('Actual','Desired');
ylabel('z (m)');
xlabel('Time (s)');
% minYLims(ylim,-1,1);
ylim([min(simData.X(:,params.num_vehicles*12 + 3))-eps max(simData.X(:,params.num_vehicles*12 + 3))+eps]);
% ylim([min(simData.X(:,params.num_vehicles*12 + 3))-eps max(simData.X(:,params.num_vehicles*12 + 3))+eps]);
fig_num = save_current_figure(fig_prefix, fig_num, save_figs_bool);
......@@ -48,9 +48,9 @@ function [fig_num] = plotData_payload(simData,params)
figure(fig_num);
subplot(3,1,1);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 4));
plot(simData.T,simData.X(params.num_vehicles*12 + 4,:));
hold on;
plot(simData.T,simData.X_des(:,params.num_vehicles*12 + 4),'r--');
plot(simData.T,simData.X_des(params.num_vehicles*12 + 4,:),'r--');
hold off;
legend('Actual','Desired');
ylabel('u (m/s)');
......@@ -58,18 +58,18 @@ function [fig_num] = plotData_payload(simData,params)
title('Load Velocity');
subplot(3,1,2);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 5));
plot(simData.T,simData.X(params.num_vehicles*12 + 5,:));
hold on;
plot(simData.T,simData.X_des(:,params.num_vehicles*12 + 5),'r--');
plot(simData.T,simData.X_des(params.num_vehicles*12 + 5,:),'r--');
hold off;
legend('Actual','Desired');
ylabel('v (m/s)');
% minYLims(ylim,-1,1);
subplot(3,1,3);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 6));
plot(simData.T,simData.X(params.num_vehicles*12 + 6,:));
hold on;
plot(simData.T,simData.X_des(:,params.num_vehicles*12 + 6),'r--');
plot(simData.T,simData.X_des(params.num_vehicles*12 + 6,:),'r--');
hold off;
legend('Actual','Desired');
ylabel('w (m/s)');
......@@ -102,18 +102,18 @@ function [fig_num] = plotData_payload(simData,params)
% ylim([-pi pi]);
subplot(3,1,1);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 7));
plot(simData.T,simData.X(params.num_vehicles*12 + 7,:));
ylabel('\phi (rad)');
% ylim([-pi pi]);
title('Load Attitude');
subplot(3,1,2);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 8));
plot(simData.T,simData.X(params.num_vehicles*12 + 8,:));
ylabel('\theta (rad)');
% ylim([-pi pi]);
subplot(3,1,3);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 9));
plot(simData.T,simData.X(params.num_vehicles*12 + 9,:));
ylabel('\psi (rad)');
% ylim([-pi pi]);
......@@ -124,14 +124,14 @@ function [fig_num] = plotData_payload(simData,params)
movegui(gcf,'onscreen');
%% Plot Net Force on Load
F_net = simData.F_nets(:,end-2:end);
F_net = simData.F_nets(end-2:end,:);
% F_net_mag = vecnorm(F_net')';
figure(fig_num);
plot(simData.T, F_net(:,1));
plot(simData.T, F_net(1,:));
hold on;
plot(simData.T, F_net(:,2));
plot(simData.T, F_net(:,3));
plot(simData.T, F_net(2,:));
plot(simData.T, F_net(3,:));
hold off;
ylabel('Force (N)');
xlabel('Time (s)');
......@@ -144,14 +144,14 @@ function [fig_num] = plotData_payload(simData,params)
movegui(gcf,'onscreen');
%% Plot Net Torque on Load
Tao_net = simData.Tao_nets(:,end-2:end);
Tao_net = simData.Tao_nets(end-2:end,:);
% F_net_mag = vecnorm(F_net')';
figure(fig_num);
plot(simData.T, Tao_net(:,1));
plot(simData.T, Tao_net(1,:));
hold on;
plot(simData.T, Tao_net(:,2));
plot(simData.T, Tao_net(:,3));
plot(simData.T, Tao_net(2,:));
plot(simData.T, Tao_net(3,:));
hold off;
ylabel('Torque (Nm)');
xlabel('Time (s)');
......
......@@ -9,9 +9,9 @@ function [fig_num] = plotData_quadrotor(simData,params)
figure(fig_num);
subplot(3,1,1);
plot(simData.T,simData.X(:,1));
plot(simData.T,simData.X(1,:));
hold on;
plot(simData.T,simData.X_des(:,1),'r--');
plot(simData.T,simData.X_des(1,:),'r--');
hold off;
ylabel('x (m)');
title('Position');
......@@ -19,18 +19,18 @@ function [fig_num] = plotData_quadrotor(simData,params)
minYLims(ylim,-1,1);
subplot(3,1,2);
plot(simData.T,simData.X(:,2));
plot(simData.T,simData.X(2,:));
hold on;
plot(simData.T,simData.X_des(:,2),'r--');
plot(simData.T,simData.X_des(2,:),'r--');
hold off;
ylabel('y (m)');
legend('Actual','Desired');
minYLims(ylim,-1,1);
subplot(3,1,3);
plot(simData.T,simData.X(:,3));
plot(simData.T,simData.X(3,:));
hold on;
plot(simData.T,simData.X_des(:,3),'r--');
plot(simData.T,simData.X_des(3,:),'r--');
hold off;
ylabel('z (m)');
xlabel('Time (s)');
......@@ -45,9 +45,9 @@ function [fig_num] = plotData_quadrotor(simData,params)
figure(fig_num);
subplot(3,1,1);
plot(simData.T,simData.X(:,4));
plot(simData.T,simData.X(4,:));
hold on;
plot(simData.T,simData.X_des(:,4),'r--');
plot(simData.T,simData.X_des(4,:),'r--');
hold off;
ylabel('u (m/s)');
title('Velocity');
......@@ -55,18 +55,18 @@ function [fig_num] = plotData_quadrotor(simData,params)
minYLims(ylim,-1,1);
subplot(3,1,2);
plot(simData.T,simData.X(:,5));
plot(simData.T,simData.X(5,:));
hold on;
plot(simData.T,simData.X_des(:,5),'r--');
plot(simData.T,simData.X_des(5,:),'r--');
hold off;
ylabel('v (m/s)');
legend('Actual','Desired');
minYLims(ylim,-1,1);
subplot(3,1,3);
plot(simData.T,simData.X(:,6));
plot(simData.T,simData.X(6,:));
hold on;
plot(simData.T,simData.X_des(:,6),'r--');
plot(simData.T,simData.X_des(6,:),'r--');
hold off;
ylabel('w (m/s)');
xlabel('Time (s)');
......@@ -81,9 +81,9 @@ function [fig_num] = plotData_quadrotor(simData,params)
figure(fig_num);
subplot(3,1,1);
plot(simData.T,simData.X(:,7));
plot(simData.T,simData.X(7,:));
hold on;
plot(simData.T,simData.X_des(:,7),'r--');
plot(simData.T,simData.X_des(7,:),'r--');
hold off;
ylabel('\phi (rad)');
title('Attitude');
......@@ -91,18 +91,18 @@ function [fig_num] = plotData_quadrotor(simData,params)
legend('Actual','Desired');
subplot(3,1,2);
plot(simData.T,simData.X(:,8));
plot(simData.T,simData.X(8,:));
hold on;
plot(simData.T,simData.X_des(:,8),'r--');
plot(simData.T,simData.X_des(8,:),'r--');
hold off;
ylabel('\theta (rad)');
ylim([-pi pi]);
legend('Actual','Desired');
subplot(3,1,3);
plot(simData.T,simData.X(:,9));
plot(simData.T,simData.X(9,:));
hold on;
plot(simData.T,simData.X_des(:,9),'r--');
plot(simData.T,simData.X_des(9,:),'r--');
hold off;
ylabel('\psi (rad)');
xlabel('Time (s)');
......@@ -117,9 +117,9 @@ function [fig_num] = plotData_quadrotor(simData,params)
figure(fig_num);
subplot(3,1,1);
plot(simData.T,simData.X(:,10));
plot(simData.T,simData.X(10,:));
hold on;
plot(simData.T,simData.X_des(:,10),'r--');
plot(simData.T,simData.X_des(10,:),'r--');
hold off;
ylabel('p (rad/s)');
title('Attitude Rate');
......@@ -127,18 +127,18 @@ function [fig_num] = plotData_quadrotor(simData,params)
minYLims(ylim,-1,1);
subplot(3,1,2);
plot(simData.T,simData.X(:,11));
plot(simData.T,simData.X(11,:));
hold on;
plot(simData.T,simData.X_des(:,11),'r--');
plot(simData.T,simData.X_des(11,:),'r--');
hold off;
ylabel('q (rad/s)');
legend('Actual','Desired');
minYLims(ylim,-1,1);
subplot(3,1,3);
plot(simData.T,simData.X(:,12));
plot(simData.T,simData.X(12,:));
hold on;
plot(simData.T,simData.X_des(:,12),'r--');
plot(simData.T,simData.X_des(12,:),'r--');
hold off;
ylabel('r (rad/s)');
xlabel('Time (s)');
......@@ -153,7 +153,7 @@ function [fig_num] = plotData_quadrotor(simData,params)
figure(fig_num);
legendEntries = cell(1,params.multirotors{1}.numRotors);
for i=1:params.multirotors{1}.numRotors
plot(simData.T,simData.U(:,i));
plot(simData.T,simData.U(i,:));
hold on;
legendEntries{i} = ['Motor ' num2str(i)];
end
......
......@@ -8,7 +8,7 @@ function [fig_num] = plotData_tethers(simData,params)
%% 1) Plot Tether Force Magnitudes
F_mags = zeros(length(simData.T),params.num_vehicles);
for i=1:params.num_vehicles
F_mags(:,i) = vecnorm(simData.F_tethers(:,i*3-2:i*3)')';
F_mags(:,i) = vecnorm(simData.F_tethers(i*3-2:i*3,:)')';
end
offset = 1;
......@@ -49,8 +49,8 @@ function [fig_num] = plotData_tethers(simData,params)
l_i = zeros(length(simData.T),params.num_vehicles);
l_dot_i = zeros(length(simData.T),params.num_vehicles);
for i=1:params.num_vehicles
l_i(:,i) = simData.Tether_states(:,i*2-1);
l_dot_i(:,i) = simData.Tether_states(:,i*2);
l_i(:,i) = simData.Tether_states(i*2-1,:);
l_dot_i(:,i) = simData.Tether_states(i*2,:);
end
figure(fig_num);
......
......@@ -16,8 +16,8 @@ params = setUpParameters();
simData = runSimulation(params);
%% Plot
% plotData(simData,params);
plotData(simData,params);
%% Animate
% animate(simData,params);
animate(simData,params);
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment