-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
stochastic.h
504 lines (392 loc) · 13.6 KB
/
stochastic.h
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
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
#pragma once
#include <memory>
#include <type_traits>
#include <variant>
#include <vector>
#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/common/name_value.h"
#include "drake/common/random.h"
#include "drake/common/symbolic.h"
namespace drake {
namespace schema {
/** @defgroup schema_stochastic Configuring distributions
@ingroup stochastic_systems
@{
This page describes how to use classes such as schema::Distribution to denote
stochastic quantities, as a bridge between loading a scenario specification and
populating the corresponding symbolic::Expression quantities into a
systems::System.
# Stochastic variables
We'll explain uses of schema::Distribution and related types using the matching
YAML syntax as parsed by yaml::YamlReadArchive.
Given this C++ data structure:
```
struct MyStuff {
schema::DistributionVariant value;
};
MyStuff stuff;
```
You might load a YAML file such as this:
```
stuff:
value: 1.0
```
The `stuff.value` is set to a constant (not stochastic) value 1.0.
Alternatively, you might load a YAML file such as this:
```
stuff:
value: !Gaussian
mean: 1.0
stddev: 0.5
```
Now, `stuff.value` is set to gaussian variable with the given mean and standard
deviation.
The exclamation point syntax is a YAML type tag, which we use to specify the
type choice within an `std::variant`. The schema::DistributionVariant is a
typedef for a specific `std::variant`.
There are a few other choices for the type.
Here, you might specify a real-valued uniform range:
```
stuff:
value: !Uniform
min: 1.0
max: 5.0
```
Or, you might choose from a set of equally-likely options:
```
stuff:
value: !UniformDiscrete
values: [1.0, 1.5, 2.0]
```
You may also use YAML's flow style to fit everything onto a single line.
These one-line spellings are the equivalent to those above.
```
stuff:
value: !Gaussian { mean: 1.0, stddev: 0.5 }
```
```
stuff:
value: !Uniform { min: 1.0, max: 5.0 }
```
```
stuff:
value: !UniformDiscrete { values: [1.0, 1.5, 2.0] }
```
# Vectors of stochastic variables
For convenience, we also provide the option to specify a vector of independent
stochastic variables with the same type.
We'll explain uses of schema::DistributionVector and related types using the
matching YAML syntax as parsed by yaml::YamlReadArchive.
Given this C++ data structure:
```
struct MyThing {
schema::DistributionVectorVariantX value;
};
MyThing thing;
```
You might load a YAML file such as this:
```
thing:
value: [1.0, 2.0, 3.0]
```
The `thing.value` is set to a constant (not stochastic) vector with three
elements.
You might also choose to constrain the vector to be a fixed size:
```
struct MyThing3 {
schema::DistributionVectorVariant3 value;
};
MyThing3 thing3;
```
Whether fixed or dynamic size, you might specify stochastic variables:
```
thing:
value: !GaussianVector
mean: [2.1, 2.2, 2.3]
stddev: [1.0] # Same stddev each.
```
Or:
```
thing:
value: !GaussianVector
mean: [2.1, 2.2, 2.3]
stddev: [1.0, 0.5, 0.2] # Different stddev each.
```
Or:
```
thing:
value: !UniformVector
min: [10.0, 20.0]
max: [11.0, 22.0]
```
@note You cannot mix, e.g., %Gaussian and %Uniform within the same vector; all
elements must be a homogeneous type.
All distributions still support constants for some elements and stochastic for
others by specifying a zero-sized range for the constant elements:
```
thing:
value: !UniformVector # The first element is a constant 2.0, not stochastic.
min: [2.0, -1.0]
max: [2.0, 1.0]
```
Or:
```
thing:
value: !GaussianVector # The first element is a constant 2.0, not stochastic.
mean: [2.0, 3.0]
stddev: [0.0, 1.0]
```
# See also
See @ref schema_transform for one practical application, of specifying
rotations, translations, and transforms using stochastic schemas.
@} */
/// Base class for a single distribution, to be used with YAML archives.
/// (See class DistributionVector for vector-valued distributions.)
///
/// See @ref serialize_tips for implementation details, especially the
/// unusually public member fields in our subclasses.
class Distribution {
public:
virtual ~Distribution();
virtual double Sample(drake::RandomGenerator* generator) const = 0;
virtual double Mean() const = 0;
virtual drake::symbolic::Expression ToSymbolic() const = 0;
protected:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Distribution)
Distribution();
};
/// A single deterministic `value`.
class Deterministic final : public Distribution {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Deterministic)
Deterministic();
explicit Deterministic(double value);
~Deterministic() final;
double Sample(drake::RandomGenerator*) const final;
double Mean() const final;
drake::symbolic::Expression ToSymbolic() const final;
template <typename Archive>
void Serialize(Archive* a) {
a->Visit(DRAKE_NVP(value));
}
double value{};
};
/// A gaussian distribution with `mean` and `stddev`.
class Gaussian final : public Distribution {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Gaussian)
Gaussian();
Gaussian(double mean, double stddev);
~Gaussian() final;
double Sample(drake::RandomGenerator*) const final;
double Mean() const final;
drake::symbolic::Expression ToSymbolic() const final;
template <typename Archive>
void Serialize(Archive* a) {
a->Visit(DRAKE_NVP(mean));
a->Visit(DRAKE_NVP(stddev));
}
double mean{};
double stddev{};
};
/// A uniform distribution with `min` inclusive and `max` exclusive.
class Uniform final : public Distribution {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Uniform)
Uniform();
Uniform(double min, double max);
~Uniform() final;
double Sample(drake::RandomGenerator*) const final;
double Mean() const final;
drake::symbolic::Expression ToSymbolic() const final;
template <typename Archive>
void Serialize(Archive* a) {
a->Visit(DRAKE_NVP(min));
a->Visit(DRAKE_NVP(max));
}
double min{};
double max{};
};
/// Chooses from among discrete `values` with equal probability.
class UniformDiscrete final : public Distribution {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(UniformDiscrete)
UniformDiscrete();
explicit UniformDiscrete(std::vector<double> values);
~UniformDiscrete() final;
double Sample(drake::RandomGenerator*) const final;
double Mean() const final;
drake::symbolic::Expression ToSymbolic() const final;
template <typename Archive>
void Serialize(Archive* a) {
a->Visit(DRAKE_NVP(values));
}
std::vector<double> values;
};
/// Variant over all kinds of distributions.
using DistributionVariant = std::variant<
double, Deterministic, Gaussian, Uniform, UniformDiscrete>;
/// Copies the given variant into a Distribution base class.
std::unique_ptr<Distribution> ToDistribution(
const DistributionVariant& var);
/// Like Distribution::Sample, but on a DistributionVariant instead.
double Sample(const DistributionVariant& var,
drake::RandomGenerator* generator);
/// Like Distribution::Mean, but on a DistributionVariant instead.
double Mean(const DistributionVariant& var);
/// Like Distribution::ToSymbolic, but on a DistributionVariant instead.
drake::symbolic::Expression ToSymbolic(const DistributionVariant& var);
/// Like Distribution::Sample, but elementwise over a collection of
/// possibly-heterogenous DistributionVariant instead.
Eigen::VectorXd Sample(const std::vector<DistributionVariant>& vec,
drake::RandomGenerator* generator);
/// Like Distribution::Mean, but elementwise over a collection of
/// possibly-heterogenous DistributionVariant instead.
Eigen::VectorXd Mean(const std::vector<DistributionVariant>& vec);
/// Like Distribution::ToSymbolic, but elementwise over a collection of
/// possibly-heterogenous DistributionVariant instead.
drake::VectorX<drake::symbolic::Expression> ToSymbolic(
const std::vector<DistributionVariant>& vec);
/// Returns true iff `var` is set to a deterministic value.
bool IsDeterministic(const DistributionVariant& var);
/// If `var` is deterministic, retrieves its value.
/// @throws exception if `var` is not deterministic.
double GetDeterministicValue(const DistributionVariant& var);
// ---------------------------------------------------------------------------
/// Base class for a vector of distributions, to be used with YAML archives.
/// (See class Distribution for scalar-valued distributions.)
///
/// See @ref serialize_tips for implementation details, especially the
/// unusually public member fields in our subclasses.
class DistributionVector {
public:
virtual ~DistributionVector();
virtual Eigen::VectorXd Sample(drake::RandomGenerator* generator) const = 0;
virtual Eigen::VectorXd Mean() const = 0;
virtual drake::VectorX<drake::symbolic::Expression> ToSymbolic() const = 0;
protected:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(DistributionVector)
DistributionVector();
};
/// A single deterministic vector `value`.
/// @tparam Size rows at compile time (max 6) or else Eigen::Dynamic.
template <int Size>
class DeterministicVector final : public DistributionVector {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(DeterministicVector)
DeterministicVector();
explicit DeterministicVector(const drake::Vector<double, Size>& value);
~DeterministicVector() final;
Eigen::VectorXd Sample(drake::RandomGenerator* generator) const final;
Eigen::VectorXd Mean() const final;
drake::VectorX<drake::symbolic::Expression> ToSymbolic() const final;
template <typename Archive>
void Serialize(Archive* a) {
a->Visit(DRAKE_NVP(value));
}
drake::Vector<double, Size> value;
};
/// A gaussian distribution with vector `mean` and vector or scalar `stddev`.
///
/// When `mean` and `stddev` both have the same number of elements, that
/// denotes an elementwise pairing of the 0th mean with 0th stddev, 1st mean
/// with 1st stddev, etc.
///
/// Alternatively, `stddev` can be a vector with a single element, no matter
/// the size of `mean`; that denotes the same `stddev` value applied to every
/// element of `mean`.
///
/// @tparam Size rows at compile time (max 6) or else Eigen::Dynamic.
template <int Size>
class GaussianVector final : public DistributionVector {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(GaussianVector)
GaussianVector();
GaussianVector(const drake::Vector<double, Size>& mean,
const drake::VectorX<double>& stddev);
~GaussianVector() final;
Eigen::VectorXd Sample(drake::RandomGenerator*) const final;
Eigen::VectorXd Mean() const final;
drake::VectorX<drake::symbolic::Expression> ToSymbolic() const final;
template <typename Archive>
void Serialize(Archive* a) {
a->Visit(DRAKE_NVP(mean));
a->Visit(DRAKE_NVP(stddev));
}
drake::Vector<double, Size> mean;
drake::VectorX<double> stddev;
};
/// A uniform distribution with vector `min` inclusive and vector `max`
/// exclusive.
/// @tparam Size rows at compile time (max 6) or else Eigen::Dynamic.
template <int Size>
class UniformVector final : public DistributionVector {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(UniformVector)
UniformVector();
UniformVector(const drake::Vector<double, Size>& min,
const drake::Vector<double, Size>& max);
~UniformVector() final;
Eigen::VectorXd Sample(drake::RandomGenerator* generator) const final;
Eigen::VectorXd Mean() const final;
drake::VectorX<drake::symbolic::Expression> ToSymbolic() const final;
template <typename Archive>
void Serialize(Archive* a) {
a->Visit(DRAKE_NVP(min));
a->Visit(DRAKE_NVP(max));
}
drake::Vector<double, Size> min;
drake::Vector<double, Size> max;
};
namespace internal {
struct InvalidVariantSelection {
template <typename Archive>
void Serialize(Archive*) {
DRAKE_UNREACHABLE();
}
};
} // namespace internal
/// Variant over all kinds of vector distributions.
///
/// If the Size parameter allows for 1-element vectors (i.e, is either 1 or
/// Eigen::Dynamic), then this variant also offers the single distribution
/// types (Deterministic, Gaussian, Uniform). If the Size parameter is 2 or
/// greater, the single distribution types are not allowed.
///
/// @tparam Size rows at compile time (max 6) or else Eigen::Dynamic.
template <int Size>
using DistributionVectorVariant = std::variant<
drake::Vector<double, Size>,
DeterministicVector<Size>,
GaussianVector<Size>,
UniformVector<Size>,
std::conditional_t<(Size == Eigen::Dynamic || Size == 1),
Deterministic,
internal::InvalidVariantSelection>,
std::conditional_t<(Size == Eigen::Dynamic || Size == 1),
Gaussian,
internal::InvalidVariantSelection>,
std::conditional_t<(Size == Eigen::Dynamic || Size == 1),
Uniform,
internal::InvalidVariantSelection>>;
/// DistributionVectorVariant that permits any vector size dynamically.
using DistributionVectorVariantX = DistributionVectorVariant<Eigen::Dynamic>;
/// Copies the given variant into a DistributionVector base class.
/// @tparam Size rows at compile time (max 6) or else Eigen::Dynamic.
template <int Size>
std::unique_ptr<DistributionVector> ToDistributionVector(
const DistributionVectorVariant<Size>& vec);
/// Returns true iff all of `vec`'s elements are set to a deterministic value.
/// @tparam Size rows at compile time (max 6) or else Eigen::Dynamic.
template <int Size>
bool IsDeterministic(const DistributionVectorVariant<Size>& vec);
/// If `vec` is deterministic, retrieves its value.
/// @throws exception if `vec` is not deterministic.
/// @tparam Size rows at compile time (max 6) or else Eigen::Dynamic.
template <int Size>
Eigen::VectorXd GetDeterministicValue(
const DistributionVectorVariant<Size>& vec);
} // namespace schema
} // namespace drake