Commit 424758a3 authored by mmroma's avatar mmroma
Browse files

Things are working! I still need to add aero drag forces as well as guidance...

Things are working! I still need to add aero drag forces as well as guidance for payload / quadrotors.
parent fc52c35d
......@@ -68,6 +68,7 @@ function [params] = parameters_Quad()
params.b = 5; % Damping Coefficient
params.NegTension = 0; % Allow "Negative Tensions"? 0 = No, 1 = Yes
params.r_Pi_P = [0;0;0]; % Tether Mounting Point in Payload Frame
params.MaxTension = 100*params.m*params.g;
end
function [params] = setUpParameters()
%setUpParameters
% Function that actually chooses which parameters function to call.
% Comment/uncomment lines to select the parameters you want to run and
% feel free to make as many parameters_*.m functions as you want.
%% Load Multirotors
params.num_vehicles = 4;
L_all = 1.5;
% Q1
params.multirotors{1} = parameters_Quad();
params.multirotors{1}.x0(1:3) = [1.5;0.5;0];
params.multirotors{1}.xd(1:3) = [0.5;-0.5;0];
params.multirotors{1}.L = L_all;
% Q2
params.multirotors{2} = parameters_Quad();
params.multirotors{2}.x0(1:3) = [1.5;1.5;0];
params.multirotors{2}.xd(1:3) = [0.5;0.5;0];
params.multirotors{2}.L = L_all;
% Q3
params.multirotors{3} = parameters_Quad();
params.multirotors{3}.x0(1:3) = [0.5;1.5;0];
params.multirotors{3}.xd(1:3) = [-0.5;0.5;0];
params.multirotors{3}.L = L_all;
% Q4
params.multirotors{4} = parameters_Quad();
params.multirotors{4}.x0(1:3) = [0.5;0.5;0];
params.multirotors{4}.xd(1:3) = [-0.5;-0.5;0];
params.multirotors{4}.L = L_all;
%% Load Payload
params.payload = parameters_Payload();
% params.payload.x0(1:3) = [0;0;sqrt(3)/2];
params.payload.x0(1:3) = [1;1;0];
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.0025; % Simulation Step Size (s)
params.anim_FPS = 60; % Animation FPS
%% Sensor Noise
params.noiseSTD = 0; %0.1; % 0-mean White Gaussian Noise (Sensor Noise)
end
function [params] = setUpParameters_sphere_1_stationary()
%setUpParameters
% Stationary Test
%% Load Payload (sphere in this case)
params.payload = parameters_Payload();
% params.payload.x0(1:3) = [0;0;sqrt(3)/2];
params.payload.x0(1:3) = [0;0;0];
r = params.payload.diameter / 2;
%% Load Multirotors
params.num_vehicles = 4;
L_all = 1.5;
height_all = -1.5;
% Q1
i = 1;
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.x0(1:3) = [0.5;-0.5; height_all];
params.multirotors{i}.xd(1:3) = [0.5;-0.5; height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',-pi/4) *...
makehgtform('yrotate',pi/4) *...
[r;0;0;1];
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
% Q2
i = 2;
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.x0(1:3) = [0.5;0.5;height_all];
params.multirotors{i}.xd(1:3) = [0.5;0.5;height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',pi/4) *...
makehgtform('yrotate',pi/4) *...
[r;0;0;1];
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
% Q3
i = 3;
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.x0(1:3) = [-0.5;-0.5;height_all];
params.multirotors{i}.xd(1:3) = [-0.5;-0.5;height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',-3*pi/4) *...
makehgtform('yrotate',pi/4) *...
[r;0;0;1];
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
% Q4
i = 4;
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.x0(1:3) = [-0.5;0.5;height_all];
params.multirotors{i}.xd(1:3) = [-0.5;0.5;height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',3*pi/4) *...
makehgtform('yrotate',pi/4) *...
[r;0;0;1];
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.0025; % Simulation Step Size (s)
params.anim_FPS = 60; % Animation FPS
%% Sensor Noise
params.noiseSTD = 0; %0.1; % 0-mean White Gaussian Noise (Sensor Noise)
end
......@@ -10,17 +10,20 @@ function [simData] = runSimulation(params)
end
X_curr = [X_curr; params.payload.x0];
T = [];
X = [];
U = [];
X_des = [];
num_tsteps = length(0:params.multirotors{1}.ctlDt:params.tFinal);
num_states = length(X_curr);
num_inputs = 4 * params.num_vehicles;
T = [];
X = zeros(num_tsteps, num_states);
X_des = zeros(num_tsteps, params.num_vehicles * 12);
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);
%% Run the simulation
tstep_count = 1;
......@@ -44,24 +47,21 @@ function [simData] = runSimulation(params)
% Record Data
T = [T; t_curr];
% X = [X; X_curr'];
X(tstep_count,:) = X_curr';
% U = [U; U_curr'];
U(tstep_count,:) = U_curr';
% X_des = [X_des; X_des_curr'];
X_des(tstep_count,:) = X_des_curr';
[~,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';
% 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);
end
% Simulate with ode45
% tspan = [t_curr, t_curr + params.multirotors{1}.ctlDt];
% [~,y] = ode45(@(t,y) systemModel(y,U_curr,params), tspan, X_curr);
% X_curr = y(end,:)';
% display('help! I need somebody');
tstep_count = tstep_count + 1;
end
......@@ -70,6 +70,11 @@ function [simData] = runSimulation(params)
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
function [xdot] = systemModel(X,U,params)
%systemModel Implements the System Model Dynamics for a multicopter with a
%slung load. Computes the derivative of the state given the state
% Inputs:
% x: state (num_vehicles*12 + 12x1)
% U: input (params.numRotors x 1)
% Outputs:
% xdot: state derivative (num_vehicles*12 + 12x1)
%% 0) Constrain Input
U = min(U,1);
U = max(U,0);
%% 1) Grab Payload Information first
m_load = params.payload.m;
load_Jx = params.payload.Jx;
load_Jy = params.payload.Jy;
load_Jz = params.payload.Jz;
load_x = X(params.num_vehicles*12 + 1);
load_y = X(params.num_vehicles*12 + 2);
load_z = X(params.num_vehicles*12 + 3);
load_u = X(params.num_vehicles*12 + 4);
load_v = X(params.num_vehicles*12 + 5);
load_w = X(params.num_vehicles*12 + 6);
load_phi = X(params.num_vehicles*12 + 7);
load_theta = X(params.num_vehicles*12 + 8);
load_psi = X(params.num_vehicles*12 + 9);
load_p = X(params.num_vehicles*12 + 10);
load_q = X(params.num_vehicles*12 + 11);
load_r = X(params.num_vehicles*12 + 12);
load_omega = [load_p; load_q; load_r];
R_P_G = rpy_to_R(load_phi, load_theta, load_psi);
load_pos_d = R_P_G * [load_u; load_v; load_w];
load_vx = load_pos_d(1);
load_vy = load_pos_d(2);
load_vz = load_pos_d(3);
%% 2) Calculate State Derivative for each Vehicle
xdot = [];
F_Tether_total = [0;0;0];
for i = 1:params.num_vehicles
%% 2.1) Get Constants
% Multirotor Constants
g = params.multirotors{i}.g;
m = params.multirotors{i}.m;
Jx = params.multirotors{i}.Jx;
Jy = params.multirotors{i}.Jy;
Jz = params.multirotors{i}.Jz;
M = params.multirotors{i}.M;
% Payload Constants
L = params.multirotors{i}.L;
k = params.multirotors{i}.k;
b = params.multirotors{i}.b;
%% 2.2) Get Vehicle States
x = X((i-1)*12 + 1);
y = X((i-1)*12 + 2);
z = X((i-1)*12 + 3);
u = X((i-1)*12 + 4);
v = X((i-1)*12 + 5);
w = X((i-1)*12 + 6);
phi = X((i-1)*12 + 7);
theta = X((i-1)*12 + 8);
psi = X((i-1)*12 + 9);
p = X((i-1)*12 + 10);
q = X((i-1)*12 + 11);
r = X((i-1)*12 + 12);
%% 2.3) Compute Forces and Torques From Propellers on Multirotor
u0 = (i-1)*params.multirotors{i}.numRotors + 1;
un = i*params.multirotors{i}.numRotors;
F_and_Taos = M * U(u0:un);
F = F_and_Taos(1);
Tao_phi = F_and_Taos(2);
Tao_theta = F_and_Taos(3);
Tao_psi = F_and_Taos(4);
%% 2.3) (1-3) Position Derivative ([x_d; y_d; z_d])
R_V_G = rpy_to_R(phi, theta, psi);
pos_d = R_V_G * [u; v; w];
%% 2.4) Compute Force From Slung Load (this might be more complicated)
% Preliminary Calcs
r_Vi_G = [x;y;z];
r_P_G = [load_x; load_y; load_z];
r_Pi_G = R_P_G * params.r_Pi_P + r_P_G;
t_Pi_G = r_Pi_G - r_Vi_G;
l_i = norm(t_Pi_G,2);
t_hat_Pi_G = t_Pi_G ./ l_i;
t_dot_Pi_G = cross(load_omega, r_Pi_G) + load_pos_d;
l_dot_i = dot(t_dot_Pi_G, t_hat_Pi_G);
F_tether_mag = (k.*(l - L) + b.*l_dot);
% Mounting Point in Ground Frame
r_Pi_G =
t_Pi_G = [
l = sqrt( (load_x-x)^2 + (load_y-y)^2 + (load_z-z)^2 );
l_dot = ((load_x-x)*(load_vx-pos_d(1)) + (load_y-y)*(load_vy-pos_d(2)) + (load_z-z)*(load_vz-pos_d(3))) / l;
r_hat = [(load_x-x); (load_y-y); (load_z-z)] ./ l;
F_Tether_i = (k.*(l - L) + b.*l_dot);
if (params.multirotors{i}.NegTension == 0)
F_Tether_i = max(F_Tether_i,0);
end
% Points in -r_hat direction
F_Tether_i = F_Tether_i .* -1 .* r_hat;
F_Tether_total = F_Tether_total + F_Tether_i;
%% 2.5) (4-6) Velocity Derivative ([u_d; v_d; w_d])
F_thrust_V = [0; 0; -F];
F_gravity_V = R_V_G' * [0; 0; m*g];
F_tether_V = - R_V_G' * F_Tether_i;
F_dist_V = params.multirotors{1}.distSTD .* randn(size(F_gravity_V));
F_net_V = F_thrust_V+ F_gravity_V + F_tether_V + F_dist_V;
vel_d = cross([p;q;r],[u;v;w]) + F_net_V./m_load;
%% 2.6) (7-9) Attitude Derivative ([phi_d; theta_d; psi_d])
att_d = att_der_matrix(phi, theta) * [p; q; r];
%% 2.7) (10-12) Attitude Rate Derivative ([p_d; q_d; r_d])
Tao_net_V = [Tao_phi; Tao_theta; Tao_psi];
attRate_d = attRate_der(Jx,Jy,Jz,p,q,r,Tao_net_V);
%% 2.8) Combine and append to xdot vector
xdot = [xdot;
pos_d;
vel_d;
att_d;
attRate_d];
end
%% 3) Calculate State Derivative for the Payload
%% 3.1) (13-15) Slung Load Position (load_pos_d from above)
%% 3.2) (16-18) Slung Load Velocity
% Calculate Forces
F_gravity_P = R_P_G' * [0; 0; m_load*g];
F_Tether_total_P = R_P_G' * F_Tether_total;
F_load_dist_P = params.payload.distSTD .* randn(size(F_gravity_P));
F_net_P = F_gravity_P + F_Tether_total_P + F_load_dist_P;
load_vel = [load_u; load_v; load_w];
load_vel_d = cross(load_omega,load_vel) + F_net_P./m_load;
%% Slung Load Attitude
load_att_d = att_der_matrix(load_phi, load_theta) * load_omega;
%% Slung Load Attitude Rate
Tao_net_P = [0; 0; 0];
load_attRate_d = attRate_der(load_Jx,load_Jy,load_Jz,load_p,load_q,load_r,Tao_net_P);
%% 3.3) Add payload derivatives to vector
xdot = [xdot;
load_pos_d;
load_vel_d;
load_att_d;
load_attRate_d];
end
function [xdot] = systemModel(X,U,params)
function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,U,params)
%systemModel Implements the System Model Dynamics for a multicopter with a
%slung load. Computes the derivative of the state given the state
% Inputs:
% x: state (num_vehicles*12 + 12x1)
% U: input (params.numRotors x 1)
% Outputs:
% xdot: state derivative (num_vehicles*12 + 12x1)
% xdot: state derivative (num_vehicles*12 + 12 x 1)
% F_nets: net forces (num_vehicles*3 + 3 x 1)
% Tao_nets: net torques (num_vehicles*3 + 3 x 1)
% F_tethers: tether forces (num_vehicles*3 x 1)
% F_drags: drag forces (num_vehicles*3 + 3 x 1)
% Tether_states: (num_vehicles*2 x 1)
%% Setup output variables
xdot = [];
F_nets = [];
Tao_nets = [];
F_tethers = [];
F_drags = [];
Tether_states = [];
%% 0) Constrain Input
U = min(U,1);
U = max(U,0);
......@@ -39,8 +52,8 @@ function [xdot] = systemModel(X,U,params)
load_vz = load_pos_d(3);
%% 2) Calculate State Derivative for each Vehicle
xdot = [];
F_Tether_total = [0;0;0];
Tao_tether_total_P = [0;0;0];
for i = 1:params.num_vehicles
%% 2.1) Get Constants
% Multirotor Constants
......@@ -103,22 +116,26 @@ function [xdot] = systemModel(X,U,params)
F_tether_mag = max(F_tether_mag,0);
end
% l = sqrt( (load_x-x)^2 + (load_y-y)^2 + (load_z-z)^2 );
% l_dot = ((load_x-x)*(load_vx-pos_d(1)) + (load_y-y)*(load_vy-pos_d(2)) + (load_z-z)*(load_vz-pos_d(3))) / l;
% r_hat = [(load_x-x); (load_y-y); (load_z-z)] ./ l;
% F_Tether_i = (k.*(l - L) + b.*l_dot);
% Constrain Max tension for debugging
F_tether_mag = min(F_tether_mag, params.multirotors{i}.MaxTension);
F_Tether_i = F_tether_mag .* -1 .* t_hat_Pi_G;
% Points in -r_hat direction
% F_Tether_i = F_Tether_i .* -1 .* r_hat;
% Add up tether forces
F_Tether_total = F_Tether_total + F_Tether_i;
% Add up torques as well
F_Tether_i_P = R_P_G' * F_Tether_i;
Tao_tether_i_P = cross(r_Pi_P, F_Tether_i_P);
Tao_tether_total_P = Tao_tether_total_P + Tao_tether_i_P;
%% 2.5) (4-6) Velocity Derivative ([u_d; v_d; w_d])
F_thrust_V = [0; 0; -F];
F_gravity_V = R_V_G' * [0; 0; m*g];
F_tether_V = - R_V_G' * F_Tether_i;
F_drag_V = [0; 0; 0];
F_dist_V = params.multirotors{1}.distSTD .* randn(size(F_gravity_V));
F_net_V = F_thrust_V+ F_gravity_V + F_tether_V + F_dist_V;
vel_d = cross([p;q;r],[u;v;w]) + F_net_V./m_load;
......@@ -130,6 +147,19 @@ function [xdot] = systemModel(X,U,params)
Tao_net_V = [Tao_phi; Tao_theta; Tao_psi];
attRate_d = attRate_der(Jx,Jy,Jz,p,q,r,Tao_net_V);
% Collect Forces and Torques and Tether States
F_nets = [F_nets;
F_net_V];
Tao_nets = [Tao_nets;
Tao_net_V];
F_tethers = [F_tethers;
F_tether_V];
F_drags = [F_drags;
F_drag_V];
Tether_states = [Tether_states;
l_i;
l_dot_i];
%% 2.8) Combine and append to xdot vector
xdot = [xdot;
pos_d;
......@@ -146,6 +176,7 @@ function [xdot] = systemModel(X,U,params)
% Calculate Forces
F_gravity_P = R_P_G' * [0; 0; m_load*g];
F_Tether_total_P = R_P_G' * F_Tether_total;
F_drag_P = [0; 0; 0];
F_load_dist_P = params.payload.distSTD .* randn(size(F_gravity_P));
F_net_P = F_gravity_P + F_Tether_total_P + F_load_dist_P;
......@@ -157,9 +188,17 @@ function [xdot] = systemModel(X,U,params)
load_att_d = att_der_matrix(load_phi, load_theta) * load_omega;
%% Slung Load Attitude Rate
Tao_net_P = [0; 0; 0];
Tao_net_P = Tao_tether_total_P;
load_attRate_d = attRate_der(load_Jx,load_Jy,load_Jz,load_p,load_q,load_r,Tao_net_P);
% Collect Forces and Torques
F_nets = [F_nets;
F_net_P];
Tao_nets = [Tao_nets;
Tao_net_P];
F_drags = [F_drags;
F_drag_P];
%% 3.3) Add payload derivatives to vector
xdot = [xdot;
load_pos_d;
......
function [] = plotData(simData,params)
%plotData
%% Plot Position
figure(1);
subplot(3,1,1);
plot(simData.T,simData.X(:,1));
hold on;
plot(simData.T,simData.X_des(:,1),'r--');
hold off;
ylabel('x (m)');
title('Position');
legend('Actual','Desired');
minYLims(ylim,-1,1);
subplot(3,1,2);
plot(simData.T,simData.X(:,2));
hold on;
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));
hold on;
plot(simData.T,simData.X_des(:,3),'r--');
hold off;
ylabel('z (m)');
xlabel('Time (s)');
legend('Actual','Desired');
minYLims(ylim,-1,1);
movegui(gcf,'northwest');
%% Plot Velocity
figure(2);
subplot(3,1,1);
plot(simData.T,simData.X(:,4));
hold on;
plot(simData.T,simData.X_des(:,4),'r--');
hold off;
ylabel('u (m/s)');
title('Velocity');
legend('Actual','Desired');
minYLims(ylim,-1,1);
subplot(3,1,2);
plot(simData.T,simData.X(:,5));
hold on;
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));
hold on;
plot(simData.T,simData.X_des(:,6),'r--');
hold off;
ylabel('w (m/s)');
xlabel('Time (s)');
legend('Actual','Desired');
minYLims(ylim,-1,1);
movegui(gcf,'northeast');
%% Plot Attitude
figure(3);
subplot(3,1,1);
plot(simData.T,simData.X(:,7));
hold on;
plot(simData.T,simData.X_des(:,7),'r--');
hold off;
ylabel('\phi (rad)');
title('Attitude');
ylim([-pi pi]);
legend('Actual','Desired');
subplot(3,1,2);
plot(simData.T,simData.X(:,8));
hold on;
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));
hold on;
plot(simData.T,simData.X_des(:,9),'r--');
hold off;
ylabel('\psi (rad)');
xlabel('Time (s)');
ylim([-pi pi]);
legend('Actual','Desired');
movegui(gcf,[1,30]);
%% Plot Attitude Rate
figure(4);
subplot(3,1,1);
plot(simData.T,simData.X(:,10));
hold on;
plot(simData.T,simData.X_des(:,10),'r--');
hold off;
ylabel('p (rad/s)');
title('Attitude Rate');
legend('Actual','Desired');
minYLims(ylim,-1,1);
subplot(3,1,2);
plot(simData.T,simData.X(:,11));
hold on;