-
Notifications
You must be signed in to change notification settings - Fork 123
/
disturbances.py
303 lines (252 loc) · 8.98 KB
/
disturbances.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
'''Disturbances.'''
import numpy as np
class Disturbance:
'''Base class for disturbance or noise applied to inputs or dyanmics.'''
def __init__(self,
env,
dim,
mask=None,
**kwargs
):
self.dim = dim
self.mask = mask
if mask is not None:
self.mask = np.asarray(mask)
assert self.dim == len(self.mask)
def reset(self,
env
):
pass
def apply(self,
target,
env
):
'''Default is identity.'''
return target
def seed(self, env):
'''Reset seed from env.'''
self.np_random = env.np_random
class DisturbanceList:
'''Combine list of disturbances as one.'''
def __init__(self,
disturbances
):
'''Initialization of the list of disturbances.'''
self.disturbances = disturbances
def reset(self,
env
):
'''Sequentially reset disturbances.'''
for disturb in self.disturbances:
disturb.reset(env)
def apply(self,
target,
env
):
'''Sequentially apply disturbances.'''
disturbed = target
for disturb in self.disturbances:
disturbed = disturb.apply(disturbed, env)
return disturbed
def seed(self, env):
'''Reset seed from env.'''
for disturb in self.disturbances:
disturb.seed(env)
class ImpulseDisturbance(Disturbance):
'''Impulse applied during a short time interval.
Examples:
* single step, square (duration=1, decay_rate=1): ______|-|_______
* multiple step, square (duration>1, decay_rate=1): ______|-----|_____
* multiple step, triangle (duration>1, decay_rate<1): ______/\\_____
'''
def __init__(self,
env,
dim,
mask=None,
magnitude=1,
step_offset=None,
duration=1,
decay_rate=1,
**kwargs
):
super().__init__(env, dim, mask)
self.magnitude = magnitude
self.step_offset = step_offset
self.max_step = int(env.EPISODE_LEN_SEC / env.CTRL_TIMESTEP)
# Specify shape of the impulse.
assert duration >= 1
assert decay_rate > 0 and decay_rate <= 1
self.duration = duration
self.decay_rate = decay_rate
def reset(self,
env
):
if self.step_offset is None:
self.current_step_offset = self.np_random.randint(self.max_step)
else:
self.current_step_offset = self.step_offset
self.current_peak_step = int(self.current_step_offset + self.duration / 2)
def apply(self,
target,
env
):
noise = 0
if env.ctrl_step_counter >= self.current_step_offset:
peak_offset = np.abs(env.ctrl_step_counter - self.current_peak_step)
if peak_offset < self.duration / 2:
decay = self.decay_rate**peak_offset
else:
decay = 0
noise = self.magnitude * decay
if self.mask is not None:
noise *= self.mask
disturbed = target + noise
return disturbed
class StepDisturbance(Disturbance):
'''Constant disturbance at all time steps (but after offset).
Applied after offset step (randomized or given): _______|---------
'''
def __init__(self,
env,
dim,
mask=None,
magnitude=1,
step_offset=None,
**kwargs
):
super().__init__(env, dim, mask)
self.magnitude = magnitude
self.step_offset = step_offset
self.max_step = int(env.EPISODE_LEN_SEC / env.CTRL_TIMESTEP)
def reset(self,
env
):
if self.step_offset is None:
self.current_step_offset = self.np_random.randint(self.max_step)
else:
self.current_step_offset = self.step_offset
def apply(self,
target,
env
):
noise = 0
if env.ctrl_step_counter >= self.current_step_offset:
noise = self.magnitude
if self.mask is not None:
noise *= self.mask
disturbed = target + noise
return disturbed
class UniformNoise(Disturbance):
'''i.i.d uniform noise ~ U(low, high) per time step.'''
def __init__(self, env, dim, mask=None, low=0.0, high=1.0, **kwargs):
super().__init__(env, dim, mask)
# uniform distribution bounds
if isinstance(low, float):
self.low = np.asarray([low] * self.dim)
elif isinstance(low, list):
self.low = np.asarray(low)
else:
raise ValueError('[ERROR] UniformNoise.__init__(): low must be specified as a float or list.')
if isinstance(high, float):
self.high = np.asarray([high] * self.dim)
elif isinstance(low, list):
self.high = np.asarray(high)
else:
raise ValueError('[ERROR] UniformNoise.__init__(): high must be specified as a float or list.')
def apply(self, target, env):
noise = self.np_random.uniform(self.low, self.high, size=self.dim)
if self.mask is not None:
noise *= self.mask
disturbed = target + noise
return disturbed
class WhiteNoise(Disturbance):
'''I.i.d Gaussian noise per time step.'''
def __init__(self,
env,
dim,
mask=None,
std=1.0,
**kwargs
):
super().__init__(env, dim, mask)
# I.i.d gaussian variance.
if isinstance(std, float):
self.std = np.asarray([std] * self.dim)
elif isinstance(std, list):
self.std = np.asarray(std)
else:
raise ValueError('[ERROR] WhiteNoise.__init__(): std must be specified as a float or list.')
assert self.dim == len(self.std), 'std shape should be the same as dim.'
def apply(self,
target,
env
):
noise = self.np_random.normal(0, self.std, size=self.dim)
if self.mask is not None:
noise *= self.mask
disturbed = target + noise
return disturbed
class BrownianNoise(Disturbance):
'''Simple random walk noise.'''
def __init__(self):
super().__init__()
class PeriodicNoise(Disturbance):
'''Sinuisodal noise.'''
def __init__(self,
env,
dim,
mask=None,
scale=1.0,
frequency=1.0,
**kwargs
):
super().__init__(env, dim)
# Sine function parameters.
self.scale = scale
self.frequency = frequency
def apply(self,
target,
env
):
phase = self.np_random.uniform(low=-np.pi, high=np.pi, size=self.dim)
t = env.pyb_step_counter * env.PYB_TIMESTEP
noise = self.scale * np.sin(2 * np.pi * self.frequency * t + phase)
if self.mask is not None:
noise *= self.mask
disturbed = target + noise
return disturbed
class StateDependentDisturbance(Disturbance):
'''Time varying and state varying, e.g. friction.
Here to provide an explicit form, can also enable friction in simulator directly.
'''
def __init__(self,
env,
dim,
mask=None,
**kwargs
):
super().__init__()
DISTURBANCE_TYPES = {'impulse': ImpulseDisturbance,
'step': StepDisturbance,
'uniform': UniformNoise,
'white_noise': WhiteNoise,
'periodic': PeriodicNoise,
}
def create_disturbance_list(disturbance_specs, shared_args, env):
'''Creates a DisturbanceList from yaml disturbance specification.
Args:
disturbance_specs (list): List of dicts defining the disturbances info.
shared_args (dict): args shared across the disturbances in the list.
env (BenchmarkEnv): Env for which the constraints will be applied
'''
disturb_list = []
# Each disturbance for the mode.
for disturb in disturbance_specs:
assert 'disturbance_func' in disturb.keys(), '[ERROR]: Every distrubance must specify a disturbance_func.'
disturb_func = disturb['disturbance_func']
assert disturb_func in DISTURBANCE_TYPES, '[ERROR] in BenchmarkEnv._setup_disturbances(), disturbance type not available.'
disturb_cls = DISTURBANCE_TYPES[disturb_func]
cfg = {key: disturb[key] for key in disturb if key != 'disturbance_func'}
disturb = disturb_cls(env, **shared_args, **cfg)
disturb_list.append(disturb)
return DisturbanceList(disturb_list)