Commit 9714900e authored by mmroma's avatar mmroma
Browse files

Things are working for 5 quads, 1 payload! Still need to add back in wind,...

Things are working for 5 quads, 1 payload! Still need to add back in wind, user forces, and disturbances, but things look good and it simulates very quickly for a 3 second hover trajectory
parent c47f3f53
......@@ -12,6 +12,9 @@ function [params] = parameters_Payload(global_params)
params.Jx = J_sphere; % Moment of Inertia about x (kg m^2)
params.Jy = J_sphere; % Moment of Inertia about y (kg m^2)
params.Jz = J_sphere; % Moment of Inertia about z (kg m^2)
params.J = [params.Jx, 0, 0;
0, params.Jy, 0;
0, 0, params.Jz];
%% Noise and Disturbances
params.distSTD = 0; % 0-mean White Gaussian Dist (Ext Force)
......
function [] = animate_simulink_data(simData, params)
%animate Animates the multirotor
% Inputs:
% simData
% Outputs:
% Animation Plot
%% Some hardcoding before we smooth things out
% params.num_vehicles = 1;
using_load = true;
%% Process Input into simpler names
T0 = simData.tout;
X0 = [simData.q1_X, simData.q2_X,simData.q3_X,simData.q4_X,simData.q5_X];
X0 = [X0, simData.payload_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
if (using_load)
%% 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);
end
%% 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');
%% 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
if (using_load)
%% 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);
end
%% Change Viewpoint
view(-75,50);
%% Animate
tic
for i=1:length(t)
title(['t = ' num2str( t(i),'%2.1f') ' s']);
if (using_load)
% 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)];
end
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);
if (using_load)
% 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
end
if (using_load)
%Draws a point tracing the payload's path
plot3(L_X(i), L_Y(i), L_Z(i), 'c.')
end
% 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;
% %recording
% if strcmp(name, 'none') ~= 1
% frame = getframe;
% writeVideo(writerObj,frame);
% end
end
%close video object
% if strcmp(name, 'none') ~= 1
% close(writerObj);
% end
end
function [] = plot_force(T,F,name)
figure();
subplot(3,1,1);
plot(T,F(:,1));
ylabel('x-dir (N)');
title(name);
subplot(3,1,2);
plot(T,F(:,2));
ylabel('y-dir (N)');
subplot(3,1,3);
plot(T,F(:,3));
ylabel('z-dir (N)');
xlabel('Time (s)');
end
function [] = plot_orientation(T,X,Xd)
figure();
subplot(3,1,1);
plot(T,X(:,7));
hold on;
plot(T,Xd(:,7),'r--');
hold off;
ylabel('\phi (rad)');
xlabel('Time (s)');
title('Attitude');
legend('Actual','Desired');
ylim([-pi pi]);
subplot(3,1,2);
plot(T,X(:,8));
hold on;
plot(T,Xd(:,8),'r--');
hold off;
ylabel('\theta (rad)');
xlabel('Time (s)');
ylim([-pi pi]);
legend('Actual','Desired');
subplot(3,1,3);
plot(T,X(:,9));
hold on;
plot(T,Xd(:,9),'r--');
hold off;
ylabel('\psi (rad)');
xlabel('Time (s)');
ylim([-pi pi]);
legend('Actual','Desired');
end
function [] = plot_position(T,X,Xd)
figure();
subplot(3,1,1);
plot(T,X(:,1));
hold on;
plot(T,Xd(:,1),'r--');
hold off;
ylabel('x (m)');
title('Position');
legend('Actual','Desired');
subplot(3,1,2);
plot(T,X(:,2));
hold on;
plot(T,Xd(:,2),'r--');
hold off;
ylabel('y (m)');
legend('Actual','Desired');
subplot(3,1,3);
plot(T,X(:,3));
hold on;
plot(T,Xd(:,3),'r--');
hold off;
ylabel('z (m)');
xlabel('Time (s)');
legend('Actual','Desired');
end
......@@ -12,8 +12,19 @@ params = setUpParameters(traj_num);
%%
% call simulink
simData = sim('coop_payload_sim_simulink');
simData = sim('coop_payload_sim');
%% Plot
% plot(out.tout, out.y);
\ No newline at end of file
plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd);
plot_force(simData.tout, simData.q1_F_gravity, 'Q1 Gravity');
%% Animate
animate_simulink_data(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