-
Notifications
You must be signed in to change notification settings - Fork 360
/
observables.py
403 lines (325 loc) · 15.3 KB
/
observables.py
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
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
import numpy as np
def sensor(modality):
"""
Decorator that should be added to any sensors that will be an observable.
Decorated functions should have signature:
any = func(obs_cache)
Where @obs_cache is a dictionary mapping observable keys to pre-computed values, and @any is either a scalar
or array. This function should also handle the case if obs_cache is either None or an empty dict.
An example use case is shown below:
>>> @sensor(modality="proprio")
>>> def joint_pos(obs_cache):
# Always handle case if obs_cache is empty
if not obs_cache:
return np.zeros(7)
# Otherwise, run necessary calculations and return output
...
out = ...
return out
Args:
modality (str): Modality for this sensor
Returns:
function: decorator function
"""
# Define standard decorator (with no args)
def decorator(func):
# Add modality attribute
func.__modality__ = modality
# Return function
return func
return decorator
def create_deterministic_corrupter(corruption, low=-np.inf, high=np.inf):
"""
Creates a deterministic corrupter that applies the same corrupted value to all sensor values
Args:
corruption (float): Corruption to apply
low (float): Minimum value for output for clipping
high (float): Maximum value for output for clipping
Returns:
function: corrupter
"""
def corrupter(inp):
inp = np.array(inp)
return np.clip(inp + corruption, low, high)
return corrupter
def create_uniform_noise_corrupter(min_noise, max_noise, low=-np.inf, high=np.inf):
"""
Creates a corrupter that applies uniform noise to a given input within range @low to @high
Args:
min_noise (float): Minimum noise to apply
max_noise (float): Maximum noise to apply
low (float): Minimum value for output for clipping
high (float): Maxmimum value for output for clipping
Returns:
function: corrupter
"""
def corrupter(inp):
inp = np.array(inp)
noise = (max_noise - min_noise) * np.random.random_sample(inp.shape) + min_noise
return np.clip(inp + noise, low, high)
return corrupter
def create_gaussian_noise_corrupter(mean, std, low=-np.inf, high=np.inf):
"""
Creates a corrupter that applies gaussian noise to a given input with mean @mean and std dev @std
Args:
mean (float): Mean of the noise to apply
std (float): Standard deviation of the noise to apply
low (float): Minimum value for output for clipping
high (float): Maxmimum value for output for clipping
Returns:
function: corrupter
"""
def corrupter(inp):
inp = np.array(inp)
noise = mean + std * np.random.randn(*inp.shape)
return np.clip(inp + noise, low, high)
return corrupter
def create_deterministic_delayer(delay):
"""
Create a deterministic delayer that always returns the same delay value
Args:
delay (float): Delay value to return
Returns:
function: delayer
"""
assert delay >= 0, "Inputted delay must be non-negative!"
return lambda: delay
def create_uniform_sampled_delayer(min_delay, max_delay):
"""
Creates uniformly sampled delayer, with minimum delay @low and maximum delay @high, both inclusive
Args:
min_delay (float): Minimum possible delay
max_delay (float): Maxmimum possible delay
Returns:
function: delayer
"""
assert min(min_delay, max_delay) >= 0, "Inputted delay must be non-negative!"
return lambda: min_delay + (max_delay - min_delay) * np.random.random()
def create_gaussian_sampled_delayer(mean, std):
"""
Creates a gaussian sampled delayer, with average delay @mean which varies by standard deviation @std
Args:
mean (float): Average delay
std (float): Standard deviation of the delay variation
Returns:
function: delayer
"""
assert mean >= 0, "Inputted mean delay must be non-negative!"
return lambda: max(0.0, int(np.round(mean + std * np.random.randn())))
# Common defaults to use
NO_CORRUPTION = lambda inp: inp
NO_FILTER = lambda inp: inp
NO_DELAY = lambda: 0.0
class Observable:
"""
Base class for all observables -- defines interface for interacting with sensors
Args:
name (str): Name for this observable
sensor (function with `sensor` decorator): Method to grab raw sensor data for this observable. Should take in a
single dict argument (observation cache if a pre-computed value is required) and return the raw sensor data
for the current timestep. Must handle case if inputted argument is empty ({}), and should have `sensor`
decorator when defined
corrupter (None or function): Method to corrupt the raw sensor data for this observable. Should take in
the output of @sensor and return the same type (corrupted data). If None, results in default no corruption
filter (None or function): Method to filter the outputted reading for this observable. Should take in the output
of @corrupter and return the same type (filtered data). If None, results in default no filter. Note that
this function can also double as an observer, where sampled data is recorded by this function.
delayer (None or function): Method to delay the raw sensor data when polling this observable. Should take in
no arguments and return a float, for the number of seconds to delay the measurement by. If None, results in
default no delayer
sampling_rate (float): Sampling rate for this observable (Hz)
enabled (bool): Whether this sensor is enabled or not. If enabled, this observable's values
are continually computed / updated every time update() is called.
active (bool): Whether this sensor is active or not. If active, this observable's current
observed value is returned from self.obs, otherwise self.obs returns None.
"""
def __init__(
self,
name,
sensor,
corrupter=None,
filter=None,
delayer=None,
sampling_rate=20,
enabled=True,
active=True,
):
# Set all internal variables and methods
self.name = name
self._sensor = sensor
self._corrupter = corrupter if corrupter is not None else NO_CORRUPTION
self._filter = filter if filter is not None else NO_FILTER
self._delayer = delayer if delayer is not None else NO_DELAY
self._sampling_timestep = 1.0 / sampling_rate
self._enabled = enabled
self._active = active
self._is_number = False # filled in during sensor check call
self._data_shape = (1,) # filled in during sensor check call
# Make sure sensor is working
self._check_sensor_validity()
# These values will be modified during update() call
self._time_since_last_sample = 0.0 # seconds
self._current_delay = self._delayer() # seconds
self._current_observed_value = 0 if self._is_number else np.zeros(self._data_shape)
self._sampled = False
def update(self, timestep, obs_cache, force=False):
"""
Updates internal values for this observable, if enabled.
Args:
timestep (float): Amount of simulation time (in sec) that has passed since last call.
obs_cache (dict): Observation cache mapping observable names to pre-computed values to pass to sensor. This
will be updated in-place during this call.
force (bool): If True, will force the observable to update its internal value to the newest value.
"""
if self._enabled:
# Increment internal time counter
self._time_since_last_sample += timestep
# If the delayed sampling time has been passed and we haven't sampled yet for this sampling period,
# we should grab a new measurement
if (
not self._sampled and self._sampling_timestep - self._current_delay >= self._time_since_last_sample
) or force:
# Get newest raw value, corrupt it, filter it, and set it as our current observed value
obs = np.array(self._filter(self._corrupter(self._sensor(obs_cache))))
self._current_observed_value = obs[0] if len(obs.shape) == 1 and obs.shape[0] == 1 else obs
# Update cache entry as well
obs_cache[self.name] = np.array(self._current_observed_value)
# Toggle sampled and re-sample next time delay
self._sampled = True
self._current_delay = self._delayer()
# If our total time since last sample has surpassed our sampling timestep,
# then we reset our timer and sampled flag
if self._time_since_last_sample >= self._sampling_timestep:
if not self._sampled:
# If we still haven't sampled yet, sample immediately and warn user that sampling rate is too low
print(
f"Warning: sampling rate for observable {self.name} is either too low or delay is too high. "
f"Please adjust one (or both)"
)
# Get newest raw value, corrupt it, filter it, and set it as our current observed value
obs = np.array(self._filter(self._corrupter(self._sensor(obs_cache))))
self._current_observed_value = obs[0] if len(obs.shape) == 1 and obs.shape[0] == 1 else obs
# Update cache entry as well
obs_cache[self.name] = np.array(self._current_observed_value)
# Re-sample next time delay
self._current_delay = self._delayer()
self._time_since_last_sample %= self._sampling_timestep
self._sampled = False
def reset(self):
"""
Resets this observable's internal values (but does not reset its sensor, corrupter, delayer, or filter)
"""
self._time_since_last_sample = 0.0
self._current_delay = self._delayer()
self._current_observed_value = 0 if self._is_number else np.zeros(self._data_shape)
def is_enabled(self):
"""
Determines whether observable is enabled or not. This observable is considered enabled if its values
are being continually computed / updated during each update() call.
Returns:
bool: True if this observable is enabled
"""
return self._enabled
def is_active(self):
"""
Determines whether observable is active or not. This observable is considered active if its current observation
value is being returned in self.obs.
Returns:
bool: True if this observable is active
"""
return self._active
def set_enabled(self, enabled):
"""
Sets whether this observable is enabled or not. If enabled, this observable's values
are continually computed / updated every time update() is called.
Args:
enabled (bool): True if this observable should be enabled
"""
self._enabled = enabled
# Reset values
self.reset()
def set_active(self, active):
"""
Sets whether this observable is active or not. If active, this observable's current
observed value is returned from self.obs, otherwise self.obs returns None.
Args:
active (bool): True if this observable should be active
"""
self._active = active
def set_sensor(self, sensor):
"""
Sets the sensor for this observable.
Args:
sensor (function with sensor decorator): Method to grab raw sensor data for this observable. Should take in
a single dict argument (observation cache if a pre-computed value is required) and return the raw
sensor data for the current timestep. Must handle case if inputted argument is empty ({}), and should
have `sensor` decorator when defined
"""
self._sensor = sensor
self._check_sensor_validity()
def set_corrupter(self, corrupter):
"""
Sets the corrupter for this observable.
Args:
corrupter (None or function): Method to corrupt the raw sensor data for this observable. Should take in
the output of self.sensor and return the same type (corrupted data).
If None, results in default no corruption
"""
self._corrupter = corrupter if corrupter is not None else NO_CORRUPTION
def set_filter(self, filter):
"""
Sets the filter for this observable. Note that this function can also double as an observer, where sampled
data is recorded by this function.
Args:
filter (None or function): Method to filter the outputted reading for this observable. Should take in
the output of @corrupter and return the same type (filtered data).
If None, results in default no filter
"""
self._filter = filter if filter is not None else NO_FILTER
def set_delayer(self, delayer):
"""
Sets the delayer for this observable.
Args:
delayer (None or function): Method to delay the raw sensor data when polling this observable. Should take
in no arguments and return a float, for the number of seconds to delay the measurement by.
If None, results in default no filter
"""
self._delayer = delayer if delayer is not None else NO_DELAY
def set_sampling_rate(self, rate):
"""
Sets the sampling rate for this observable.
Args:
rate (int): New sampling rate for this observable (Hz)
"""
self._sampling_timestep = 1.0 / rate
def _check_sensor_validity(self):
"""
Internal function that checks the validity of this observable's sensor. It does the following:
- Asserts that the inputted sensor has its __modality__ attribute defined from the sensor decorator
- Asserts that the inputted sensor can handle the empty dict {} arg case
- Updates the corresponding name, and data-types for this sensor
"""
try:
_ = self.modality
self._data_shape = np.array(self._sensor({})).shape
self._is_number = len(self._data_shape) == 1 and self._data_shape[0] == 1
except Exception as e:
from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER
ROBOSUITE_DEFAULT_LOGGER.error(e)
raise ValueError("Current sensor for observable {} is invalid.".format(self.name))
@property
def obs(self):
"""
Current observation from this observable
Returns:
None or float or np.array: If active, current observed value from this observable. Otherwise, None
"""
return self._current_observed_value if self._active else None
@property
def modality(self):
"""
Modality of this sensor
Returns:
str: Modality name for this observable
"""
return self._sensor.__modality__