1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117
|
%% plot the positions independently for right and left arm (elbow,....)
if(1)
fig=figure(1);
for t = 1:length(DataF(1,:,2))
sequence = struct('itdebut', t, 'itfin', t, 'sujet', Fname, 'fichier', Data);
clf
hold on %maintien du tracé
grid on %quadrillage du repère
%Coordonnées de la tête
x_t=DataF(1,t,1);
y_t=DataF(1,t,2);
z_t=DataF(1,t,3);
%Coordonnées de l'épaule droite
x_eg=DataF(2,t,1);
y_eg=DataF(2,t,2);
z_eg=DataF(2,t,3);
%Coordonnées de l'épaule gauche
x_ed=DataF(5,t,1);
y_ed=DataF(5,t,2);
z_ed=DataF(5,t,3);
for i=2:length(DATALINES_D)
hold on
%Tracé de la tête
% Rayon de la tête
r1 = 0.08;
%Tracé de la tête
[x1,y1,z1]=sphere(50);
x1 = x1 * r1 + x_t;
y1 = y1 * r1 + y_t;
z1 = z1 * r1 + z_t;
objet = surf(x1,y1,z1);
set(objet,'Facecolor',[0 0 1],'EdgeColor','None','FaceLighting','None');
axis equal
%---DEBUT RECONSTRUCTION DE L'EPAULE DROITE----
%Projection de la tête pour la reconstruction de l'EG
% Rayon de la sphère
%r = 0.1;
% Tracé du projeté de la tête dans le plan de l'éapule
%[x,y,z]=sphere(50);
%x = x * r +x_t;
% y = y * r + y_t;
% z = z * r + z_ed;
%objet1 = surf(x,y,z);
%set(objet1,'Facecolor',[0 0 1],'EdgeColor','None','FaceLighting','None');
%axis equal
%construction de l'épaule gauche à partir de l'épaule droite
%Tracé de la première tangente
%plot3([x_ed x_t], [y_ed y_t],...
% [z_ed (z_ed-0.1) ],'-+g');
%Tracé de la deuxième tangente
% plot3([x_ed x_t], [y_ed y_t],...
% [z_ed (z_ed+0.1) ],'-+g');
%représentation de la translation(=épaules)
%plot3([x_t x_eg], [y_t y_eg],...
% [(z_ed-0.1) z_eg ],'-+g');
%représentation de la translation(=épaules)
%plot3([x_t x_eg], [y_t y_eg],...
% [(z_ed-0.1) z_eg ],'-+g');
%Transformation du vecteur =>EDT' qui translate le point de tangence en
%épaule gauche
x_ed = x_t + (x_t-x_eg) ;
y_ed = y_t + (y_t-y_eg);
z_ed = z_eg ; % math:(z_ed-0.1) + ((z_ed-0.1)-z_ed), mais pour hteur epaule:z_ed
DataF(5,t,1)=x_ed;
DataF(5,t,2)=y_ed;
DataF(5,t,3)=z_ed;
%Tracé de l'ensemble (bras) gauche
plot3([DataF(i,t,1) DataF(i+1,t,1)], [DataF(i,t,2) DataF(i+1,t,2)],...
[DataF(i,t,3) DataF(i+1,t,3)],'-+r'); %Enregistrment d'une séquence
%Tracé de l'ensemble (bras + avant-bras) droit
plot3([DataF(i+3,t,1) DataF(i+4,t,1)], [DataF(i+3,t,2) DataF(i+4,t,2)],...
[DataF(i+3,t,3) DataF(i+4,t,3)],'-+g');
legend('tête','bras gauche','bras droit')
%---FIN RECONSTRUCTION DE L'EPAULE DROITE---
end
title(sprintf('iteration %d', t))
%Reconnaissance de la main gauche
plot3([DataF(4,t,1)], [DataF(4,t,2)],...
[DataF(4,t,3)],'r*');
%Reconnaissance de la main droite
plot3([DataF(7,t,1)], [DataF(7,t,2)],...
[DataF(7,t,3)],'g*');
%vue 3D par défaut
view(3)
pause(0.1)
end
end |
Partager