Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_3DOF/show_the_results.m
35b4c71bda0dmaster

Last Change
Raw File

show_the_results.m
View Options

function IDDATA = show_the_results(IDDATA, FDDATA);
%{
Function for ploting the results
--------------------------------------------------------------------------
Syntax :
FDDATA = show_the_results(IDDATA, FDDATA);
--------------------------------------------------------------------------
File Description :
--------------------------------------------------------------------------
%}
% saving the forward dynamic results in a proper way for plotting aimes
for j = 1:6
for i=1:size(IDDATA.t,2)-1
if lt(j,4) % generalized coordinates
IDDATA.Coordinate_Evolution(j,i) = FDDATA(i).y(j,end);
else % generalized velocities
IDDATA.Coordinate_Evolution_D(j-3,i) = FDDATA(i).y(j,end);
end
end
end
% Show the resulted motion and its derivatives
figure
subplot(1,2,1);
p1 = plot(IDDATA.t(1:size(IDDATA.t,2)-1),...
(180/pi)*IDDATA.ImposedMotion([1,4,7],1:size(IDDATA.t,2)-1),...
IDDATA.t(1:size(IDDATA.t,2)-1),...
(180/pi)*IDDATA.Coordinate_Evolution(:,1:size(IDDATA.t,2)-1),...
'--','linewidth', 1);
p1(1).Color = 'r';p1(2).Color = 'g';p1(3).Color = 'b';
p1(4).Color = 'r';p1(5).Color = 'g';p1(6).Color = 'b';
grid on;
title('The imposed and integrated motions, angles',...
'fontsize', 8, ...
'fontweight', 'bold', ...
'fontname', 'sansserif');
xlabel('time [s]','fontsize',10,'fontweight','bold');
ylabel('angle [Deg]','fontsize',10,'fontweight','bold');
Motion1 = legend('imp.\psi','imp.\theta','imp.\phi',...
'int.\psi','int.\theta','int.\phi',...
'Location','best');
set(Motion1,'FontSize',10);
subplot(1,2,2);
p2 = plot(IDDATA.t(1:size(IDDATA.t,2)-1),...
(180/pi)*IDDATA.ImposedMotion([2,5,8],1:size(IDDATA.t,2)-1),...
IDDATA.t(1:size(IDDATA.t,2)-1),...
(180/pi)*IDDATA.Coordinate_Evolution_D(:,1:size(IDDATA.t,2)-1),...
'--','linewidth', 1);;
p2(1).Color = 'r';p2(2).Color = 'g';p2(3).Color = 'b';
p2(4).Color = 'r';p2(5).Color = 'g';p2(6).Color = 'b';
grid on;
title('The imposed and integrated motions, velocities',...
'fontsize', 8, ...
'fontweight', 'bold', ...
'fontname', 'sansserif');
xlabel('time [s]','fontsize',10,'fontweight','bold');
ylabel('angle [Deg/s]','fontsize',10,'fontweight','bold');
Motion2 = legend('imp. d\psi','imp. d\theta','imp. d\phi',...
'int. d\psi','int. d\theta','int. d\phi',...
'Location','best');
set(Motion2,'FontSize',10);
% subplot(1,3,3)
% p3 = plot(KEDATA.t(1,1:size(KEDATA.Coordinate_Evolution, 2)-2),KEDATA.Coordinate_EvolutionDD(:,:),'linewidth', 1);
% p3(1).Color = 'r';p3(2).Color = 'g';p3(3).Color = 'b';grid on;
% title('The imposed motion, accelerations',...
% 'fontsize', 8, ...
% 'fontweight', 'bold', ...
% 'fontname', 'sansserif');%['X' num2str(i)]
% xlabel('time[s]','fontsize',10,'fontweight','bold');
% ylabel('acceleration [rad/s2]','fontsize',10,'fontweight','bold');
% Imposed_Motion = legend('dd\theta_1','dd\theta_2','dd\theta_3');
% set(Imposed_Motion,'FontSize',10);
% Define the control input for plotting
% lambda = 0;
% for j = 1:size(IDDATA.t,2)-1
%
% DDq_d = IDDATA.ImposedMotion([3 6 9]',j);
%
% De = IDDATA.Coordinate_Evolution_D(:,j) - IDDATA.ImposedMotion([2 5 8]',j); % the velocity error
% e = IDDATA.Coordinate_Evolution(:,j) - IDDATA.ImposedMotion([1 4 7]',j); % the position error
%
% q1 = IDDATA.Coordinate_Evolution(1,j);
% q2 = IDDATA.Coordinate_Evolution(2,j);
% q3 = IDDATA.Coordinate_Evolution(3,j);
%
% dq1 = IDDATA.Coordinate_Evolution_D(1,j);
% dq2 = IDDATA.Coordinate_Evolution_D(2,j);
% dq3 = IDDATA.Coordinate_Evolution_D(3,j);
%
% % Quantify the constants
% M = DYDATA.Parameters(1,1);g = DYDATA.Parameters(1,2);CG = DYDATA.Parameters(1,3);
%
% Ixx = DYDATA.Parameters(2,1);Iyy = DYDATA.Parameters(2,2);Izz = DYDATA.Parameters(2,3);
%
% EquationsofMotion
%
% % Define the control input of the closed-loop system
% IDDATA.GeneralizedForces_CL(:,j) = MDYN(:,:)*(DDq_d - 2*lambda*De - lambda^2*e) - RHS;
%
%
%
% end
% figure
%
% plot(IDDATA.t(1,1:size(IDDATA.t,2)-1),IDDATA.GeneralizedForces_CL(:,:),IDDATA.t(1,1:size(IDDATA.t,2)-1),...
% IDDATA.GeneralizedForces(:,1:size(IDDATA.t,2)-1),IDDATA.t(1,1:size(IDDATA.t,2)-1),...
% IDDATA.GeneralizedForces_CL(:,:) - IDDATA.GeneralizedForces(:,1:size(IDDATA.t,2)-1))
return
c4science · Help