Commit 9b01ca3d authored by mmroma's avatar mmroma
Browse files

Save point before I begin trying to get the rc_pilot controller to work again.

parent 4af3153d
%
M = [-0.25, -0.25, -0.25, -0.25;
-0.5, 0.5, 0.5, -0.5;
0.5, -0.5, 0.5, -0.5;
0.5, 0.5, -0.5, -0.5];
%%
M_inv = inv(M);
\ No newline at end of file
%%
fs = 200; % Sampling frequency [Hz]
fc = 0.008*fs; % cutoff frequency [Hz] (< 0.5 fs)
Wn = 2*pi*fc/fs; % Normalized cutoff frequency
%% 1st order butterworth lpf
[b1,a1] = butter(1,Wn / pi);
figure(1);
freqz(b1,a1);
title('1st order butterworth');
%% Exponential Averager
Wn = 2*pi*fc/fs; % Normalized cutoff frequency
alpha = 2 - cos(Wn) - sqrt(cos(Wn)^2 - 4*cos(Wn) + 3);
% alpha = 1 - exp(-Wn);
% alpha = 0.75;
%%
b2 = (1-alpha);
a2 = [1 -alpha];
figure(2);
freqz(b2,a2);
title('1st order Exponential Averager');
\ No newline at end of file
function [f] = plotHapticForceEstimate(data, location,lw,fs)
function [f] = plotHapticForceEstimate(data, params, location,lw,fs)
%plotHapticForceEstimate
%% 4) Plot Results
f = figure();
dims = {'X','Y','Z'};
for i=1:3
subplot(3,1,i);
plot(data.tout,data.F_user_est(:,i),'LineWidth',lw);
% plot(data.tout,data.F_user_est(:,i),'LineWidth',lw);
plot(0:params.ctlDt:params.tFinal,data.F_user_est(:,i),'LineWidth',lw);
hold on;
plot(data.tout,data.F_user(:,i),'LineWidth',lw);
% plot(data.tout,data.F_user_est_unfilt(:,i),'--');
......
......@@ -7,7 +7,7 @@ function [params] = compute_quad_guidance_2(params)
%% Init
eps = 0.5; % Load distribution parameter
eps = 1 / (2*sqrt(2) + 1); % Load distribution parameter
n = params.num_vehicles;
......
......@@ -4,7 +4,7 @@ function [params] = parameters_Payload(global_params)
%%
params.g = global_params.g;% Acc. Due to Gravity (m/s^2)
params.m = 1; % Point Mass (kg)
params.m = 1.211; % Point Mass (kg)
params.diameter = .246; % Diameter of sphere
%
......
......@@ -77,7 +77,7 @@ function [params] = parameters_Quad(global_params)
params.L = 0.5; % Unstretched Length (m)
params.k = 20; % Spring Constant
params.b = 1; % Damping Coefficient
params.NegTension = 1; % Allow "Negative Tensions"? 0 = No, 1 = Yes
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;
......
......@@ -18,37 +18,38 @@ function [params] = setUpParameters(traj_num)
params.curr_run = 1;
%% Time Step Parameters
params.ctlDt = 0.01; % Controller Step Size (s)
params.ctlDt = 0.005; % Controller Step Size (s)
params.tStep = params.ctlDt / 10; % Simulation Step Size (s)
params.ctlFs = 1 / params.ctlDt; % Controller Frequency
%% Disturbance Forces & Torques
quads_dist_F_power = 0;
% quads_dist_F_power = 1e-3;
% quads_dist_F_power = 0;
quads_dist_F_power = 1e-5;
quads_dist_M_power = 0;
load_dist_F_power = 0;
% load_dist_F_power = 1e-3;
% load_dist_F_power = 0;
load_dist_F_power = 1e-5;
load_dist_M_power = 0;
%% Sensor Noise
% quad_noise_power = [ones(3,1).* 1e-8;
% ones(3,1).* 1e-6;
% ones(3,1).* 1e-6;
% ones(3,1).* 1e-6];
%
% load_noise_power = [ones(3,1).* 1e-8;
% ones(3,1).* 1e-6;
% ones(3,1).* 1e-6;
% ones(3,1).* 1e-6];
quad_noise_power = [ones(3,1).* 0;
ones(3,1).* 0;
ones(3,1).* 0;
ones(3,1).* 0];
quad_noise_power = [ones(3,1).* 1e-8;
ones(3,1).* 1e-8;
ones(3,1).* 1e-8;
ones(3,1).* 1e-8];
load_noise_power = [ones(3,1).* 0;
ones(3,1).* 0;
ones(3,1).* 0;
ones(3,1).* 0];
load_noise_power = [ones(3,1).* 1e-8;
ones(3,1).* 1e-8;
ones(3,1).* 1e-8;
ones(3,1).* 1e-8];
% quad_noise_power = [ones(3,1).* 0;
% ones(3,1).* 0;
% ones(3,1).* 0;
% ones(3,1).* 0];
%
% load_noise_power = [ones(3,1).* 0;
% ones(3,1).* 0;
% ones(3,1).* 0;
% ones(3,1).* 0];
%% Load Payload Parameters
params.payload = parameters_Payload(params);
......@@ -57,11 +58,13 @@ function [params] = setUpParameters(traj_num)
params.payload.dist_F_power = load_dist_F_power;
params.payload.dist_M_power = load_dist_M_power;
params.payload.noise_power = load_noise_power;
params.payload.m = 0.5;
params.payload.m = 1.2;
r = params.payload.diameter / 2;
%% Adjust Payload Controller Parameters
params.e_load_pos_Kp = [diag([0.7, 0.7, 0.7]), diag([0.2, 0.2, 0.2])];
params.e_load_pos_Ki = [diag([0.05, 0.05, 0.05]), diag([0.0, 0.0, 0.0])];
% params.e_load_pos_Kp = [diag([2, 2, 2]), diag([0.7, 0.7, 0.7])];
% params.e_load_pos_Kp = [diag([1, 1, 1]), diag([0.1, 0.1, 0.1])];
......@@ -97,7 +100,7 @@ function [params] = setUpParameters(traj_num)
end
%% Set Wind Parameters
params.wind.enable = 0;
params.wind.enable = 1;
params.wind.speed = 0; % (m/s)
params.wind.yaw = 0; % (rad). 0 = +x, pi/2 = +y
......@@ -128,19 +131,26 @@ function [params] = setUpParameters(traj_num)
%% User Force Input
params.user_force_method = 7;%6;
params.user_force_amplitude = 0.5; % Newton's
params.user_force_amplitude = 2.5; % Newton's
params.user_force = user_force_input(params);
%% Haptic Force Estimation
params.admittance_controller.acc_noise_power = ones(3,1).* 1e-4;
params.admittance_controller.acc_noise_power = ones(3,1).* 1e-4;
% params.admittance_controller.acc_noise_power = ones(3,1).* 0;
params.admittance_controller.acc_noise_seed = [477;478;479];
params.admittance_controller.lpf_cutoff_freq = 2*pi*5;
lpf_fc = 2; % Hz
Wn = 2*pi*lpf_fc/params.ctlFs; % Normalized cutoff frequency
alpha = 2 - cos(Wn) - sqrt(cos(Wn)^2 - 4*cos(Wn) + 3);
params.admittance_controller.lpf_fc = lpf_fc;
params.admittance_controller.Wn = Wn;
params.admittance_controller.alpha = alpha;
%% Admittance Controller
params.admittance_controller.en = 1; % 0 = False, 1 = True
params.admittance_controller.c = 5;
params.admittance_controller.m = 0.1;
params.admittance_controller.turn_on_force = 0.1;
params.admittance_controller.c = 10;
params.admittance_controller.m = 1.211;
params.admittance_controller.turn_on_force = 1.5;
......
function [q] = drawPayload_sphere(ax,r,c)
%drawPayload
[X, Y, Z] = sphere();
q_(1) = surf(X.*r,Y.*r,Z.*r,'FaceColor',c,'EdgeColor',c);
% Create Elements
% q_(1) = patch([qw/2 qw/2 -qw/2 -qw/2],...
% [qw/2 -qw/2 -qw/2 qw/2],...
% [qh/2 qh/2 qh/2 qh/2],...
% payloadColor);
% q_(2) = patch([qw/2 qw/2 -qw/2 -qw/2],...
% [qw/2 -qw/2 -qw/2 qw/2],...
% [-qh/2 -qh/2 -qh/2 -qh/2],...
% payloadColor);
% q_(3) = patch([qw/2 qw/2 qw/2 qw/2],...
% [qw/2 -qw/2 -qw/2 qw/2],...
% [qh/2 qh/2 -qh/2 -qh/2],...
% payloadColor);
% q_(4) = patch([-qw/2 -qw/2 -qw/2 -qw/2],...
% [qw/2 -qw/2 -qw/2 qw/2],...
% [qh/2 qh/2 -qh/2 -qh/2],...
% payloadColor);
% q_(5) = patch([qw/2 -qw/2 -qw/2 qw/2],...
% [qw/2 qw/2 qw/2 qw/2],...
% [qh/2 qh/2 -qh/2 -qh/2],...
% payloadColor);
% q_(6) = patch([qw/2 -qw/2 -qw/2 qw/2],...
% [-qw/2 -qw/2 -qw/2 -qw/2],...
% [qh/2 qh/2 -qh/2 -qh/2],...
% payloadColor);
% Create Parent
q = hgtransform('Parent',ax);
set(q_,'Parent',q)
end
......@@ -105,8 +105,9 @@ function [] = animate_simulink_data(simData, params, using_load)
%% Draw Payload
pw = 0.05;
ph = 0.05;
payloadColor = 'r';
p = drawPayload(ax,pw,ph,payloadColor);
payloadColor = 'k';
% p = drawPayload(ax,pw,ph,payloadColor);
p = drawPayload_sphere(ax,params.payload.diameter/2,payloadColor);
payload_T = eye(4);
set(p,'Matrix',payload_T);
end
......
function [] = paper_plot_admittance_control(d)
function [] = paper_plot_admittance_control(d,p)
%PAPER_PLOT_ADMITTANCE_CONTROL
lw = 2; %line width
......@@ -8,7 +8,8 @@ function [] = paper_plot_admittance_control(d)
f = plot_position(d.tout, d.payload_X, d.Xd_load,'','northwest',lw,fs);
% Adjust ylims
f.Children(6).YLim = [-0.1, 0.6]; %x
f.Children(6).YLim = [-0.1, 1.2]; %x
f.Children(6).XLim = [0, 10]; %x
f.Children(4).YLim = [-0.1, 0.1]; %y
f.Children(2).YLim = [1.4, 1.6]; %z
% extra_ylims = [-0.05, 0.05];
......@@ -28,9 +29,9 @@ function [] = paper_plot_admittance_control(d)
f = plot_velocity(d.tout, d.payload_X, d.Xd_load,'','northeast',lw,fs);
% Adjust ylims
f.Children(6).YLim = [-0.1, 0.2]; %x
f.Children(6).YLim = [-0.2, 0.4]; %x
f.Children(4).YLim = [-0.1, 0.1]; %y
f.Children(2).YLim = [-0.1, 0.1]; %z
f.Children(2).YLim = [-0.1, 0.2]; %z
% Adjust font size and boldness
......@@ -41,12 +42,12 @@ function [] = paper_plot_admittance_control(d)
saveas(f,'Plotting/saved_plots/sim_payload_velocity.eps')
%% 3) Haptic Force Estimate
f = plotHapticForceEstimate(d,'southwest',lw,fs);
f = plotHapticForceEstimate(d,p,'southwest',lw,fs);
% Adjust ylims
f.Children(6).YLim = [-0.1, 0.6]; %x
f.Children(4).YLim = [-0.1, 0.1]; %y
f.Children(2).YLim = [-0.1, 0.1]; %z
f.Children(6).YLim = [-0.2, 2.7]; %x
f.Children(4).YLim = [-1, 1]; %y
f.Children(2).YLim = [-1, 1]; %z
% Adjust font size and boldness
......@@ -56,5 +57,13 @@ function [] = paper_plot_admittance_control(d)
saveas(f,'Plotting/saved_plots/sim_haptic_force_estimate.png')
saveas(f,'Plotting/saved_plots/sim_haptic_force_estimate.eps')
%% 4) 3D System
limits = [2.0, -0.7, 1.1, -1.1, 1.7, -0.5];
f = plotData_3d(d,p,limits);
% Save the plot
saveas(f,'Plotting/saved_plots/sim_3d_system_view.png')
saveas(f,'Plotting/saved_plots/sim_3d_system_view.eps')
end
function [fig_num] = plotData_3d(simData,params)
%plotData_3d
fig_prefix = strcat(params.fig_prefix,"payload_");
fig_num = params.fig_num;
save_figs_bool = params.save_figs_bool;
function [f] = plotData_3d(simData, params, limits)
%animate Animates the multirotor
% Inputs:
% simData
% Outputs:
% Animation Plot
%% 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
%% Set Up limits
cubeSideLength = 3;
%% 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;
% xUlim = cubeSideLength;
% xLlim = -cubeSideLength;
xUlim = limits(1);
xLlim = limits(2);
% Y Graph Limtis
yUlim = cubeSideLength;
yLlim = -cubeSideLength;
% yUlim = cubeSideLength;
% yLlim = -cubeSideLength;
yUlim = limits(3);
yLlim = limits(4);
% Z Graph Limits
zUlim = cubeSideLength;
zLlim = -cubeSideLength;
%% Plot X, Y
figure(fig_num);
plot(simData.X(params.num_vehicles*12 + 1,:), simData.X(params.num_vehicles*12 + 2,:));
axis([xLlim xUlim yLlim yUlim]);
ax = gca;
xlabel('x (m)');
ylabel('y (m)');
title('Load Position (in terms of X, Y)');
set(gca, 'Ydir', 'reverse');
grid on;
% zUlim = cubeSideLength;
% zLlim = -cubeSideLength;
zUlim = limits(5);
zLlim = limits(6);
%fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
movegui(gcf,'southwest');
movegui(gcf,'onscreen');
%% Plot Y,Z
figure(fig_num);
plot(simData.X(params.num_vehicles*12 + 2,:), simData.X(params.num_vehicles*12 + 3,:));
axis([yLlim yUlim zLlim zUlim]);
%% Set up the graph
f = figure(100); clf;
axis([xLlim xUlim yLlim yUlim zLlim zUlim]);
ax = gca;
xlabel('y (m)');
ylabel('z (m)');
title('Load Position (in terms of Y, Z)');
set(gca, 'Xdir', 'reverse');
xlabel('X');
ylabel('Y');
zlabel('Z');
% Flip Z Axis and Y Axis
set(gca, 'Zdir', 'reverse');
set(gca, 'Ydir', 'reverse');
grid on;
%fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
movegui(gcf,'north');
movegui(gcf,'southeast');
movegui(gcf,'onscreen');
%% 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);
%% Plot X,Z
figure(fig_num);
plot(simData.X(params.num_vehicles*12 + 1,:), simData.X(params.num_vehicles*12 + 3,:));
axis([xLlim xUlim zLlim zUlim]);
ax = gca;
for quad = 1 : params.num_vehicles
mr{quad} = drawQuad(ax,qw,qh,quadFrontColor,quadBackColor);
set(mr{quad},'Matrix',multiRotor_T);
end
xlabel('x (m)');
ylabel('z (m)');
title('Load Position (in terms of X, Z)');
set(gca, 'Ydir', 'reverse');
grid on;
%fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
%% 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;
% p = drawPayload(ax,pw,ph,payloadColor);
payloadColor = 'k';
p = drawPayload_sphere(ax,params.payload.diameter/2,payloadColor);
payload_T = eye(4);
set(p,'Matrix',payload_T);
%% Change Viewpoint
view(-75,50);
movegui(gcf,'northwest');
movegui(gcf,'onscreen');
%% Animate
i = 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, L_Y, L_Z, 'c.');
end
......@@ -24,7 +24,7 @@ simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFina
% [params, simData] = hapticForceEstimation(params,simData);
%% Plot (Animation is always north, do the 5 other directions
paper_plot_admittance_control(simData);
paper_plot_admittance_control(simData, params);
% plot_position(simData.tout, simData.payload_X, simData.Xd_load,'Payload Position','northwest');
% plot_velocity(simData.tout, simData.payload_X, simData.Xd_load,'Payload Velocity','northeast');
......
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