Commit cc367e8b authored by mmroma's avatar mmroma
Browse files

In progress of debugging.

parent 1d28ac58
......@@ -2,6 +2,6 @@ function [coarse_WPs] = WPs_set1_hover()
%WPs_set1
h = 1.5;
coarse_WPs = [0, 0, h, 0;
0, 0, h, 10];
0, 0, h, 20];
end
......@@ -2,8 +2,8 @@ function [Xd, T] = gen_payload_traj(params)
%gen_payload_traj
%% 1) Design Coarse Waypoints [x, y, z, t] in meters and sec
% coarse_WPs = WPs_set1_hover();
coarse_WPs = WPs_set2_square();
coarse_WPs = WPs_set1_hover();
% coarse_WPs = WPs_set2_square();
% coarse_WPs = WPs_set3_rise_and_fall();
%% 2) Extract Relevant Params
......
......@@ -32,7 +32,7 @@ function [params] = parameters_Quad(global_params)
params.controller = @(X,params) fullstate_lqr_intAct(X,params);
% Controller Rate (s)
params.ctlDt = 0.005;
params.ctlDt = 0.01;
%% Initial Conditions
params.x0 = [0;... % x position [m]
......
function [params] = setUpParameters()
%setUpParameters
% Wind Added and a 5th quad (cube)
% Calls parameters_Quad() and parameters_Payload() with default
% settings and then adjusts from there.
%% Debugging Parameters (true / false)
params.disable_linear_vehicle_motion = false;
params.disable_angular_vehicle_motion = false;
params.disable_linear_payload_motion = false;
params.disable_angular_payload_motion = false;
%% Global Parameters
params.g = 9.81;
......@@ -9,21 +16,20 @@ function [params] = setUpParameters()
quad_dist_STD = 0; %0.1; % 0-mean White Gaussian Quad Disturbance
load_dist_STD = 0; %0.1; % 0-mean White Gaussian Load Disturbance
% 0-mean White Gaussian Noise (Sensor Noise) on each vehicle's state
% std = 0.001;
%% Sensor Noise on Vehicle States (0-mean White Gaussian Noise)
std = 0;
params.noiseSTD = [std;std;std;
0;0;0;
0;0;0;
0;0;0];
%% Load Payload (sphere in this case)
%% Load Payload Parameters
params.payload = parameters_Payload(params);
params.payload.distSTD = load_dist_STD;
params.payload.m = 0.5;
r = params.payload.diameter / 2;
% r = 0;
%% Adjust Payload Controller Parameters
int_limit = 100;
params.e_load_pos_Kp = 0;
params.e_load_pos_Ki = 0;
......@@ -50,7 +56,7 @@ function [params] = setUpParameters()
params.l_i_last{i} = L_all;
end
%% Wind Parameters
%% Set Wind Parameters
params.wind.enable = 0;
params.wind.speed = 0; % (m/s)
params.wind.yaw = 0; % (rad). 0 = +x, pi/2 = +y
......@@ -58,8 +64,10 @@ function [params] = setUpParameters()
params.wind.rho_f = 1.225; % (kg/m^3)
params.wind.mu = 1.802e-5; % (kg/ms)
params.wind.load_d = params.payload.diameter; % m
params.wind.C_D = @(Re) C_D_sphere(Re); % Sphere Drag Model
% params.wind.C_D = @(Re) C_D_cube(Re); % Cube Drag Model
c_bar_d = 0.04; % Quad Lumped Drag
params.wind.C_bar_d = [c_bar_d, 0, 0;
0, c_bar_d, 0;
......@@ -70,33 +78,16 @@ function [params] = setUpParameters()
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.005; % Simulation Step Size (s)
params.tStep = 0.0001; % Simulation Step Size (s)
params.anim_FPS = 30; % Animation FPS
%% Guidance (version 1)
% [params.payload.xd, params.tFinal] = gen_payload_traj(params);
% params.payload.x0 = params.payload.xd(1,:)';
% for i=1:params.num_vehicles
% params.multirotors{i}.xd = compute_quad_guidance(params,i);
% params.multirotors{i}.x0 = params.multirotors{i}.xd(1,:)';
% end
%
% params.multirotors{5}.L = L_all - 0.6;
%% Guidamce (version 2)
%% Set Guidamce
[params.payload.xd, params.tFinal] = gen_payload_traj(params);
params.payload.x0 = params.payload.xd(1,:)';
params = compute_quad_guidance_2(params);
%% Control (new stuff)
%% Adjust Quad Control Based on Payload Trajectory
params = compute_quad_eq_state_and_controller(params);
%% Sanity check guidance
% plot_guidance(params);
%% Perturb Load Off Equilibrium
params.payload.x0(1:2) = [0; 0];
end
......@@ -92,7 +92,7 @@ function [simData] = runSimulation(params)
%% 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);
% X_curr = constrain_euler_angles(X_curr);
end
%% 2.10) Increment the time step count
......
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 + 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)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% 1) Initialization %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 1.1) Setup output variables
xdot = [];
F_nets = [];
Tao_nets = [];
F_tethers = [];
F_drags = [];
Tether_states = [];
%% 1.2) Constrain Input
U = min(U,1);
U = max(U,0);
%% 1.3) Rename Payload State for convenience
X_payload = X(params.num_vehicles*12+1:params.num_vehicles*12+12);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% 2) Calculate Dynamics for each Vehicle and Tether %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
F_Tether_G = cell(1,params.num_vehicles);
for i = 1:params.num_vehicles
%% 2.1) Grab Vehicle State and Input
X_vehicle = X((i-1)*12+1:i*12);
u0 = (i-1)*params.multirotors{i}.numRotors + 1;
un = i*params.multirotors{i}.numRotors;
U_vehicle = U(u0:un);
%% 2.2) Calculate Vehicle's Thrust and Torque from Propellers (in the vehicle frame)
[F_Thrust_V, Thrust_Force_Mag, Thrust_Torque_V] = calc_propeller_force_torque(U_vehicle, params.multirotors{i});
%% 2.3) Calculate Tether Force (in the ground and vehicle frame)
[F_Tether_G{i}, F_Tether_V, l_i, l_dot_i] = calc_tether_force(X_vehicle, params.multirotors{i}, X_payload);
%% 2.4) Calculate Drag Force (in the vehicle frame)
F_Drag_V = calc_drag_force_vehicle(X_vehicle, Thrust_Force_Mag, params);
%% 2.5) Calculate Gravity Force (in the vehicle frame)
F_gravity_V = calc_gravity_force(X_vehicle, params.multirotors{i});
%% 2.6) Calculate Disturbance Force (in the vehicle frame)
F_dist_V = calc_disturbance_force(params.multirotors{1}.distSTD);
%% 2.7) Calculate Vehicle's Rigid Body Dynamics
Net_Force = F_Thrust_V + F_Tether_V + F_Drag_V + F_gravity_V + F_dist_V;
Net_Torque = Thrust_Torque_V;
[X_vehicle_dot] = rigid_body_dynamics(X_vehicle, Net_Force, Net_Torque);
%% 2.8) Collect Forces, Torques, Tether States, and xdot
F_nets = [F_nets;
Net_Force];
Tao_nets = [Tao_nets;
Net_Torque];
F_tethers = [F_tethers;
F_Tether_V];
F_drags = [F_drags;
F_Drag_V];
Tether_states = [Tether_states;
l_i;
l_dot_i];
xdot = [xdot;
X_vehicle_dot];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% 3) Calculate Dynamics for the payload %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 3.1) Calculate Drag Force on Payload
F_Drag_P = calc_drag_force_payload(X_payload, params);
%% 3.2) Calculate Gravity Force on Payload
F_Gravity_P = calc_gravity_force(X_payload, params.payload);
%% 3.3) Calculate Disturbance Force (in the payload frame)
F_Dist_P = calc_disturbance_force(params.payload.distSTD );
%% 3.4) Add up forces and Torques
% Add up torques as well
F_Tether_i_P = R_P_G' * F_Tether_i;
% Tao_tether_i_P = R_P_G' * cross(R_P_G * r_Pi_P, 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;
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;
%% 3.4) Calculate Tether Forces and Torques in Payload Frame
[F_Tethers_P, Tether_Torques_P] = calc_tether_force_torque_payload_frame
%% 3.4) Calculate Payload's Rigid Body Dynamics
Net_Force = F_Tethers_P + F_Drag_P + F_Gravity_P + F_Dist_P;
Net_Torque =
[X_payload_dot] = rigid_body_dynamics(X_payload, Net_Force, Net_Torque);
%% 3.5) Collect Forces, Torques, and xdot
F_nets = [F_nets;
F_net_P];
Tao_nets = [Tao_nets;
Tao_net_P];
F_drags = [F_drags;
F_drag_P];
xdot = [xdot;
load_pos_d;
load_vel_d;
load_att_d;
load_attRate_d];
end
......@@ -65,6 +65,14 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
%% 2.7) Calculate Vehicle's Rigid Body Dynamics
Net_Force = F_Thrust_V + F_Tether_V + F_Drag_V + F_gravity_V + F_dist_V;
Net_Torque = Thrust_Torque_V;
if (params.disable_linear_vehicle_motion)
Net_Force = [0;0;0];
end
if (params.disable_angular_vehicle_motion)
Net_Torque = [0;0;0];
end
[X_vehicle_dot] = rigid_body_dynamics(X_vehicle, params.multirotors{i}, Net_Force, Net_Torque);
%% 2.8) Collect Forces, Torques, Tether States, and xdot
......@@ -104,6 +112,14 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
%% 3.5) Calculate Payload's Rigid Body Dynamics
Net_Force = F_Tethers_P + F_Drag_P + F_Gravity_P + F_Dist_P;
Net_Torque = Tether_Torques_P;
if (params.disable_linear_payload_motion)
Net_Force = [0;0;0];
end
if (params.disable_angular_payload_motion)
Net_Torque = [0;0;0];
end
[X_payload_dot] = rigid_body_dynamics(X_payload, params.payload, Net_Force, Net_Torque);
%% 3.6) Collect Forces, Torques, and xdot
......
......@@ -11,7 +11,7 @@ function [] = plotData(simData,params)
%% Plot!
% params.fig_num = plotData_quadrotor(simData,params);
params.fig_num = plotData_payload(simData,params);
params.fig_num = plotData_tethers(simData,params);
% params.fig_num = plotData_tethers(simData,params);
% params.fig_num = plotData_drag(simData,params);
end
......
......@@ -15,7 +15,7 @@ function [fig_num] = plotData_payload(simData,params)
hold off;
legend('Actual','Desired');
ylabel('x (m)');
minYLims(ylim,-1,1);
% minYLims(ylim,-1,1);
title('Load Position');
subplot(3,1,2);
......@@ -25,7 +25,7 @@ function [fig_num] = plotData_payload(simData,params)
hold off;
legend('Actual','Desired');
ylabel('y (m)');
minYLims(ylim,-1,1);
% minYLims(ylim,-1,1);
subplot(3,1,3);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 3));
......@@ -35,11 +35,51 @@ function [fig_num] = plotData_payload(simData,params)
legend('Actual','Desired');
ylabel('z (m)');
xlabel('Time (s)');
minYLims(ylim,-1,1);
% minYLims(ylim,-1,1);
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);
movegui(gcf,'northwest');
movegui(gcf,'onscreen');
%% Plot Velocity of Load
figure(fig_num);
subplot(3,1,1);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 4));
hold on;
plot(simData.T,simData.X_des(:,params.num_vehicles*12 + 4),'r--');
hold off;
legend('Actual','Desired');
ylabel('u (m/s)');
% minYLims(ylim,-1,1);
title('Load Velocity');
subplot(3,1,2);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 5));
hold on;
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));
hold on;
plot(simData.T,simData.X_des(:,params.num_vehicles*12 + 6),'r--');
hold off;
legend('Actual','Desired');
ylabel('w (m/s)');
xlabel('Time (s)');
% minYLims(ylim,-1,1);
fig_num = save_current_figure(fig_prefix, fig_num, save_figs_bool);
movegui(gcf,'north');
movegui(gcf,'onscreen');
%% Plot Attitude of Load
......@@ -64,18 +104,18 @@ function [fig_num] = plotData_payload(simData,params)
subplot(3,1,1);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 7));
ylabel('\phi (rad)');
ylim([-pi pi]);
% ylim([-pi pi]);
title('Load Attitude');
subplot(3,1,2);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 8));
ylabel('\theta (rad)');
ylim([-pi pi]);
% ylim([-pi pi]);
subplot(3,1,3);
plot(simData.T,simData.X(:,params.num_vehicles*12 + 9));
ylabel('\psi (rad)');
ylim([-pi pi]);
% ylim([-pi pi]);
fig_num = save_current_figure(fig_prefix, fig_num, save_figs_bool);
......
......@@ -19,5 +19,5 @@ simData = runSimulation(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