-
Notifications
You must be signed in to change notification settings - Fork 0
/
myslider.m~
185 lines (150 loc) · 9.23 KB
/
myslider.m~
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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
close all
clear all
clc
Subject = 'Giuseppe';
Task = '9';
data = load(['datasets/' Task '.dat']);
IDS_Definition
%MkSuppDimensions = [15 7]; %MkSuppDimensions = [15 7] = [Suppx Suppy]
MkSuppDimensions = [15 7 22.5 55]; %MkSuppDimensions = [15 7 22.5 55] = [Suppx Suppy ParX ParZ]
%dataset_reorg_UpperLimb_NoPlots
dataset_reorg_UpperLimb_NoPlots
%MKPos=MKPosSmoothed;
MKPosSmoothed = MKPos;
[NumFrame,~] = size(MKPosSmoothed);
% MKPos = zeros(NumFrame, 1);
%
% for i = 1 : NumFrame
% tmp1 = cell2mat(RotatedData(i)); % export from the cell
% tmp2 = tmp1([find(MkALL==ReferenceIDS(1)) find(MkALL==ReferenceIDS(2))...
% find(MkALL==ReferenceIDS(3)) find(MkALL==ReferenceIDS(4))...
% find(MkALL==ArmIDS(1)) find(MkALL==ArmIDS(2))...
% find(MkALL==ArmIDS(3)) find(MkALL==ArmIDS(4))...
% find(MkALL==ForearmIDS(1)) find(MkALL==ForearmIDS(2))...
% find(MkALL==ForearmIDS(3)) find(MkALL==ForearmIDS(4))...
% find(MkALL==HandIDS(1)) find(MkALL==HandIDS(2))...
% find(MkALL==HandIDS(3)) find(MkALL==HandIDS(4))],:);
%
%
% for j = 1 : MkNOUL
% MKPos(i, 1 +(j-1)*3 : 3 +(j-1)*3) = tmp2(j,2:4);
% end
% end
%
load (['EstimatedAngles/EstimatedQArm' Subject Task '.mat'])
load (['EstimatedParameters/UpperLimbParametersDEF' Subject '.mat'])
%UpperLimbParametersDEF=[-20 200 -100 0 40 120 0 20 180 45 10 0 220 250];
%UpperLimbParametersDEF = UpperLimbParametersDEF + [0 0 0 0 0 0 0 0 50 0 0 0 0 40];
%fv = stlread('Torso2.stl');
%fv.vertices = fv.vertices*10;
%UpperLimbParametersDEF = [40 240 -60 0 0 140 0 0 190 45 0 0 300 270];
v = VideoWriter('video.avi');
open(v);
figure;
for i = 1:NumFrame
%close all
clf;hold on;
AnglesNow = EstimatedQ(:,i);%*0 + [0 0 0 0 0 0 0]';
MeasuresNow = MKPos(i,:)'; %column vector 48 elem
EstimationsNow = hfun([UpperLimbParametersDEF MkSuppDimensions], AnglesNow); %column vector 36 elem
%%%%%%%%%%%%%%%%%%%%%% Reference Sys Plot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
plot3([0 30],[0 0],[0 0],'r','LineWidth',2)
plot3([0 0],[0 30],[0 0],'g','LineWidth',2)
plot3([0 0],[0 0],[0 30],'b','LineWidth',2)
%%%%%%%%%%%%%%%% Estimated Positions of Joints %%%%%%%%%%%%%%%%%%%%%%%%
TShoulder = gWorldS1fun([UpperLimbParametersDEF MkSuppDimensions], AnglesNow);
DistWorldShoulder = TShoulder(1:3,4);
TElbow = gWorldE1fun([UpperLimbParametersDEF MkSuppDimensions], AnglesNow);
DistWorldElbow = TElbow(1:3,4);
TElbow = gWorldE1fun([UpperLimbParametersDEF MkSuppDimensions], AnglesNow);
DistWorldElbow = TElbow(1:3,4);
TWrist = gWorldW1fun([UpperLimbParametersDEF MkSuppDimensions], AnglesNow);
DistWorldWrist = TWrist(1:3,4);
THand = gWorldHfun([UpperLimbParametersDEF MkSuppDimensions], AnglesNow);
DistWorldHand = THand(1:3,4);
plot3([DistWorldShoulder(1) DistWorldElbow(1) DistWorldWrist(1) DistWorldHand(1)],...
[DistWorldShoulder(2) DistWorldElbow(2) DistWorldWrist(2) DistWorldHand(2)],...
[DistWorldShoulder(3) DistWorldElbow(3) DistWorldWrist(3) DistWorldHand(3)],'Color',[191/255 191/255 239/255],'LineWidth',10)
plot3(DistWorldShoulder(1),DistWorldShoulder(2),DistWorldShoulder(3),'ok','MarkerSize',20,'MarkerFaceColor','k')
plot3(DistWorldElbow(1),DistWorldElbow(2),DistWorldElbow(3),'ok','MarkerSize',20,'MarkerFaceColor','k')
plot3(DistWorldWrist(1),DistWorldWrist(2),DistWorldWrist(3),'ok','MarkerSize',20,'MarkerFaceColor','k')
%%%%%%%%%%%%%%%%%%% Star Positions (ESTIMED) %%%%%%%%%%%%%%%%%%%%%%%%%%
r = 43; %lunghezza stanghette
Gamma = pi/6; %Angolo fra le stanghette
hMK1 = 45; %quota led terzo piano
hMK2 = 40; %quota led secondo piano
hMK3 = 35; %quota led primo piano
hMK4 = 67; %quota led piano alto == lunghezza lato corto
d1 = 5; %lunghezza collo del led pi? alto (cappuccio)
StarLocalCoords = [r*cos(Gamma) r*sin(Gamma) hMK1;... %MK1
r 0 hMK2;... %MK2
r*cos(Gamma) -r*sin(Gamma) hMK3;... %MK3
0 0 hMK4]; %MK4
plot3([StarLocalCoords(1,1) StarLocalCoords(2,1) StarLocalCoords(3,1) StarLocalCoords(4,1)],...
[StarLocalCoords(1,2) StarLocalCoords(2,2) StarLocalCoords(3,2) StarLocalCoords(4,2)],...
[StarLocalCoords(1,3) StarLocalCoords(2,3) StarLocalCoords(3,3) StarLocalCoords(4,3)],'kd','LineWidth',2)
MeasuresNow = [StarLocalCoords(1,:)'; StarLocalCoords(2,:)'; StarLocalCoords(3,:)'; StarLocalCoords(4,:)';MeasuresNow];
EstimationsNow = [StarLocalCoords(1,:)'; StarLocalCoords(2,:)'; StarLocalCoords(3,:)'; StarLocalCoords(4,:)';EstimationsNow];
%%%%%%%%%%%%%%%%%%% Star Positions (MEASURED) %%%%%%%%%%%%%%%%%%%%%%%%%
plot3([MeasuresNow(1) MeasuresNow(4) MeasuresNow(7) MeasuresNow(10)],...
[MeasuresNow(2) MeasuresNow(5) MeasuresNow(8) MeasuresNow(11)],...
[MeasuresNow(3) MeasuresNow(6) MeasuresNow(9) MeasuresNow(12)],'r*','LineWidth',2)
%%%%%%%%%%%%%% MEASURED Positions of Markers ARM %%%%%%%%%%%%%%%%%%%%%%
plot3([MeasuresNow(13) MeasuresNow(16) MeasuresNow(19) MeasuresNow(22) MeasuresNow(25) MeasuresNow(28)],...
[MeasuresNow(14) MeasuresNow(17) MeasuresNow(20) MeasuresNow(23) MeasuresNow(26) MeasuresNow(29)],...
[MeasuresNow(15) MeasuresNow(18) MeasuresNow(21) MeasuresNow(24) MeasuresNow(27) MeasuresNow(30)],'r*','LineWidth',2)
%%%%%%%%%%%% MEASURED Positions of Markers FOREARM %%%%%%%%%%%%%%%%%%%%
plot3([MeasuresNow(31) MeasuresNow(34) MeasuresNow(37) MeasuresNow(40) MeasuresNow(43) MeasuresNow(46)],...
[MeasuresNow(32) MeasuresNow(35) MeasuresNow(38) MeasuresNow(41) MeasuresNow(44) MeasuresNow(47)],...
[MeasuresNow(33) MeasuresNow(36) MeasuresNow(39) MeasuresNow(42) MeasuresNow(45) MeasuresNow(48)],'r*','LineWidth',2)
%%%%%%%%%%%%% MEASURED Positions of Markers HAND %%%%%%%%%%%%%%%%%%%%%%
plot3([MeasuresNow(49) MeasuresNow(52) MeasuresNow(55) MeasuresNow(58)],...
[MeasuresNow(50) MeasuresNow(53) MeasuresNow(56) MeasuresNow(59)],...
[MeasuresNow(51) MeasuresNow(54) MeasuresNow(57) MeasuresNow(60)],'r*','LineWidth',2)
%%%%%%%%%%%% ESTIMATED Positions of Markers ARM %%%%%%%%%%%%%%%%%%%%%%%
plot3([EstimationsNow(13) EstimationsNow(16) EstimationsNow(19) EstimationsNow(22) EstimationsNow(25) EstimationsNow(28)],...
[EstimationsNow(14) EstimationsNow(17) EstimationsNow(20) EstimationsNow(23) EstimationsNow(26) EstimationsNow(29)],...
[EstimationsNow(15) EstimationsNow(18) EstimationsNow(21) EstimationsNow(24) EstimationsNow(27) EstimationsNow(30)],'kd','LineWidth',2)
%%%%%%%%%% ESTIMATED Positions of Markers FOREARM %%%%%%%%%%%%%%%%%%%%%
plot3([EstimationsNow(31) EstimationsNow(34) EstimationsNow(37) EstimationsNow(40) EstimationsNow(43) EstimationsNow(46)],...
[EstimationsNow(32) EstimationsNow(35) EstimationsNow(38) EstimationsNow(41) EstimationsNow(44) EstimationsNow(47)],...
[EstimationsNow(33) EstimationsNow(36) EstimationsNow(39) EstimationsNow(42) EstimationsNow(45) EstimationsNow(48)],'kd','LineWidth',2)
%%%%%%%%%%% ESTIMATED Positions of Markers HAND %%%%%%%%%%%%%%%%%%%%%%
plot3([EstimationsNow(49) EstimationsNow(52) EstimationsNow(55) EstimationsNow(58)],...
[EstimationsNow(50) EstimationsNow(53) EstimationsNow(56) EstimationsNow(59)],...
[EstimationsNow(51) EstimationsNow(54) EstimationsNow(57) EstimationsNow(60)],'kd','LineWidth',2)
x = [DistWorldShoulder(1) DistWorldShoulder(1) DistWorldShoulder(1) DistWorldShoulder(1)];
y = [DistWorldShoulder(2) DistWorldShoulder(2)-400 DistWorldShoulder(2)-400 DistWorldShoulder(2)];
z = [DistWorldShoulder(3) DistWorldShoulder(3) DistWorldShoulder(3)-100 DistWorldShoulder(3)-100];
fill3(x,y,z,'yellow')
x = [DistWorldShoulder(1) DistWorldShoulder(1)-600 DistWorldShoulder(1)-600 DistWorldShoulder(1)];
y = [DistWorldShoulder(2) DistWorldShoulder(2) DistWorldShoulder(2) DistWorldShoulder(2)];
z = [DistWorldShoulder(3) DistWorldShoulder(3) DistWorldShoulder(3)-100 DistWorldShoulder(3)-100];
fill3(x,y,z,'yellow')
x = [DistWorldShoulder(1) DistWorldShoulder(1) DistWorldShoulder(1)-600 DistWorldShoulder(1)-600];
y = [DistWorldShoulder(2) DistWorldShoulder(2)-400 DistWorldShoulder(2)-400 DistWorldShoulder(2)];
z = [DistWorldShoulder(3) DistWorldShoulder(3) DistWorldShoulder(3) DistWorldShoulder(3)];
fill3(x,y,z,'yellow')
%
% h = patch(fv,'FaceColor', [0.8 0.8 1.0], ...
% 'EdgeColor', 'none', ...
% 'FaceLighting', 'gouraud', ...
% 'AmbientStrength', 0.15);
% camlight('headlight');
% material('dull');
%rotate(h,[1 0 0],-90)
%rotate(h,[0 0 1],-90)
axis( [-500 500 -300 500 -200 800])
%view(91.6000,-50.8000) %back view
%view(0,0) %top view
%view(-83.6000,69.2000) %feet view
%view(-90, 90) %front view
%view(88.000, -15.6)
view ( 109.2000,-3.6)
xlabel x; ylabel y; zlabel z;
frame = getframe(gca,[0 0 450 420]);
writeVideo(v,frame);
disp (['Exported ' mat2str(i) ' of ' mat2str(NumFrame)])
end
%movie2avi(F,'myavifile.avi')
close (v);