Commit ea8401d6 authored by mmroma's avatar mmroma
Browse files

In process of making admittance control work.

parent 2230dbf3
function [params] = compute_quad_eq_state_and_controller(params)
% compute_quad_eq_state_and_controller
%% What we have from guidance
% params.multirotors{i}.xd
% params.F_Tether_i_P
% params.s_hat_i_P
% params.l_d_i
% params.s_i_P
% params.f_d_i = f_d_i;
% params.f_thrust_i = f_thrust_i;
% params.phi_i = phi_i;
% params.theta_i = theta_i;
%%
end
...@@ -6,12 +6,14 @@ function [params, simData] = hapticForceEstimation(params,simData) ...@@ -6,12 +6,14 @@ function [params, simData] = hapticForceEstimation(params,simData)
%% Level 0) Direct Haptic Force Calculation w/ No Filter %% Level 0) Direct Haptic Force Calculation w/ No Filter
F_User_est0 = direcHapticForceCalc(estimationData); F_User_est0 = direcHapticForceCalc(estimationData);
plotHapticForceEstimate(estimationData, F_User_est0);
%% Level 1) Kalman Filter Haptic Force Calculation %% Level 1) Kalman Filter Haptic Force Calculation
[F_User_est1, F_User_est1_Sigma] = kalmanFilterHapticForceCalc(estimationData); % [F_User_est1, F_User_est1_Sigma] = kalmanFilterHapticForceCalc(estimationData);
plotHapticForceEstimate(estimationData, F_User_est1); % plotHapticForceEstimate(estimationData, F_User_est1);
plotHapticForceEstimateUncertainty(estimationData, F_User_est1_Sigma); % plotHapticForceEstimateUncertainty(estimationData, F_User_est1_Sigma);
%% Save Data to simData
simData.estimationData = estimationData;
simData.estimationData.F_User_est0 = F_User_est0;
end end
...@@ -44,9 +44,9 @@ function [F_User_est, F_User_est_Sigma] = kalmanFilterHapticForceCalc(data) ...@@ -44,9 +44,9 @@ function [F_User_est, F_User_est_Sigma] = kalmanFilterHapticForceCalc(data)
u_t = data.F_Gravity; u_t = data.F_Gravity;
for i=1:data.num_points for i=1:data.num_points
if i==499 % if i==499
disp("Help"); % disp("Help");
end % end
% Predict % Predict
Mu_t1_hat = A * Mu_t + B * u_t; Mu_t1_hat = A * Mu_t + B * u_t;
......
function [] = plotHapticForceEstimate(data, F_User_est) function [] = plotHapticForceEstimate(data, location)
%plotHapticForceEstimate %plotHapticForceEstimate
%% 4) Plot Results %% 4) Plot Results
figure(); figure();
dims = {'X','Y','Z'}; dims = {'X','Y','Z'};
for i=1:3 for i=1:3
subplot(3,1,i); subplot(3,1,i);
plot(data.T,F_User_est(i,:)); plot(data.tout,data.F_user_est(:,i));
hold on; hold on;
plot(data.T,data.F_User(i,:)); plot(data.tout,data.F_user(:,i));
% plot(data.tout,data.F_user_est_unfilt(:,i),'--');
hold off; hold off;
legend('Estimate','Actual'); legend('Estimate','Actual');
% legend('Estimate','Actual','Unfiltered Estimate');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('Force (N)'); ylabel('Force (N)');
title([dims{i} ' Direction - Haptic Force Input']); title([dims{i} ' Direction - Haptic Force Input']);
end end
movegui(location);
% figure(2); % figure(2);
% dims = {'X','Y','Z'}; % dims = {'X','Y','Z'};
% for i=1:3 % for i=1:3
......
...@@ -23,7 +23,7 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData) ...@@ -23,7 +23,7 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData)
F_Tethers_G = zeros(3,num_points); F_Tethers_G = zeros(3,num_points);
simData.F_tethers_P % simData.F_tethers_P
for i=1:num_points for i=1:num_points
R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i)); R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i));
F_Tethers_G(:,i) = R_P_G * simData.F_tethers_P(i,:)'; F_Tethers_G(:,i) = R_P_G * simData.F_tethers_P(i,:)';
......
...@@ -2,6 +2,6 @@ function [coarse_WPs] = WPs_set1_hover() ...@@ -2,6 +2,6 @@ function [coarse_WPs] = WPs_set1_hover()
%WPs_set1 %WPs_set1
h = 1.5; h = 1.5;
coarse_WPs = [0, 0, h, 0; coarse_WPs = [0, 0, h, 0;
0, 0, h, 20]; 0, 0, h, 10];
end end
...@@ -4,7 +4,9 @@ function [F_user] = user_force_input(params) ...@@ -4,7 +4,9 @@ function [F_user] = user_force_input(params)
%% Setup %% Setup
num_points = size(params.payload.xd,2); num_points = size(params.payload.xd,2);
first_quarter_point = floor(num_points / 4);
halfway_point = floor(num_points / 2); halfway_point = floor(num_points / 2);
three_quarters_point = floor(num_points * 3 / 4);
F_user = zeros(3,num_points); F_user = zeros(3,num_points);
%% %%
...@@ -24,6 +26,11 @@ function [F_user] = user_force_input(params) ...@@ -24,6 +26,11 @@ function [F_user] = user_force_input(params)
case 5 % Ramp case 5 % Ramp
inds = halfway_point:num_points; inds = halfway_point:num_points;
F_user(1,halfway_point:end) = params.user_force_amplitude .*(inds - halfway_point)./(num_points - halfway_point); F_user(1,halfway_point:end) = params.user_force_amplitude .*(inds - halfway_point)./(num_points - halfway_point);
case 6 % Step Up and Down
F_user(1,first_quarter_point:end) = params.user_force_amplitude;
F_user(1,halfway_point:end) = 0;
end end
% %% Double Check Waveform % %% Double Check Waveform
......
...@@ -22,21 +22,33 @@ function [params] = setUpParameters(traj_num) ...@@ -22,21 +22,33 @@ function [params] = setUpParameters(traj_num)
params.tStep = params.ctlDt / 10; % Simulation Step Size (s) params.tStep = params.ctlDt / 10; % Simulation Step Size (s)
%% Disturbance Forces & Torques %% Disturbance Forces & Torques
quads_dist_F_power = 1e-3; quads_dist_F_power = 0;
% quads_dist_F_power = 1e-3;
quads_dist_M_power = 0; quads_dist_M_power = 0;
load_dist_F_power = 1e-3; load_dist_F_power = 0;
% load_dist_F_power = 1e-3;
load_dist_M_power = 0; load_dist_M_power = 0;
%% Sensor Noise %% Sensor Noise
quad_noise_power = [ones(3,1).* 1e-8; % quad_noise_power = [ones(3,1).* 1e-8;
ones(3,1).* 1e-6; % ones(3,1).* 1e-6;
ones(3,1).* 1e-6; % ones(3,1).* 1e-6;
ones(3,1).* 1e-4]; % 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];
load_noise_power = [ones(3,1).* 1e-8; load_noise_power = [ones(3,1).* 0;
ones(3,1).* 1e-6; ones(3,1).* 0;
ones(3,1).* 1e-6; ones(3,1).* 0;
ones(3,1).* 1e-4]; ones(3,1).* 0];
%% Load Payload Parameters %% Load Payload Parameters
params.payload = parameters_Payload(params); params.payload = parameters_Payload(params);
...@@ -49,10 +61,11 @@ function [params] = setUpParameters(traj_num) ...@@ -49,10 +61,11 @@ function [params] = setUpParameters(traj_num)
r = params.payload.diameter / 2; r = params.payload.diameter / 2;
%% Adjust Payload Controller Parameters %% Adjust Payload Controller Parameters
int_limit = 1; int_limit = 1; % meters
params.e_load_pos_Kp = 1; % params.e_load_pos_Kp = diag([1,1,1]);
params.e_load_pos_Ki = 0.1; params.e_load_pos_Kp = diag([0.2,0.2,1]);
params.e_load_pos_Kd = 0.1; params.e_load_pos_Ki = diag([0.01,0.01,0.1]);
% params.e_load_pos_Kd = diag([0,0,0]);;
params.e_load_pos_int_LL = [-int_limit;-int_limit;-int_limit]; params.e_load_pos_int_LL = [-int_limit;-int_limit;-int_limit];
params.e_load_pos_int_UL = [int_limit;int_limit;int_limit]; params.e_load_pos_int_UL = [int_limit;int_limit;int_limit];
params.e_load_pos_int = zeros(3,1); params.e_load_pos_int = zeros(3,1);
...@@ -115,14 +128,23 @@ function [params] = setUpParameters(traj_num) ...@@ -115,14 +128,23 @@ function [params] = setUpParameters(traj_num)
[params.payload.xd, params.tFinal] = gen_payload_traj(params); [params.payload.xd, params.tFinal] = gen_payload_traj(params);
params.payload.x0 = params.payload.xd(:,1); params.payload.x0 = params.payload.xd(:,1);
params = compute_quad_guidance_2(params); params = compute_quad_guidance_2(params);
%% Adjust Quad Control Based on Payload Trajectory
params = compute_quad_eq_state_and_controller(params);
%% User Force Input %% User Force Input
params.user_force_method = 4; params.user_force_method = 1;%6;
params.user_force_amplitude = 3; % Newton's params.user_force_amplitude = 0.5; % Newton's
params.user_force = user_force_input(params); 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_seed = [477;478;479];
params.admittance_controller.lpf_cutoff_freq = 2*pi*5;
%% Admittance Controller
params.admittance_controller.en = 0; % 0 = False, 1 = True
params.admittance_controller.c = 5;
params.admittance_controller.m = 0.1;
end end
function [] = plot_position(T,X,Xd) function [] = plot_position(T,X,Xd,title_string,location)
figure(); figure();
...@@ -8,7 +8,7 @@ function [] = plot_position(T,X,Xd) ...@@ -8,7 +8,7 @@ function [] = plot_position(T,X,Xd)
plot(T,Xd(:,1),'r--'); plot(T,Xd(:,1),'r--');
hold off; hold off;
ylabel('x (m)'); ylabel('x (m)');
title('Position'); title(title_string);
legend('Actual','Desired'); legend('Actual','Desired');
subplot(3,1,2); subplot(3,1,2);
...@@ -27,6 +27,8 @@ function [] = plot_position(T,X,Xd) ...@@ -27,6 +27,8 @@ function [] = plot_position(T,X,Xd)
ylabel('z (m)'); ylabel('z (m)');
xlabel('Time (s)'); xlabel('Time (s)');
legend('Actual','Desired'); legend('Actual','Desired');
movegui(location);
end end
function [] = plot_vec3(T,v,name) function [] = plot_vec3(T,v,name,location)
figure(); figure();
subplot(3,1,1); subplot(3,1,1);
...@@ -15,12 +15,6 @@ function [] = plot_vec3(T,v,name) ...@@ -15,12 +15,6 @@ function [] = plot_vec3(T,v,name)
ylabel('z-dir'); ylabel('z-dir');
xlabel('Time (s)'); xlabel('Time (s)');
if strcmp(name, 'Xd Load') movegui(location);
movegui('southwest');
elseif strcmp(name, 'Xd Load Control')
movegui('south');
elseif strcmp(name, 'X Load')
movegui('southeast');
end
end end
...@@ -11,7 +11,7 @@ ...@@ -11,7 +11,7 @@
addpath(genpath(pwd)); addpath(genpath(pwd));
%% Set up Parameters %% Set up Parameters
traj_num = 1; traj_num = 2;
params = setUpParameters(traj_num); params = setUpParameters(traj_num);
%% Load Top Level Simulink Model (helps Simulink load ahead of time) %% Load Top Level Simulink Model (helps Simulink load ahead of time)
...@@ -21,15 +21,18 @@ open('coop_payload_sim'); ...@@ -21,15 +21,18 @@ open('coop_payload_sim');
simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFinal)); simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFinal));
%% Haptic Force Estimation (post-process) %% Haptic Force Estimation (post-process)
[params, simData] = hapticForceEstimation(params,simData); % [params, simData] = hapticForceEstimation(params,simData);
%% Plot %% Plot
plot_vec3(simData.tout, simData.Xd_load,'Xd Load'); plot_position(simData.tout, simData.payload_X, simData.Xd_load,'Payload Position','northwest');
plot_vec3(simData.tout, simData.Xd_load_control,'Xd Load Control'); plotHapticForceEstimate(simData,'northeast');
plot_vec3(simData.tout, simData.payload_X,'X Load'); plot_vec3(simData.tout, simData.Xd_load_control(:,1:3),'Load Control - Added Position','southwest');
plot_vec3(simData.tout, simData.Xd_load_control(:,4:6),'Load Control - Added Velocity','south');
plot_position(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Position','southeast');
% plot_position(simData.tout, simData.payload_X, simData.Xd_load_control);
% plot_position(simData.tout, simData.q1_X, simData.q1_Xd); % plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
% plot_orientation(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'); % plot_force(simData.tout, simData.q1_F_gravity, 'Q1 Gravity');
......
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