Commit 2df5c851 authored by mmroma's avatar mmroma
Browse files

Cleaning

parent d6324aa8
.DS_Store
*.asv
*.slxc
\ No newline at end of file
*.slxc
slprj/
\ No newline at end of file
*
*/
!.gitignore
!save_current_figure.m
!save_video.m
\ No newline at end of file
function [fig_num] = save_current_figure(prefix,fig_num,save_figs_bool)
%SAVE_CURRENT_FIGURE
if ~save_figs_bool
fig_num = fig_num + 1;
return
else
figname = strcat(prefix,string(fig_num));
fig_num = fig_num + 1;
savefig( strcat('Figures&Animations/',figname,'.fig'));
saveas(gcf,strcat('Figures&Animations/',figname,'.png'));
saveas(gcf,strcat('Figures&Animations/',figname,'.pdf'));
end
end
%unnecessary function
function [] = save_video(name, simData,params)
writerObj = VideoWriter(['Figures&Animations/' name '.avi']);
writerObj.FrameRate = 60;
open(writerObj);
%% Process Input into simpler names
T0 = simData.T';
X0 = simData.X';
anim_FPS = params.anim_FPS;
%% Interpolate to get requested display rate
deltaT = 1 / anim_FPS;
t = T0(1) : deltaT : T0(end);
load_ind = params.num_vehicles + 1;
% Get Data for Each Vehicle
for quad = 1:params.num_vehicles
%% Obtain X,Y,Z from state
X{quad} = interp1(T0,X0(:,(quad-1)*12 + 1),t);
Y{quad} = interp1(T0,X0(:,(quad-1)*12 + 2),t);
Z{quad} = interp1(T0,X0(:,(quad-1)*12 + 3),t);
%% Obtain Roll, Pitch, Yaw from state
Phi{quad} = interp1(T0,X0(:,(quad-1)*12 + 7),t);
Theta{quad} = interp1(T0,X0(:,(quad-1)*12 + 8),t);
Psi{quad} = interp1(T0,X0(:,(quad-1)*12 + 9),t);
%% Process mounting points
r_Pi_P{quad} = params.multirotors{quad}.r_Pi_P;
end
%% Obtain Load X,Y,Z from state
L_X = interp1(T0,X0(:,params.num_vehicles*12 + 1),t);
L_Y = interp1(T0,X0(:,params.num_vehicles*12 + 2),t);
L_Z = interp1(T0,X0(:,params.num_vehicles*12 + 3),t);
%% Obtain Load Roll, Pitch, Yaw from state
Phi{load_ind} = interp1(T0,X0(:,params.num_vehicles*12 + 7),t);
Theta{load_ind} = interp1(T0,X0(:,params.num_vehicles*12 + 8),t);
Psi{load_ind} = interp1(T0,X0(:,params.num_vehicles*12 + 9),t);
%% Set Graph Limits
cubeSideLength = 3;
% X Graph Limits
xUlim = cubeSideLength;
xLlim = -cubeSideLength;
% Y Graph Limtis
yUlim = cubeSideLength;
yLlim = -cubeSideLength;
% Z Graph Limits
zUlim = cubeSideLength;
zLlim = -cubeSideLength;
% zUlim = -1;
% zLlim = -2;
%% Set up the graph
figure(100); clf;
axis([xLlim xUlim yLlim yUlim zLlim zUlim]);
ax = gca;
xlabel('X');
ylabel('Y');
zlabel('Z');
% Flip Z Axis and Y Axis
set(gca, 'Zdir', 'reverse');
set(gca, 'Ydir', 'reverse');
grid on;
movegui(gcf,'north');
movegui(gcf,'offscreen');
%% Draw Multirotor
% Quadrotor in Cross Configuration
qw = 0.25; %Quad Width
qh = 0.04; %Quad Height
quadFrontColor = [1 0 0];
quadBackColor = [0 0 0];
multiRotor_T = eye(4);
for quad = 1 : params.num_vehicles
mr{quad} = drawQuad(ax,qw,qh,quadFrontColor,quadBackColor);
set(mr{quad},'Matrix',multiRotor_T);
end
%% Draw Tether
for quad = 1 : params.num_vehicles
tether{quad} = line([X{quad}(1) L_X(1)],[Y{quad}(1) L_Y(1)],[Z{quad}(1) L_Z(1)]);
set(tether{quad},'LineWidth',2,'Color','g');
end
%% Draw Payload
pw = 0.05;
ph = 0.05;
payloadColor = 'r';
p = drawPayload(ax,pw,ph,payloadColor);
payload_T = eye(4);
set(p,'Matrix',payload_T);
%% Change Viewpoint
view(-75,50);
tic
for i=1:length(t)
title(['t = ' num2str( t(i),'%2.1f') ' s']);
% Update Payload
payload_T = makehgtform('zrotate',Psi{load_ind}(i)) *...
makehgtform('yrotate',Theta{load_ind}(i)) *...
makehgtform('xrotate',Phi{load_ind}(i));
payload_T(1:3,4) = [L_X(i); L_Y(i); L_Z(i)];
set(p,'Matrix',payload_T);
R_P_G = payload_T(1:3,1:3);
r_P_G = [L_X(i); L_Y(i); L_Z(i)];
for quad = 1 : params.num_vehicles
% Update Multirotor
multiRotor_T = makehgtform('zrotate',Psi{quad}(i)) *...
makehgtform('yrotate',Theta{quad}(i)) *...
makehgtform('xrotate',Phi{quad}(i));
multiRotor_T(1:3,4) = [X{quad}(i); Y{quad}(i); Z{quad}(i)];
set(mr{quad},'Matrix',multiRotor_T);
% Update Tether
r_Pi_G = R_P_G * r_Pi_P{quad} + r_P_G;
set(tether{quad},'XData',[X{quad}(i) r_Pi_G(1)],'YData',[Y{quad}(i) r_Pi_G(2)],'ZData',[Z{quad}(i) r_Pi_G(3)]);
end
%Draws a point tracing the payload's path
plot3(L_X(i), L_Y(i), L_Z(i), 'c.')
% Display the Animation and sleep to maintain the display FPS
drawnow;
sleepTime = deltaT - toc;
if (sleepTime > 0)
java.lang.Thread.sleep((sleepTime)*1000);
end
tic;
frame = getframe;
writeVideo(writerObj,frame);
end
close(writerObj);
end
\ No newline at end of file
%% GenerateSimData
% Generates simData and params by doing a parameter sweep over
% parameters of interest
%% Add Necessary Folders to MATLAB Path (add everything)
addpath(genpath(pwd))
%% 1) Loop Over Every Parameter & Generate simData
trajectories = {1, 2, 3, 10, 11, 16, 19, 20, 21, 22};
payload_masses = {0.1, 0.5};
wind_speed = {0 , 5};
run_counter = 1;
for traj = trajectories
%% Set up parameters
params = setUpParameters(traj{1});
for payload_mass = payload_masses
%% Change parameter
params.payload.m = payload_mass{1};
for wind = wind_speed
%% Change parameter
params.wind.enable = 1;
params.wind.speed = wind{1};
%% Simulate
simData = runSimulation(params);
%% Save Data to file
saveData(simData,params, 'run');
%% Increment Run Counter
run_counter = run_counter + 1;
end
end
end
%PlotSimData(1, run_counter);
\ No newline at end of file
%% PlotSimData
% Plots simData and params and saves them
function [] = PlotSimData(end_file_num, optionalVarChangeDefaultParam)
%% Determine if there is an optional argument
start_file_num = 1;
if nargin > 1
start_file_num = optionalVarChangeDefaultParam;
end
%% 1) Loop Over Every Simulation data file
for file = start_file_num:end_file_num
%% Set up file info
filename = ['run' int2str(file) '.mat'];
load(filename);
params.curr_run = file;
%% Display current round
fprintf(['Run' int2str(file) '\n']);
%% Plot
plotData(simData,params);
save_current_figure("WPs_run", 1, params.curr_run, 1);
%% Transition to animation
pause(10);
%close all;
close(figure(1));
close(figure(2));
close(figure(3));
close(figure(4));
close(figure(5));
%% Animate
animate(['Run_' int2str(params.curr_run)], simData,params);
pause(10);
close all
end
end
\ No newline at end of file
......@@ -6,7 +6,9 @@ Matthew Romano (mmroma@umich.edu)
## Overview
This is a simulation written in MATLAB for the cooperative payload transportation project. A system model including multiple quadrotors, a payload, aerodynamic drag, and user input is considered.
This is a simulation written in MATLAB for the cooperative payload transportation project. A system model including multiple quadrotors, a payload, aerodynamic drag, and user input is considered.
Note: The PhysicsModel/ folder is mostly unused. This was used previously when MATLAB-based simulation was being done, but now that I've switched over to using Simulink only a few helper functions from that are being used, however, which is a good enough reason to keep it for now.
## Running the Code
......
*
*/
!.gitignore
\ No newline at end of file
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