-
Notifications
You must be signed in to change notification settings - Fork 1
/
simStart.m
189 lines (144 loc) · 4.5 KB
/
simStart.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
186
187
188
189
function data = simStart(graphics)
global world;
data = zeros([ceil(world.maxIter*world.stepSize/world.sampleRate),size(world.sensors{1}.taxels,1)]);
%initialize sensor values (make sure currently set so can correctly update
%object.
updateSensors;
dataStep = 1;
if world.record
% world.video = VideoWriter('data/video.avi'); %named in setRecordOn
world.video.FrameRate = 10;
open(world.video);
end
for currentStep = 1:world.maxIter;
% disp(currentStep*world.stepSize);
updateObjects(currentStep);
updateRobots(currentStep);
updateSensors;
%this currently only handles one sensor
%this is (xy)/z - floor(xy/z) <= x/z
%this checks for when it is 'time' to read the data.
if world.stepSize*currentStep/world.sampleRate - ...
floor(world.stepSize*currentStep/world.sampleRate) <= ...
world.stepSize/world.sampleRate
% data(dataStep,:) = gather(readSensors);
data(dataStep,:) = readSensors;
dataStep = dataStep + 1;
% display(max(data(currentStep,:)));
if graphics
updateGraphics;
end
end
end
if world.record
close(world.video);
end
end
function loadSim
%display('This function should load simulation parameters');
global world;
world.maxIter = 100;
world.stepSize = 1;
world.sensors = [];
world.objects = [];
%world.addSensor(initRectangularSensor(10,10,1,0,2));
addObject(initSphereObject(10));
world.scene = figure('units','normalized','outerposition',[0 0 1 1]);
world.gravity = [0;0;-9.8];
end
function updateRobots(currentStep)
%display('This function should update robot position');
global world;
for bot = 1:length(world.robots)
world.robots{bot} = updateRobot(world.robots{bot},world.stepSize,currentStep);
end
end
function updateObjects(currentStep)
%display('This function should update object position');
global world;
if strcmp(world.type,'quasistatic')
for obj = 1:length(world.objects)
% world.objects{obj} = updateObject(world.objects{obj},world.stepSize,currentStep);
world.objects{obj} = updateObjectQS(world.objects{obj},world.stepSize,currentStep);
end
elseif strcmp(world.type,'kinematic')
for obj = 1:length(world.objects)
world.objects{obj} = updateObject(world.objects{obj},world.stepSize,currentStep);
% world.objects{obj} = updateObjectQS(world.objects{obj},world.stepSize,currentStep);
end
else
warning('unrecognized update model: acceptable models are quasistatic and kinematic\nDefaulting to quasistatic');
for obj = 1:length(world.objects)
% world.objects{obj} = updateObject(world.objects{obj},world.stepSize,currentStep);
world.objects{obj} = updateObjectQS(world.objects{obj},world.stepSize,currentStep);
end
end
end
function updateSensors
%display('This function should update sensor values');
global world;
for sen = 1:length(world.sensors);
world.sensors{sen} = resetSensorValues(world.sensors{sen});
for obj = 1:length(world.objects)
[world.sensors{sen},world.objects{obj}] = updateTaxels(world.sensors{sen},world.objects{obj});
end
end
end
function data = readSensors
%this currently only reads the first sensor, need to fix
data = [];
%display('This function should update sensor values');
global world;
for sen = 1:1 %length(world.sensors)
data = [data; readSensor(world.sensors{sen})];
end
end
%TODO: make it easy to pick with style graphics I want
%TODO: make it easier to draw sensor readings (i.e., different shapes)
function updateGraphics
%display('This function should update graphics');
global world;
%access correct figure;
if isempty(world.scene)
world.scene = figure('units','normalized','outerposition',[0 0 1 1]);
view([-.4,-1,.1]);
end
figure(world.scene);
%clear figure
cla;
if world.record
subh1 = subplot(1,2,1); title('Geometric view');
end
cla;
hold on;
%draw object
for obj = 1:length(world.objects);
drawObject(world.objects{obj},world.graphicsResolution);
end
%draw sensors
for sen = 1:length(world.sensors);
drawSensor(world.sensors{sen},world.graphicsResolution);
end
% view([-.4,-1,.1]);
view([0,2,0]);
% zlim([0,.2])
xlim([-.2,.2])
[az,el] = view;
% view([-az,el])
% view([-1,-1,-1]);
if world.record
subh2 = subplot(1,2,2);
cla;
pcolor(reshape(readSensors,[64,64])); title('Ground Truth Signal'); caxis([0,.02]);
axis equal;
end
%update figure;
if ~world.record
drawnow limitrate;
end
%uncomment to make video
if world.record
frame = getframe(world.scene);
writeVideo(world.video,frame);
end
end