Commit 322cee41 authored by mmroma's avatar mmroma
Browse files

In process of accelerating code

parent f49affbb
......@@ -2,6 +2,6 @@ function [coarse_WPs] = WPs_set1_hover()
%WPs_set1
h = 1.5;
coarse_WPs = [0, 0, h, 0;
0, 0, h, 20];
0, 0, h, 1];
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
......
function [params] = setUpParameters()
%setUpParameters
% Calls parameters_Quad() and parameters_Payload() with default
% settings and then adjusts from there.
% settings and then adjusts from there. All edits should be made to
% this file.
%% Debugging Parameters (true / false)
params.disable_linear_vehicle_motion = false;
......@@ -12,7 +13,11 @@ function [params] = setUpParameters()
%% Global Parameters
params.g = 9.81;
%% Noise and Distrubance Forces
%% Time Step Parameters
params.ctlDt = 0.05; % Controller Step Size (s)
params.tStep = params.ctlDt / 10; % Simulation Step Size (s)
%% Noise and Disturbance Forces
quad_dist_STD = 0; %0.1; % 0-mean White Gaussian Quad Disturbance
load_dist_STD = 0; %0.1; % 0-mean White Gaussian Load Disturbance
......@@ -54,6 +59,7 @@ function [params] = setUpParameters()
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
params.multirotors{i}.distSTD = quad_dist_STD;
params.l_i_last{i} = L_all;
params.multirotors{i}.ctlDt = params.ctlDt;
end
%% Set Wind Parameters
......@@ -78,7 +84,6 @@ function [params] = setUpParameters()
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.001; % Simulation Step Size (s)
params.anim_FPS = 30; % Animation FPS
%% Set Guidamce
......
function [F_drag_P] = calc_drag_force_payload(X_payload, params_global)
function [F_drag_P] = calc_drag_force_payload(X, params_global)
%calc_drag_force_payload
%% 0) Init
X = X_struct_readable(X_payload);
R_P_G = rpy_to_R(X.phi, X.theta, X.psi);
R_P_G = X.R_B_G;
%% 1)
......
function [F_Drag] = calc_drag_force_vehicle(X_vehicle, Thrust_Force_Mag, params_global)
function [F_Drag] = calc_drag_force_vehicle(X, Thrust_Force_Mag, params_global)
%calc_drag_force_vehicle
% Calculates the drag force on the vehicle in the vehicle frame
% Inputs:
......@@ -8,8 +8,7 @@ function [F_Drag] = calc_drag_force_vehicle(X_vehicle, Thrust_Force_Mag, params_
% F_Drag: Drag Force in the Vehicle frame (3x1)
%% 0) Init
X = X_struct_readable(X_vehicle);
R_V_G = rpy_to_R(X.phi, X.theta, X.psi);
R_V_G = X.R_B_G;
%% 1) Calculate Drag Force on Quadrotor
V_W_Vi_rel_Vi = R_V_G' * params_global.wind.R_W_G * [params_global.wind.speed; 0; 0] - [X.u;X.v;X.w];
......
function [F_gravity_B] = calc_gravity_force(X_body, params)
function [F_gravity_B] = calc_gravity_force(X, params)
%calc_gravity_force
%% 0) Init
X = X_struct_readable(X_body);
R_B_G = rpy_to_R(X.phi, X.theta, X.psi);
%% 1) Calc
F_gravity_B = R_B_G' * [0; 0; params.m*params.g];
end
function [F_tether, F_tether_V, l_i, l_dot_i] = calc_tether_force(X_vehicle, params_vehicle, X_payload)
function [F_tether, F_tether_V, l_i, l_dot_i] = calc_tether_force(Xv, params_vehicle, Xp)
%calc_tether_force
% Calculates the the tether force in the ground frame for the given
% vehicle's tether
% Inputs:
% X_vehicle: vehicle state (12x1)
% X_vehicle: vehicle state struct
% params_vehicle: struct of parameters for vehicle
% X_payload: payload state (12x1)
% X_payload: payload state struct (12x1)
% Outputs:
% F_tether: tether force in ground frame (3x1)
%% 0) Init
Xv = X_struct_readable(X_vehicle);
Xp = X_struct_readable(X_payload);
R_P_G = rpy_to_R(Xp.phi, Xp.theta, Xp.psi);
R_V_G = rpy_to_R(Xv.phi, Xv.theta, Xv.psi);
R_P_G = Xp.R_B_G;
R_V_G = Xv.R_B_G;
%% 1) Calculate Transformations
r_Vi_G = [Xv.x;Xv.y;Xv.z];
......
function [F_Tethers_P, Tether_Torques_P] = calc_tether_force_torque_payload_frame(X_Payload, params_global, F_Tether_G)
function [F_Tethers_P, Tether_Torques_P] = calc_tether_force_torque_payload_frame(X, params_global, F_Tether_G)
%calc_tether_force_torque_payload_frame
%% 0) Init
X = X_struct_readable(X_Payload);
R_P_G = rpy_to_R(X.phi, X.theta, X.psi);
R_P_G = X.R_B_G;
F_Tethers_P = [0; 0; 0];
Tether_Torques_P = [0; 0; 0];
......
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
......@@ -12,5 +12,12 @@ function [X_struct] = X_struct_readable(X)
X_struct.p = X(10);
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
function [X_dot] = rigid_body_dynamics(X_body, params, Net_Force, Net_Torque)
function [X_dot] = rigid_body_dynamics(X, params, Net_Force, Net_Torque)
%rigid_body_dynamics
% Calculates the state derivative for a generic rigid body given the
% current state, parameters, net force, and net torque
......@@ -10,12 +10,9 @@ function [X_dot] = rigid_body_dynamics(X_body, params, Net_Force, Net_Torque)
% Outputs:
% X_dot: state derivative (12x1)
%% Initialize state by converting the vector to a struct
X = X_struct_readable(X_body);
%% (1-3) pos_d
R_B_G = rpy_to_R(X.phi, X.theta, X.psi);
pos_d = R_B_G * [X.u; X.v; X.w];
pos_d = X.R_B_G * [X.u; X.v; X.w];
%% (4-6) Velocity Derivative ([u_d; v_d; w_d])
vel_d = -cross([X.p;X.q;X.r],[X.u;X.v;X.w]) + Net_Force./params.m;
......
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,29 +8,32 @@ function [simData] = runSimulation(params)
% 1) Initialization %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 1.1) Initialize Current State based on parameters files
X_curr = [];
for i = 1:params.num_vehicles
X_curr = [X_curr; params.multirotors{i}.x0];
end
X_curr = [X_curr; params.payload.x0];
%% 1.2) Calculate variable sizes based on parameters
num_tsteps = length(0:params.multirotors{1}.ctlDt:params.tFinal);
num_states = length(X_curr);
%% 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.3) Initialize Logging Variables to hold the data
T = [];
X = zeros(num_tsteps, num_states);
X_des = zeros(num_tsteps, (params.num_vehicles +1) * 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);
%% 1.2) Initialize Variables with proper sizes
X_curr = zeros(num_states,1);
X_des_curr = zeros(num_states,1);
U_curr = zeros(num_inputs,1);
T = zeros(1,num_tsteps);
X = zeros(num_states, num_tsteps);
X_des = zeros((params.num_vehicles +1) * 12, num_tsteps);
U = zeros(num_inputs, num_tsteps);
F_nets = zeros((params.num_vehicles+1) * 3, num_tsteps);
Tao_nets = zeros((params.num_vehicles+1) * 3, num_tsteps);
F_tethers = zeros((params.num_vehicles) * 3, num_tsteps);
F_drags = zeros((params.num_vehicles+1) * 3, num_tsteps);
Tether_states = zeros((params.num_vehicles) * 2, num_tsteps);
%% 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));
......@@ -50,59 +53,47 @@ function [simData] = runSimulation(params)
% 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
U_curr = [];
X_des_curr = [];
%% 2.3) Compute the Payload Augmented Desired State Control
%% 2.2) 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
%% 2.3) 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_];
U_curr(i*4-3:i*4) = U_curr_;
X_des_curr(i*12-11:i*12) = X_des_curr_;
end
%% 2.5) Constrain Control Input
%% 2.4) 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.5) Record payload desired state
X_des_curr(params.num_vehicles*12+1:end) = params.payload.xd(tstep_count,:)';
%% 2.7) Record Essential Simulation Data
T = [T; t_curr];
X(tstep_count,:) = X_curr';
U(tstep_count,:) = U_curr';
X_des(tstep_count,:) = X_des_curr';
%% 2.6) 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
%% 2.7) 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
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 ode45
%% 2.8) 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
%% 2.9) Increment the time step count
tstep_count = tstep_count + 1;
end
......
......@@ -19,19 +19,19 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 1.1) Setup output variables
xdot = [];
F_nets = [];
Tao_nets = [];
F_tethers = [];
F_drags = [];
Tether_states = [];
xdot = zeros(params.num_vehicles*12+12,1);
F_nets = zeros(params.num_vehicles*3+3,1);
Tao_nets = zeros(params.num_vehicles*3+3,1);
F_tethers = zeros(params.num_vehicles*3,1);
F_drags = zeros(params.num_vehicles*3+3,1);
Tether_states = zeros(params.num_vehicles*2,1);
%% 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);
X_payload = X_struct_readable(X(params.num_vehicles*12+1:params.num_vehicles*12+12));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
......@@ -42,10 +42,8 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
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);
X_vehicle = X_struct_readable(X((i-1)*12+1:i*12));
U_vehicle = U((i-1)*params.multirotors{i}.numRotors+1:i*params.multirotors{i}.numRotors);
%% 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});
......@@ -57,38 +55,31 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
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});
F_gravity_V = X_vehicle.R_B_G' * [0; 0; params.multirotors{i}.m*params.multirotors{i}.g];
%% 2.6) Calculate Disturbance Force (in the vehicle frame)
F_dist_V = calc_disturbance_force(params.multirotors{1}.distSTD);
F_dist_V = params.multirotors{1}.distSTD .* randn(size([0;0;0]));
%% 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
% 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
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];
F_nets(i*3-2:i*3) = Net_Force;
Tao_nets(i*3-2:i*3) = Net_Torque;
F_tethers(i*3-2:i*3) = F_Tether_V;
F_drags(i*3-2:i*3) = F_Drag_V;
Tether_states(i*2-1:i*2) = [l_i; l_dot_i];
xdot(i*12-11:i*12) = X_vehicle_dot;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
......@@ -101,10 +92,10 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
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);
F_Gravity_P = X_payload.R_B_G' * [0; 0; params.payload.m*params.payload.g];
%% 3.3) Calculate Disturbance Force (in the payload frame)
F_Dist_P = calc_disturbance_force(params.payload.distSTD );
F_Dist_P = params.payload.distSTD .* randn(size([0;0;0]));
%% 3.4) Calculate Tether Forces and Torques in Payload Frame
[F_Tethers_P, Tether_Torques_P] = calc_tether_force_torque_payload_frame(X_payload, params, F_Tether_G);
......@@ -113,24 +104,20 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
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
% 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
F_nets = [F_nets;
Net_Force];
Tao_nets = [Tao_nets;
Net_Torque];
F_drags = [F_drags;
F_Drag_P];
xdot = [xdot;
X_payload_dot];
F_nets(params.num_vehicles*3+1:end) = Net_Force;
Tao_nets(params.num_vehicles*3+1:end) = Net_Torque;
F_drags(params.num_vehicles*3+1:end) = F_Drag_P;
xdot(params.num_vehicles*12+1:end) = X_payload_dot;