forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
/
symbolic_expression.h
1503 lines (1338 loc) · 62.3 KB
/
symbolic_expression.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
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#pragma once
#ifndef DRAKE_COMMON_SYMBOLIC_HEADER
// TODO(soonho-tri): Change to #error, when #6613 merged.
#warning Do not directly include this file. Include "drake/common/symbolic.h".
#endif
#include <algorithm> // for cpplint only
#include <cstddef>
#include <functional>
#include <limits>
#include <map>
#include <memory>
#include <ostream>
#include <random>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <utility>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include "drake/common/cond.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/drake_deprecated.h"
#include "drake/common/drake_throw.h"
#include "drake/common/dummy_value.h"
#include "drake/common/eigen_types.h"
#include "drake/common/extract_double.h"
#include "drake/common/hash.h"
#include "drake/common/random.h"
#include "drake/common/symbolic.h"
namespace drake {
namespace symbolic {
#ifndef DRAKE_DOXYGEN_CXX
class Expression;
namespace internal {
// Helper to implement (deprecated) Expression::ToPolynomial.
// TODO(soonho-tri): Remove this on or after 2020-07-01 when we remove
// Expression::ToPolynomial.
struct ToPolynomialHelperTag {};
template <typename Dummy>
auto ToPolynomialHelper(const Expression& e, const Dummy&) {
return ToPolynomial(e, Dummy{});
}
} // namespace internal
#endif
/** Kinds of symbolic expressions. */
enum class ExpressionKind {
Constant, ///< constant (double)
Var, ///< variable
Add, ///< addition (+)
Mul, ///< multiplication (*)
Div, ///< division (/)
Log, ///< logarithms
Abs, ///< absolute value function
Exp, ///< exponentiation
Sqrt, ///< square root
Pow, ///< power function
Sin, ///< sine
Cos, ///< cosine
Tan, ///< tangent
Asin, ///< arcsine
Acos, ///< arccosine
Atan, ///< arctangent
Atan2, ///< arctangent2 (atan2(y,x) = atan(y/x))
Sinh, ///< hyperbolic sine
Cosh, ///< hyperbolic cosine
Tanh, ///< hyperbolic tangent
Min, ///< min
Max, ///< max
Ceil, ///< ceil
Floor, ///< floor
IfThenElse, ///< if then else
NaN, ///< NaN
UninterpretedFunction, ///< Uninterpreted function
// TODO(soonho): add Integral
};
/** Total ordering between ExpressionKinds. */
bool operator<(ExpressionKind k1, ExpressionKind k2);
class ExpressionCell; // In symbolic_expression_cell.h
class ExpressionConstant; // In symbolic_expression_cell.h
class ExpressionVar; // In symbolic_expression_cell.h
class UnaryExpressionCell; // In symbolic_expression_cell.h
class BinaryExpressionCell; // In symbolic_expression_cell.h
class ExpressionAdd; // In symbolic_expression_cell.h
class ExpressionMul; // In symbolic_expression_cell.h
class ExpressionDiv; // In symbolic_expression_cell.h
class ExpressionLog; // In symbolic_expression_cell.h
class ExpressionAbs; // In symbolic_expression_cell.h
class ExpressionExp; // In symbolic_expression_cell.h
class ExpressionSqrt; // In symbolic_expression_cell.h
class ExpressionPow; // In symbolic_expression_cell.h
class ExpressionSin; // In symbolic_expression_cell.h
class ExpressionCos; // In symbolic_expression_cell.h
class ExpressionTan; // In symbolic_expression_cell.h
class ExpressionAsin; // In symbolic_expression_cell.h
class ExpressionAcos; // In symbolic_expression_cell.h
class ExpressionAtan; // In symbolic_expression_cell.h
class ExpressionAtan2; // In symbolic_expression_cell.h
class ExpressionSinh; // In symbolic_expression_cell.h
class ExpressionCosh; // In symbolic_expression_cell.h
class ExpressionTanh; // In symbolic_expression_cell.h
class ExpressionMin; // In symbolic_expression_cell.h
class ExpressionMax; // In symbolic_expression_cell.h
class ExpressionCeiling; // In symbolic_expression_cell.h
class ExpressionFloor; // In symbolic_expression_cell.h
class ExpressionIfThenElse; // In symbolic_expression_cell.h
class ExpressionUninterpretedFunction; // In symbolic_expression_cell.h
class Formula; // In symbolic_formula.h
class Expression;
// Substitution is a map from a Variable to a symbolic expression. It is used in
// Expression::Substitute and Formula::Substitute methods as an argument.
using Substitution = std::unordered_map<Variable, Expression>;
/** Represents a symbolic form of an expression.
Its syntax tree is as follows:
@verbatim
E := Var | Constant | E + ... + E | E * ... * E | E / E | log(E)
| abs(E) | exp(E) | sqrt(E) | pow(E, E) | sin(E) | cos(E) | tan(E)
| asin(E) | acos(E) | atan(E) | atan2(E, E) | sinh(E) | cosh(E) | tanh(E)
| min(E, E) | max(E, E) | ceil(E) | floor(E) | if_then_else(F, E, E)
| NaN | uninterpreted_function(name, {v_1, ..., v_n})
@endverbatim
In the implementation, Expression is a simple wrapper including a shared pointer
to ExpressionCell class which is a super-class of different kinds of symbolic
expressions (i.e. ExpressionAdd, ExpressionMul, ExpressionLog,
ExpressionSin). Note that it includes a shared pointer, not a unique pointer, to
allow sharing sub-expressions.
@note The sharing of sub-expressions is not yet implemented.
@note -E is represented as -1 * E internally.
@note A subtraction E1 - E2 is represented as E1 + (-1 * E2) internally.
The following simple simplifications are implemented:
@verbatim
E + 0 -> E
0 + E -> E
E - 0 -> E
E - E -> 0
E * 1 -> E
1 * E -> E
E * 0 -> 0
0 * E -> 0
E / 1 -> E
E / E -> 1
pow(E, 0) -> 1
pow(E, 1) -> E
E * E -> E^2 (= pow(E, 2))
sqrt(E * E) -> |E| (= abs(E))
sqrt(E) * sqrt(E) -> E
@endverbatim
Constant folding is implemented:
@verbatim
E(c1) + E(c2) -> E(c1 + c2) // c1, c2 are constants
E(c1) - E(c2) -> E(c1 - c2)
E(c1) * E(c2) -> E(c1 * c2)
E(c1) / E(c2) -> E(c1 / c2)
f(E(c)) -> E(f(c)) // c is a constant, f is a math function
@endverbatim
For the math functions which are only defined over a restricted domain (namely,
log, sqrt, pow, asin, acos), we check the domain of argument(s), and throw
std::domain_error exception if a function is not well-defined for a given
argument(s).
Relational operators over expressions (==, !=, <, >, <=, >=) return
symbolic::Formula instead of bool. Those operations are declared in
symbolic_formula.h file. To check structural equality between two expressions a
separate function, Expression::EqualTo, is provided.
symbolic::Expression can be used as a scalar type of Eigen types.
*/
class Expression {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Expression)
/** Default constructor. It constructs Zero(). */
Expression() { *this = Zero(); }
/** Constructs a constant. */
// NOLINTNEXTLINE(runtime/explicit): This conversion is desirable.
Expression(double d);
/** Constructs an expression from @p var.
* @pre @p var is neither a dummy nor a BOOLEAN variable.
*/
// NOLINTNEXTLINE(runtime/explicit): This conversion is desirable.
Expression(const Variable& var);
/** Returns expression kind. */
ExpressionKind get_kind() const;
/** Collects variables in expression. */
Variables GetVariables() const;
/** Checks structural equality.
*
* Two expressions e1 and e2 are structurally equal when they have the same
* internal AST(abstract-syntax tree) representation. Please note that we can
* have two computationally (or extensionally) equivalent expressions which
* are not structurally equal. For example, consider:
*
* e1 = 2 * (x + y)
* e2 = 2x + 2y
*
* Obviously, we know that e1 and e2 are evaluated to the same value for all
* assignments to x and y. However, e1 and e2 are not structurally equal by
* the definition. Note that e1 is a multiplication expression
* (is_multiplication(e1) is true) while e2 is an addition expression
* (is_addition(e2) is true).
*
* One main reason we use structural equality in EqualTo is due to
* Richardson's Theorem. It states that checking ∀x. E(x) = F(x) is
* undecidable when we allow sin, asin, log, exp in E and F. Read
* https://en.wikipedia.org/wiki/Richardson%27s_theorem for details.
*
* Note that for polynomial cases, you can use Expand method and check if two
* polynomial expressions p1 and p2 are computationally equal. To do so, you
* check the following:
*
* (p1.Expand() - p2.Expand()).EqualTo(0).
*/
bool EqualTo(const Expression& e) const;
/** Provides lexicographical ordering between expressions.
This function is used as a compare function in map<Expression> and
set<Expression> via std::less<drake::symbolic::Expression>. */
bool Less(const Expression& e) const;
/** Checks if this symbolic expression is convertible to Polynomial. */
bool is_polynomial() const;
/** Returns a Polynomial representing this expression.
* Note that the ID of a variable is preserved in this translation.
* \pre{is_polynomial() is true.}
*/
template <typename Dummy = internal::ToPolynomialHelperTag>
DRAKE_DEPRECATED("2020-07-01",
"Use drake::ToPolynomial(const Expression&) instead.")
auto ToPolynomial() const {
return internal::ToPolynomialHelper<Dummy>(*this, Dummy{});
}
/** Evaluates using a given environment (by default, an empty environment) and
* a random number generator. If there is a random variable in this expression
* which is unassigned in @p env, this method uses @p random_generator to
* sample a value and use the value to substitute all occurrences of the
* variable in this expression.
*
* @throws std::runtime_error if there exists a non-random variable in this
* expression whose assignment is not provided by
* @p env.
* @throws std::runtime_error if an unassigned random variable is detected
* while @p random_generator is `nullptr`.
* @throws std::runtime_error if NaN is detected during evaluation.
*/
double Evaluate(const Environment& env = Environment{},
RandomGenerator* random_generator = nullptr) const;
/** Evaluates using an empty environment and a random number generator. It
* uses @p random_generator to sample values for the random variables in this
* expression.
*
* See the above overload for the exceptions that it might throw.
*/
double Evaluate(RandomGenerator* random_generator) const;
/** Partially evaluates this expression using an environment @p
* env. Internally, this method promotes @p env into a substitution
* (Variable → Expression) and call Evaluate::Substitute with it.
*
* @throws std::runtime_error if NaN is detected during evaluation.
*/
Expression EvaluatePartial(const Environment& env) const;
/** Returns true if this symbolic expression is already
* expanded. Expression::Expand() uses this flag to avoid calling
* ExpressionCell::Expand() on an pre-expanded expressions.
* Expression::Expand() also sets this flag before returning the result.
*
* @note This check is conservative in that `false` does not always indicate
* that the expression is not expanded. This is because exact checks can be
* costly and we want to avoid the exact check at the construction time.
*/
bool is_expanded() const;
/** Expands out products and positive integer powers in expression. For
* example, `(x + 1) * (x - 1)` is expanded to `x^2 - 1` and `(x + y)^2` is
* expanded to `x^2 + 2xy + y^2`. Note that Expand applies recursively to
* sub-expressions. For instance, `sin(2 * (x + y))` is expanded to `sin(2x +
* 2y)`. It also simplifies "division by constant" cases. See
* "drake/common/test/symbolic_expansion_test.cc" to find the examples.
*
* @throws std::runtime_error if NaN is detected during expansion.
*/
Expression Expand() const;
/** Returns a copy of this expression replacing all occurrences of @p var
* with @p e.
* @throws std::runtime_error if NaN is detected during substitution.
*/
Expression Substitute(const Variable& var, const Expression& e) const;
/** Returns a copy of this expression replacing all occurrences of the
* variables in @p s with corresponding expressions in @p s. Note that the
* substitutions occur simultaneously. For example, (x / y).Substitute({{x,
* y}, {y, x}}) gets (y / x).
* @throws std::runtime_error if NaN is detected during substitution.
*/
Expression Substitute(const Substitution& s) const;
/** Differentiates this symbolic expression with respect to the variable @p
* var.
* @throws std::runtime_error if it is not differentiable.
*/
Expression Differentiate(const Variable& x) const;
/** Let `f` be this Expression, computes a row vector of derivatives,
* `[∂f/∂vars(0), ... , ∂f/∂vars(n-1)]` with respect to the variables
* @p vars.
*/
RowVectorX<Expression> Jacobian(
const Eigen::Ref<const VectorX<Variable>>& vars) const;
/** Returns string representation of Expression. */
std::string to_string() const;
/** Returns zero. */
static Expression Zero();
/** Returns one. */
static Expression One();
/** Returns Pi, the ratio of a circle’s circumference to its diameter. */
static Expression Pi();
/** Return e, the base of natural logarithms. */
static Expression E();
/** Returns NaN (Not-a-Number). */
static Expression NaN();
/** Implements the @ref hash_append concept. */
template <class HashAlgorithm>
friend void hash_append(HashAlgorithm& hasher,
const Expression& item) noexcept {
DelegatingHasher delegating_hasher(
[&hasher](const void* data, const size_t length) {
return hasher(data, length);
});
item.HashAppend(&delegating_hasher);
}
friend Expression operator+(Expression lhs, const Expression& rhs);
// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
friend Expression& operator+=(Expression& lhs, const Expression& rhs);
/** Provides prefix increment operator (i.e. ++x). */
Expression& operator++();
/** Provides postfix increment operator (i.e. x++). */
Expression operator++(int);
/** Provides unary plus operator. */
friend Expression operator+(const Expression& e);
friend Expression operator-(Expression lhs, const Expression& rhs);
// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
friend Expression& operator-=(Expression& lhs, const Expression& rhs);
/** Provides unary minus operator. */
friend Expression operator-(const Expression& e);
/** Provides prefix decrement operator (i.e. --x). */
Expression& operator--();
/** Provides postfix decrement operator (i.e. x--). */
Expression operator--(int);
friend Expression operator*(Expression lhs, const Expression& rhs);
// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
friend Expression& operator*=(Expression& lhs, const Expression& rhs);
friend Expression operator/(Expression lhs, const Expression& rhs);
// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
friend Expression& operator/=(Expression& lhs, const Expression& rhs);
friend Expression log(const Expression& e);
friend Expression abs(const Expression& e);
friend Expression exp(const Expression& e);
friend Expression sqrt(const Expression& e);
friend Expression pow(const Expression& e1, const Expression& e2);
friend Expression sin(const Expression& e);
friend Expression cos(const Expression& e);
friend Expression tan(const Expression& e);
friend Expression asin(const Expression& e);
friend Expression acos(const Expression& e);
friend Expression atan(const Expression& e);
friend Expression atan2(const Expression& e1, const Expression& e2);
friend Expression sinh(const Expression& e);
friend Expression cosh(const Expression& e);
friend Expression tanh(const Expression& e);
friend Expression min(const Expression& e1, const Expression& e2);
friend Expression max(const Expression& e1, const Expression& e2);
friend Expression ceil(const Expression& e);
friend Expression floor(const Expression& e);
/** Constructs if-then-else expression.
@verbatim
if_then_else(cond, expr_then, expr_else)
@endverbatim
The value returned by the above if-then-else expression is @p expr_then if
@p cond is evaluated to true. Otherwise, it returns @p expr_else.
The semantics is similar to the C++'s conditional expression constructed by
its ternary operator, @c ?:. However, there is a key difference between the
C++'s conditional expression and our @c if_then_else expression in a way the
arguments are evaluated during the construction.
- In case of the C++'s conditional expression, <tt> cond ? expr_then :
expr_else</tt>, the then expression @c expr_then (respectively, the else
expression @c expr_else) is \b only evaluated when the conditional
expression @c cond is evaluated to \b true (respectively, when @c cond is
evaluated to \b false).
- In case of the symbolic expression, <tt>if_then_else(cond, expr_then,
expr_else)</tt>, however, \b both arguments @c expr_then and @c expr_else
are evaluated first and then passed to the @c if_then_else function.
@note This function returns an \b expression and it is different from the
C++'s if-then-else \b statement.
@note While it is still possible to define <tt> min, max, abs</tt> math
functions using @c if_then_else expression, it is highly \b recommended to
use the provided native definitions for them because it allows solvers to
detect specific math functions and to have a room for special
optimizations.
@note More information about the C++'s conditional expression and ternary
operator is available at
http://en.cppreference.com/w/cpp/language/operator_other#Conditional_operator.
*/
friend Expression if_then_else(const Formula& f_cond,
const Expression& e_then,
const Expression& e_else);
friend Expression uninterpreted_function(std::string name,
std::vector<Expression> arguments);
friend std::ostream& operator<<(std::ostream& os, const Expression& e);
friend void swap(Expression& a, Expression& b) { std::swap(a.ptr_, b.ptr_); }
friend bool is_constant(const Expression& e);
friend bool is_variable(const Expression& e);
friend bool is_addition(const Expression& e);
friend bool is_multiplication(const Expression& e);
friend bool is_division(const Expression& e);
friend bool is_log(const Expression& e);
friend bool is_abs(const Expression& e);
friend bool is_exp(const Expression& e);
friend bool is_sqrt(const Expression& e);
friend bool is_pow(const Expression& e);
friend bool is_sin(const Expression& e);
friend bool is_cos(const Expression& e);
friend bool is_tan(const Expression& e);
friend bool is_asin(const Expression& e);
friend bool is_acos(const Expression& e);
friend bool is_atan(const Expression& e);
friend bool is_atan2(const Expression& e);
friend bool is_sinh(const Expression& e);
friend bool is_cosh(const Expression& e);
friend bool is_tanh(const Expression& e);
friend bool is_min(const Expression& e);
friend bool is_max(const Expression& e);
friend bool is_ceil(const Expression& e);
friend bool is_floor(const Expression& e);
friend bool is_if_then_else(const Expression& e);
friend bool is_uninterpreted_function(const Expression& e);
// Note that the following cast functions are only for low-level operations
// and not exposed to the user of drake/common/symbolic_expression.h
// header. These functions are declared in
// drake/common/symbolic_expression_cell.h header.
friend std::shared_ptr<const ExpressionConstant> to_constant(
const Expression& e);
friend std::shared_ptr<const ExpressionVar> to_variable(const Expression& e);
friend std::shared_ptr<const UnaryExpressionCell> to_unary(
const Expression& e);
friend std::shared_ptr<const BinaryExpressionCell> to_binary(
const Expression& e);
friend std::shared_ptr<const ExpressionAdd> to_addition(const Expression& e);
friend std::shared_ptr<const ExpressionMul> to_multiplication(
const Expression& e);
friend std::shared_ptr<const ExpressionDiv> to_division(const Expression& e);
friend std::shared_ptr<const ExpressionLog> to_log(const Expression& e);
friend std::shared_ptr<const ExpressionAbs> to_abs(const Expression& e);
friend std::shared_ptr<const ExpressionExp> to_exp(const Expression& e);
friend std::shared_ptr<const ExpressionSqrt> to_sqrt(const Expression& e);
friend std::shared_ptr<const ExpressionPow> to_pow(const Expression& e);
friend std::shared_ptr<const ExpressionSin> to_sin(const Expression& e);
friend std::shared_ptr<const ExpressionCos> to_cos(const Expression& e);
friend std::shared_ptr<const ExpressionTan> to_tan(const Expression& e);
friend std::shared_ptr<const ExpressionAsin> to_asin(const Expression& e);
friend std::shared_ptr<const ExpressionAcos> to_acos(const Expression& e);
friend std::shared_ptr<const ExpressionAtan> to_atan(const Expression& e);
friend std::shared_ptr<const ExpressionAtan2> to_atan2(const Expression& e);
friend std::shared_ptr<const ExpressionSinh> to_sinh(const Expression& e);
friend std::shared_ptr<const ExpressionCosh> to_cosh(const Expression& e);
friend std::shared_ptr<const ExpressionTanh> to_tanh(const Expression& e);
friend std::shared_ptr<const ExpressionMin> to_min(const Expression& e);
friend std::shared_ptr<const ExpressionMax> to_max(const Expression& e);
friend std::shared_ptr<const ExpressionCeiling> to_ceil(const Expression& e);
friend std::shared_ptr<const ExpressionFloor> to_floor(const Expression& e);
friend std::shared_ptr<const ExpressionIfThenElse> to_if_then_else(
const Expression& e);
friend std::shared_ptr<const ExpressionUninterpretedFunction>
to_uninterpreted_function(const Expression& e);
// Cast functions which takes a pointer to a non-const Expression.
friend std::shared_ptr<ExpressionConstant> to_constant(Expression* e);
friend std::shared_ptr<ExpressionVar> to_variable(Expression* e);
friend std::shared_ptr<UnaryExpressionCell> to_unary(Expression* e);
friend std::shared_ptr<BinaryExpressionCell> to_binary(Expression* e);
friend std::shared_ptr<ExpressionAdd> to_addition(Expression* e);
friend std::shared_ptr<ExpressionMul> to_multiplication(Expression* e);
friend std::shared_ptr<ExpressionDiv> to_division(Expression* e);
friend std::shared_ptr<ExpressionLog> to_log(Expression* e);
friend std::shared_ptr<ExpressionAbs> to_abs(Expression* e);
friend std::shared_ptr<ExpressionExp> to_exp(Expression* e);
friend std::shared_ptr<ExpressionSqrt> to_sqrt(Expression* e);
friend std::shared_ptr<ExpressionPow> to_pow(Expression* e);
friend std::shared_ptr<ExpressionSin> to_sin(Expression* e);
friend std::shared_ptr<ExpressionCos> to_cos(Expression* e);
friend std::shared_ptr<ExpressionTan> to_tan(Expression* e);
friend std::shared_ptr<ExpressionAsin> to_asin(Expression* e);
friend std::shared_ptr<ExpressionAcos> to_acos(Expression* e);
friend std::shared_ptr<ExpressionAtan> to_atan(Expression* e);
friend std::shared_ptr<ExpressionAtan2> to_atan2(Expression* e);
friend std::shared_ptr<ExpressionSinh> to_sinh(Expression* e);
friend std::shared_ptr<ExpressionCosh> to_cosh(Expression* e);
friend std::shared_ptr<ExpressionTanh> to_tanh(Expression* e);
friend std::shared_ptr<ExpressionMin> to_min(Expression* e);
friend std::shared_ptr<ExpressionMax> to_max(Expression* e);
friend std::shared_ptr<ExpressionCeiling> to_ceil(Expression* e);
friend std::shared_ptr<ExpressionFloor> to_floor(Expression* e);
friend std::shared_ptr<ExpressionIfThenElse> to_if_then_else(Expression* e);
friend std::shared_ptr<ExpressionUninterpretedFunction>
to_uninterpreted_function(Expression* e);
friend class ExpressionAddFactory;
friend class ExpressionMulFactory;
// The following classes call the private method `set_expand()` and need to be
// friends of this class.
friend class ExpressionAdd;
friend class ExpressionMul;
friend class ExpressionDiv;
friend class ExpressionPow;
private:
// This is a helper function used to handle `Expression(double)` constructor.
static std::shared_ptr<ExpressionCell> make_cell(double d);
explicit Expression(std::shared_ptr<ExpressionCell> ptr);
void HashAppend(DelegatingHasher* hasher) const;
// Note: We use "non-const" ExpressionCell type. This allows us to perform
// destructive updates on the pointed cell if the cell is not shared with
// other Expressions (that is, ptr_.use_count() == 1).
std::shared_ptr<ExpressionCell> ptr_;
/** Sets this symbolic expression as already expanded. */
Expression& set_expanded();
};
Expression operator+(Expression lhs, const Expression& rhs);
// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
Expression& operator+=(Expression& lhs, const Expression& rhs);
Expression operator+(const Expression& e);
Expression operator-(Expression lhs, const Expression& rhs);
// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
Expression& operator-=(Expression& lhs, const Expression& rhs);
Expression operator-(const Expression& e);
Expression operator*(Expression lhs, const Expression& rhs);
// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
Expression& operator*=(Expression& lhs, const Expression& rhs);
Expression operator/(Expression lhs, const Expression& rhs);
// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
Expression& operator/=(Expression& lhs, const Expression& rhs);
Expression log(const Expression& e);
Expression abs(const Expression& e);
Expression exp(const Expression& e);
Expression sqrt(const Expression& e);
Expression pow(const Expression& e1, const Expression& e2);
Expression sin(const Expression& e);
Expression cos(const Expression& e);
Expression tan(const Expression& e);
Expression asin(const Expression& e);
Expression acos(const Expression& e);
Expression atan(const Expression& e);
Expression atan2(const Expression& e1, const Expression& e2);
Expression sinh(const Expression& e);
Expression cosh(const Expression& e);
Expression tanh(const Expression& e);
Expression min(const Expression& e1, const Expression& e2);
Expression max(const Expression& e1, const Expression& e2);
Expression ceil(const Expression& e);
Expression floor(const Expression& e);
Expression if_then_else(const Formula& f_cond, const Expression& e_then,
const Expression& e_else);
/** Constructs an uninterpreted-function expression with @p name and @p
* arguments. An uninterpreted function is an opaque function that has no other
* property than its name and a list of its arguments. This is useful to
* applications where it is good enough to provide abstract information of a
* function without exposing full details. Declaring sparsity of a system is a
* typical example.
*/
Expression uninterpreted_function(std::string name,
std::vector<Expression> arguments);
void swap(Expression& a, Expression& b);
std::ostream& operator<<(std::ostream& os, const Expression& e);
/** Checks if @p e is a constant expression. */
bool is_constant(const Expression& e);
/** Checks if @p e is a constant expression representing @p v. */
bool is_constant(const Expression& e, double v);
/** Checks if @p e is 0.0. */
bool is_zero(const Expression& e);
/** Checks if @p e is 1.0. */
bool is_one(const Expression& e);
/** Checks if @p e is -1.0. */
bool is_neg_one(const Expression& e);
/** Checks if @p e is 2.0. */
bool is_two(const Expression& e);
/** Checks if @p e is NaN. */
bool is_nan(const Expression& e);
/** Checks if @p e is a variable expression. */
bool is_variable(const Expression& e);
/** Checks if @p e is an addition expression. */
bool is_addition(const Expression& e);
/** Checks if @p e is a multiplication expression. */
bool is_multiplication(const Expression& e);
/** Checks if @p e is a division expression. */
bool is_division(const Expression& e);
/** Checks if @p e is a log expression. */
bool is_log(const Expression& e);
/** Checks if @p e is an abs expression. */
bool is_abs(const Expression& e);
/** Checks if @p e is an exp expression. */
bool is_exp(const Expression& e);
/** Checks if @p e is a square-root expression. */
bool is_sqrt(const Expression& e);
/** Checks if @p e is a power-function expression. */
bool is_pow(const Expression& e);
/** Checks if @p e is a sine expression. */
bool is_sin(const Expression& e);
/** Checks if @p e is a cosine expression. */
bool is_cos(const Expression& e);
/** Checks if @p e is a tangent expression. */
bool is_tan(const Expression& e);
/** Checks if @p e is an arcsine expression. */
bool is_asin(const Expression& e);
/** Checks if @p e is an arccosine expression. */
bool is_acos(const Expression& e);
/** Checks if @p e is an arctangent expression. */
bool is_atan(const Expression& e);
/** Checks if @p e is an arctangent2 expression. */
bool is_atan2(const Expression& e);
/** Checks if @p e is a hyperbolic-sine expression. */
bool is_sinh(const Expression& e);
/** Checks if @p e is a hyperbolic-cosine expression. */
bool is_cosh(const Expression& e);
/** Checks if @p e is a hyperbolic-tangent expression. */
bool is_tanh(const Expression& e);
/** Checks if @p e is a min expression. */
bool is_min(const Expression& e);
/** Checks if @p e is a max expression. */
bool is_max(const Expression& e);
/** Checks if @p e is a ceil expression. */
bool is_ceil(const Expression& e);
/** Checks if @p e is a floor expression. */
bool is_floor(const Expression& e);
/** Checks if @p e is an if-then-else expression. */
bool is_if_then_else(const Expression& e);
/** Checks if @p e is an uninterpreted-function expression. */
bool is_uninterpreted_function(const Expression& e);
/** Returns the constant value of the constant expression @p e.
* \pre{@p e is a constant expression.}
*/
double get_constant_value(const Expression& e);
/** Returns the embedded variable in the variable expression @p e.
* \pre{@p e is a variable expression.}
*/
const Variable& get_variable(const Expression& e);
/** Returns the argument in the unary expression @p e.
* \pre{@p e is a unary expression.}
*/
const Expression& get_argument(const Expression& e);
/** Returns the first argument of the binary expression @p e.
* \pre{@p e is a binary expression.}
*/
const Expression& get_first_argument(const Expression& e);
/** Returns the second argument of the binary expression @p e.
* \pre{@p e is a binary expression.}
*/
const Expression& get_second_argument(const Expression& e);
/** Returns the constant part of the addition expression @p e. For instance,
* given 7 + 2 * x + 3 * y, it returns 7.
* \pre{@p e is an addition expression.}
*/
double get_constant_in_addition(const Expression& e);
/** Returns the map from an expression to its coefficient in the addition
* expression @p e. For instance, given 7 + 2 * x + 3 * y, the return value
* maps 'x' to 2 and 'y' to 3.
* \pre{@p e is an addition expression.}
*/
const std::map<Expression, double>& get_expr_to_coeff_map_in_addition(
const Expression& e);
/** Returns the constant part of the multiplication expression @p e. For
* instance, given 7 * x^2 * y^3, it returns 7.
* \pre{@p e is a multiplication expression.}
*/
double get_constant_in_multiplication(const Expression& e);
/** Returns the map from a base expression to its exponent expression in the
* multiplication expression @p e. For instance, given 7 * x^2 * y^3 * z^x, the
* return value maps 'x' to 2, 'y' to 3, and 'z' to 'x'.
* \pre{@p e is a multiplication expression.}
*/
const std::map<Expression, Expression>&
get_base_to_exponent_map_in_multiplication(const Expression& e);
/** Returns the name of an uninterpreted-function expression @p e.
* \pre @p e is an uninterpreted-function expression.
*/
const std::string& get_uninterpreted_function_name(const Expression& e);
/** Returns the arguments of an uninterpreted-function expression @p e.
* \pre @p e is an uninterpreted-function expression.
*/
const std::vector<Expression>& get_uninterpreted_function_arguments(
const Expression& e);
/** Returns the conditional formula in the if-then-else expression @p e.
* @pre @p e is an if-then-else expression.
*/
const Formula& get_conditional_formula(const Expression& e);
/** Returns the 'then' expression in the if-then-else expression @p e.
* @pre @p e is an if-then-else expression.
*/
const Expression& get_then_expression(const Expression& e);
/** Returns the 'else' expression in the if-then-else expression @p e.
* @pre @p e is an if-then-else expression.
*/
const Expression& get_else_expression(const Expression& e);
Expression operator+(const Variable& var);
Expression operator-(const Variable& var);
/// Returns the Taylor series expansion of `f` around `a` of order `order`.
///
/// @param[in] f Symbolic expression to approximate using Taylor series
/// expansion.
/// @param[in] a Symbolic environment which specifies the point of
/// approximation. If a partial environment is provided,
/// the unspecified variables are treated as symbolic
/// variables (e.g. decision variable).
/// @param[in] order Positive integer which specifies the maximum order of the
/// resulting polynomial approximating `f` around `a`.
Expression TaylorExpand(const Expression& f, const Environment& a, int order);
} // namespace symbolic
} // namespace drake
namespace std {
/* Provides std::hash<drake::symbolic::Expression>. */
template <>
struct hash<drake::symbolic::Expression> : public drake::DefaultHash {};
#if defined(__GLIBCXX__)
// https://gcc.gnu.org/onlinedocs/libstdc++/manual/unordered_associative.html
template <>
struct __is_fast_hash<hash<drake::symbolic::Expression>> : std::false_type {};
#endif
/* Provides std::less<drake::symbolic::Expression>. */
template <>
struct less<drake::symbolic::Expression> {
bool operator()(const drake::symbolic::Expression& lhs,
const drake::symbolic::Expression& rhs) const {
return lhs.Less(rhs);
}
};
/* Provides std::equal_to<drake::symbolic::Expression>. */
template <>
struct equal_to<drake::symbolic::Expression> {
bool operator()(const drake::symbolic::Expression& lhs,
const drake::symbolic::Expression& rhs) const {
return lhs.EqualTo(rhs);
}
};
/* Provides std::numeric_limits<drake::symbolic::Expression>. */
template <>
struct numeric_limits<drake::symbolic::Expression>
: public std::numeric_limits<double> {};
/// Provides std::uniform_real_distribution, U(a, b), for symbolic expressions.
///
/// When operator() is called, it returns a symbolic expression `a + (b - a) *
/// v` where v is a symbolic random variable associated with the standard
/// uniform distribution.
///
/// @see std::normal_distribution<drake::symbolic::Expression> for the internal
/// representation of this implementation.
template <>
class uniform_real_distribution<drake::symbolic::Expression> {
public:
using RealType = drake::symbolic::Expression;
using result_type = RealType;
/// Constructs a new distribution object with a minimum value @p a and a
/// maximum value @p b.
///
/// @throw std::runtime_error if a and b are constant expressions but a > b.
explicit uniform_real_distribution(RealType a, RealType b = 1.0)
: a_{std::move(a)},
b_{std::move(b)},
random_variables_{std::make_shared<std::vector<Variable>>()} {
if (is_constant(a_) && is_constant(b_) &&
get_constant_value(a_) > get_constant_value(b_)) {
throw std::runtime_error(
"In constructing a uniform_real_distribution<Expression>, we "
"detected that the minimum distribution parameter " +
a_.to_string() +
" is greater than the maximum distribution parameter " +
b_.to_string() + ".");
}
}
/// Constructs a new distribution object with a = 0.0 and b = 1.0.
uniform_real_distribution() : uniform_real_distribution{0.0} {}
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(uniform_real_distribution);
/// Resets the internal state of the distribution object.
void reset() { index_ = 0; }
/// Generates a symbolic expression representing a random value that is
/// distributed according to the associated probability function.
result_type operator()() {
if (random_variables_->size() == index_) {
random_variables_->emplace_back(
"random_uniform_" + std::to_string(index_),
drake::symbolic::Variable::Type::RANDOM_UNIFORM);
}
const drake::symbolic::Variable& v{(*random_variables_)[index_++]};
return a_ + (b_ - a_) * v;
}
/// Generates a symbolic expression representing a random value that is
/// distributed according to the associated probability function.
///
/// @note We provide this method, which takes a random generator, for
/// compatibility with the std::uniform_real_distribution::operator().
template <class Generator>
result_type operator()(Generator&) {
return (*this)();
}
/// Returns the minimum value a.
RealType a() const { return a_; }
/// Returns the maximum value b.
RealType b() const { return b_; }
/// Returns the minimum potentially generated value.
result_type min() const { return a_; }
/// Returns the maximum potentially generated value.
result_type max() const { return b_; }
private:
using Variable = drake::symbolic::Variable;
RealType a_;
RealType b_;
std::shared_ptr<std::vector<Variable>> random_variables_;
std::vector<Variable>::size_type index_{0};
friend bool operator==(
const uniform_real_distribution<drake::symbolic::Expression>& lhs,
const uniform_real_distribution<drake::symbolic::Expression>& rhs) {
return lhs.a().EqualTo(rhs.a()) && lhs.b().EqualTo(rhs.b()) &&
(lhs.index_ == rhs.index_) &&
(lhs.random_variables_ == rhs.random_variables_);
}
};
inline bool operator!=(
const uniform_real_distribution<drake::symbolic::Expression>& lhs,
const uniform_real_distribution<drake::symbolic::Expression>& rhs) {
return !(lhs == rhs);
}
inline std::ostream& operator<<(
std::ostream& os,
const uniform_real_distribution<drake::symbolic::Expression>& d) {
return os << d.a() << " " << d.b();
}
/// Provides std::normal_distribution, N(μ, σ), for symbolic expressions.
///
/// When operator() is called, it returns a symbolic expression `μ + σ * v`
/// where v is a symbolic random variable associated with the standard normal
/// (Gaussian) distribution.
///
/// It keeps a shared pointer to the vector of symbolic random variables that
/// has been created for the following purposes:
///
/// - When `reset()` is called, it rewinds `index_` to zero so that the next
/// operator (re)-uses the first symbolic random variable.
/// @code
/// random_device rd;
/// RandomGenerator g{rd()};
/// std::normal_distribution<Expression> d(0.0, 1.0);
///
/// const Expression e1{d(g)};
/// const Expression e2{d(g)};
/// d.reset();
/// const Expression e3{d(g)};
///
/// EXPECT_FALSE(e1.EqualTo(e2));
/// EXPECT_TRUE(e1.EqualTo(e3));
/// @endcode
///
/// - When an instance of this class is copied, the original and copied
/// distributions share the vector of symbolic random variables. We want to
/// make sure that the two generate identical sequences of elements.
/// @code
/// random_device rd;
/// RandomGenerator g{rd()};
///
/// std::normal_distribution<Expression> d1(0.0, 1.0);
/// std::normal_distribution<Expression> d2(d1);
/// const Expression e1_1{d1(g)};
/// const Expression e1_2{d1(g)};
///
/// const Expression e2_1{d2(g)};
/// const Expression e2_2{d2(g)};
///
/// EXPECT_TRUE(e1_1.EqualTo(e2_1));
/// EXPECT_TRUE(e1_2.EqualTo(e2_2));
/// @endcode
template <>
class normal_distribution<drake::symbolic::Expression> {
public:
using RealType = drake::symbolic::Expression;
using result_type = RealType;
/// Constructs a new distribution object with @p mean and @p stddev.
///
/// @throw std::runtime_error if stddev is a non-positive constant expression.
explicit normal_distribution(RealType mean, RealType stddev = 1.0)
: mean_{std::move(mean)},
stddev_{std::move(stddev)},
random_variables_{std::make_shared<std::vector<Variable>>()} {
if (is_constant(stddev_) && get_constant_value(stddev_) <= 0) {
throw std::runtime_error(
"In constructing a normal_distribution<Expression>, we "
"detected that the stddev distribution parameter " +
stddev_.to_string() + " is non-positive.");
}
}
/// Constructs a new distribution object with mean = 0.0 and stddev = 1.0.
normal_distribution() : normal_distribution{0.0} {}
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(normal_distribution);
/// Resets the internal state of the distribution object.
void reset() { index_ = 0; }
/// Generates a symbolic expression representing a random value that is
/// distributed according to the associated probability function.
result_type operator()() {
if (random_variables_->size() == index_) {
random_variables_->emplace_back(
"random_gaussian_" + std::to_string(index_),