Commit c65f434f authored by mmroma's avatar mmroma
Browse files

Adding plotting.

parent 60541b9a
...@@ -5,7 +5,9 @@ function [F_user] = user_force_input(params) ...@@ -5,7 +5,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); first_quarter_point = floor(num_points / 4);
one_third_pt = floor(num_points / 3);
halfway_point = floor(num_points / 2); halfway_point = floor(num_points / 2);
two_third_pt = floor(2 * num_points / 3);
three_quarters_point = floor(num_points * 3 / 4); three_quarters_point = floor(num_points * 3 / 4);
F_user = zeros(3,num_points); F_user = zeros(3,num_points);
...@@ -31,6 +33,19 @@ function [F_user] = user_force_input(params) ...@@ -31,6 +33,19 @@ function [F_user] = user_force_input(params)
F_user(1,first_quarter_point:end) = params.user_force_amplitude; F_user(1,first_quarter_point:end) = params.user_force_amplitude;
F_user(1,halfway_point:end) = 0; F_user(1,halfway_point:end) = 0;
case 7 % Ramp Up, Constant, Ramp Down
% Ramp Up
inds = first_quarter_point:one_third_pt;
F_user(1,inds) = params.user_force_amplitude .*(inds - inds(1))./(length(inds));
% Constant
inds = one_third_pt:two_third_pt;
F_user(1,inds) = ones(1,length(inds)) .* params.user_force_amplitude;
% Ramp Down
inds = two_third_pt:three_quarters_point;
F_user(1,inds) = params.user_force_amplitude .*(inds(end) - inds)./(length(inds));
end end
% %% Double Check Waveform % %% Double Check Waveform
......
...@@ -127,7 +127,7 @@ function [params] = setUpParameters(traj_num) ...@@ -127,7 +127,7 @@ function [params] = setUpParameters(traj_num)
params = compute_quad_guidance_2(params); params = compute_quad_guidance_2(params);
%% User Force Input %% User Force Input
params.user_force_method = 6;%6; params.user_force_method = 7;%6;
params.user_force_amplitude = 0.5; % Newton's params.user_force_amplitude = 0.5; % Newton's
params.user_force = user_force_input(params); params.user_force = user_force_input(params);
......
function [] = plot_position(T,X,Xd,title_string,location) function [f] = plot_position(T,X,Xd,title_string,location)
figure(); f = figure();
subplot(3,1,1); subplot(3,1,1);
plot(T,X(:,1)); plot(T,X(:,1));
...@@ -9,7 +9,7 @@ function [] = plot_position(T,X,Xd,title_string,location) ...@@ -9,7 +9,7 @@ function [] = plot_position(T,X,Xd,title_string,location)
hold off; hold off;
ylabel('x (m)'); ylabel('x (m)');
title(title_string); title(title_string);
legend('Actual','Desired'); legend('Actual','Desired','Location','Best');
subplot(3,1,2); subplot(3,1,2);
plot(T,X(:,2)); plot(T,X(:,2));
...@@ -17,7 +17,7 @@ function [] = plot_position(T,X,Xd,title_string,location) ...@@ -17,7 +17,7 @@ function [] = plot_position(T,X,Xd,title_string,location)
plot(T,Xd(:,2),'r--'); plot(T,Xd(:,2),'r--');
hold off; hold off;
ylabel('y (m)'); ylabel('y (m)');
legend('Actual','Desired'); legend('Actual','Desired','Location','Best');
subplot(3,1,3); subplot(3,1,3);
plot(T,X(:,3)); plot(T,X(:,3));
...@@ -26,7 +26,7 @@ function [] = plot_position(T,X,Xd,title_string,location) ...@@ -26,7 +26,7 @@ function [] = plot_position(T,X,Xd,title_string,location)
hold off; hold off;
ylabel('z (m)'); ylabel('z (m)');
xlabel('Time (s)'); xlabel('Time (s)');
legend('Actual','Desired'); legend('Actual','Desired','Location','Best');
movegui(location); movegui(location);
......
function [] = paper_plot_admittance_control(d)
%PAPER_PLOT_ADMITTANCE_CONTROL
%% 1) Payload Position
f = plot_position(d.tout, d.payload_X, d.Xd_load,'','northwest');
% Adjust ylims
extra_ylims = [-0.05, 0.05];
f.Children(6).YLim = f.Children(6).YLim + extra_ylims; %x
f.Children(4).YLim = f.Children(4).YLim + extra_ylims; %y
f.Children(2).YLim = f.Children(2).YLim + extra_ylims; %z
% Adjust font size and boldness
% Adjust line widths
% Save the plot
%% 2) Payload Velocity
plot_velocity(d.tout, d.payload_X, d.Xd_load,'','northeast');
%% 3) Haptic Force Estimate
plotHapticForceEstimate(d,'southwest');
end
# Ignore everything in this directory
*
# Except this file
!.gitignore
\ No newline at end of file
...@@ -23,11 +23,13 @@ simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFina ...@@ -23,11 +23,13 @@ simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFina
%% Haptic Force Estimation (post-process) %% Haptic Force Estimation (post-process)
% [params, simData] = hapticForceEstimation(params,simData); % [params, simData] = hapticForceEstimation(params,simData);
%% Plot %% Plot (Animation is always north, do the 5 other directions
plot_position(simData.tout, simData.payload_X, simData.Xd_load,'Payload Position','northwest'); paper_plot_admittance_control(simData);
plot_velocity(simData.tout, simData.payload_X, simData.Xd_load,'Payload Velocity','northeast');
plotHapticForceEstimate(simData,'northeast'); % 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');
%
% plotHapticForceEstimate(simData,'northeast');
% plot_vec3(simData.tout, simData.Xd_load_control(:,1:3),'Load Control - Added Position','southwest'); % 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_vec3(simData.tout, simData.Xd_load_control(:,4:6),'Load Control - Added Velocity','south');
% %
......
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