13#include "dlinear/libs/libgmp.h"
14#include "dlinear/util/exception.h"
15#include "dlinear/util/logging.h"
21 case FormulaKind::Geq:
22 return FormulaKind::Leq;
24 return FormulaKind::Lt;
25 case FormulaKind::Leq:
26 return FormulaKind::Geq;
28 return FormulaKind::Gt;
47std::set<Formula>
map(
const std::set<Formula> &formulas,
const std::function<
Formula(
const Formula &)> &func) {
48 std::set<Formula> result;
49 std::transform(formulas.cbegin(), formulas.cend(), std::inserter(result, result.begin()), func);
54 switch (f.get_kind()) {
55 case FormulaKind::False:
56 case FormulaKind::True:
57 case FormulaKind::Var:
59 case FormulaKind::Neq:
61 case FormulaKind::Geq:
63 case FormulaKind::Leq:
64 case FormulaKind::Forall:
66 case FormulaKind::And:
69 case FormulaKind::Not: {
70 const Formula &negated_formula{get_operand(f)};
71 return is_variable(negated_formula) || is_relational(negated_formula);
74 DLINEAR_UNREACHABLE();
85 if (is_conjunction(f)) {
88 if (is_disjunction(f)) {
89 const auto &operands = get_operands(f);
92 std::all_of(operands.cbegin(), operands.cend(), [](
const Formula &formula) { return is_atomic(formula); })};
95 DLINEAR_UNREACHABLE();
99 if (is_conjunction(f)) {
100 DLINEAR_ASSERT(std::all_of(get_operands(f).begin(), get_operands(f).end(),
102 "All operands must be clauses");
103 return get_operands(f);
105 DLINEAR_ASSERT(
is_clause(f),
"Must be a clause");
114 if (is_disjunction(f)) {
117 if (is_conjunction(f)) {
118 const auto &operands = get_operands(f);
120 std::all_of(operands.cbegin(), operands.cend(), [](
const Formula &formula) { return is_clause(formula); })};
123 DLINEAR_UNREACHABLE();
127 auto begin1 = variables1.
begin();
128 auto begin2 = variables2.
begin();
129 const auto end1 = variables1.
end();
130 const auto end2 = variables2.
end();
131 while (begin1 != end1 && begin2 != end2) {
132 if (begin1->less(*begin2)) {
135 if (!begin2->less(*begin1)) {
146class DeltaStrengthenVisitor {
148 DeltaStrengthenVisitor() =
default;
149 [[nodiscard]] Formula Strengthen(
const Formula &f,
const double delta)
const {
153 return Visit(f, delta);
157 [[nodiscard]] Expression Visit(
const Expression &e,
const double delta)
const {
158 return VisitExpression<Expression>(
this, e, delta);
160 [[nodiscard]] Expression VisitVariable(
const Expression &e,
const double)
const {
return e; }
161 [[nodiscard]] Expression VisitConstant(
const Expression &e,
const double)
const {
return e; }
162 [[nodiscard]] Expression VisitAddition(
const Expression &e,
const double delta)
const {
163 Expression ret{get_constant_in_addition(e)};
164 for (
const auto &p : get_expr_to_coeff_map_in_addition(e)) {
165 const Expression &e_i{p.first};
166 const mpq_class &coeff{p.second};
167 ret += coeff * Visit(e_i, delta);
171 [[nodiscard]] Expression VisitMultiplication(
const Expression &e,
const double delta)
const {
172 Expression ret{get_constant_in_multiplication(e)};
173 for (
const auto &p : get_base_to_exponent_map_in_multiplication(e)) {
174 const Expression &base{p.first};
175 const Expression &exponent{p.second};
176 ret *= pow(Visit(base, delta), Visit(exponent, delta));
180 [[nodiscard]] Expression VisitDivision(
const Expression &e,
const double delta)
const {
181 return Visit(get_first_argument(e), delta) / Visit(get_second_argument(e), delta);
183 [[nodiscard]] Expression VisitLog(
const Expression &e,
const double delta)
const {
184 return log(Visit(get_argument(e), delta));
186 [[nodiscard]] Expression VisitAbs(
const Expression &e,
const double delta)
const {
187 return abs(Visit(get_argument(e), delta));
189 [[nodiscard]] Expression VisitExp(
const Expression &e,
const double delta)
const {
190 return exp(Visit(get_argument(e), delta));
192 [[nodiscard]] Expression VisitSqrt(
const Expression &e,
const double delta)
const {
193 return sqrt(Visit(get_argument(e), delta));
195 [[nodiscard]] Expression VisitPow(
const Expression &e,
const double delta)
const {
196 return pow(Visit(get_first_argument(e), delta), Visit(get_second_argument(e), delta));
198 [[nodiscard]] Expression VisitSin(
const Expression &e,
const double delta)
const {
199 return sin(Visit(get_argument(e), delta));
201 [[nodiscard]] Expression VisitCos(
const Expression &e,
const double delta)
const {
202 return cos(Visit(get_argument(e), delta));
204 [[nodiscard]] Expression VisitTan(
const Expression &e,
const double delta)
const {
205 return tan(Visit(get_argument(e), delta));
207 [[nodiscard]] Expression VisitAsin(
const Expression &e,
const double delta)
const {
208 return asin(Visit(get_argument(e), delta));
210 [[nodiscard]] Expression VisitAcos(
const Expression &e,
const double delta)
const {
211 return acos(Visit(get_argument(e), delta));
213 [[nodiscard]] Expression VisitAtan(
const Expression &e,
const double delta)
const {
214 return atan(Visit(get_argument(e), delta));
216 [[nodiscard]] Expression VisitAtan2(
const Expression &e,
const double delta)
const {
217 return atan2(Visit(get_first_argument(e), delta), Visit(get_second_argument(e), delta));
219 [[nodiscard]] Expression VisitSinh(
const Expression &e,
const double delta)
const {
220 return sinh(Visit(get_argument(e), delta));
222 [[nodiscard]] Expression VisitCosh(
const Expression &e,
const double delta)
const {
223 return cosh(Visit(get_argument(e), delta));
225 [[nodiscard]] Expression VisitTanh(
const Expression &e,
const double delta)
const {
226 return tanh(Visit(get_argument(e), delta));
228 [[nodiscard]] Expression VisitMin(
const Expression &e,
const double delta)
const {
229 return min(Visit(get_first_argument(e), delta), Visit(get_second_argument(e), delta));
231 [[nodiscard]] Expression VisitMax(
const Expression &e,
const double delta)
const {
232 return max(Visit(get_first_argument(e), delta), Visit(get_second_argument(e), delta));
234 [[nodiscard]] Expression VisitIfThenElse(
const Expression &e,
const double delta)
const {
235 return if_then_else(Visit(get_conditional_formula(e), delta), Visit(get_then_expression(e), delta),
236 Visit(get_else_expression(e), delta));
238 [[nodiscard]] Expression VisitUninterpretedFunction(
const Expression &e,
const double)
const {
return e; }
240 [[nodiscard]] Formula Visit(
const Formula &f,
const double delta)
const {
241 return VisitFormula<Formula>(
this, f, delta);
243 [[nodiscard]] Formula VisitFalse(
const Formula &f,
const double)
const {
return f; }
244 [[nodiscard]] Formula VisitTrue(
const Formula &f,
const double)
const {
return f; }
245 [[nodiscard]] Formula VisitVariable(
const Formula &f,
const double)
const {
return f; }
246 [[nodiscard]] Formula VisitEqualTo(
const Formula &f,
const double delta)
const {
248 DLINEAR_WARN_FMT(
"Strengthening {} with {} results in false. However, we return {}.", f, delta, f);
253 const Expression lhs{Visit(get_lhs_expression(f), delta)};
254 const Expression rhs{Visit(get_rhs_expression(f), delta)};
255 return VisitGreaterThanOrEqualTo(lhs >= rhs, delta) && VisitLessThanOrEqualTo(lhs <= rhs, delta);
258 [[nodiscard]] Formula VisitNotEqualTo(
const Formula &f,
const double delta)
const {
262 const Expression lhs{Visit(get_lhs_expression(f), delta)};
263 const Expression rhs{Visit(get_rhs_expression(f), delta)};
264 return VisitGreaterThan(lhs > rhs, delta) || VisitLessThan(lhs < rhs, delta);
266 return Formula::True();
269 [[nodiscard]] Formula VisitGreaterThan(
const Formula &f,
const double delta)
const {
274 const Expression lhs{Visit(get_lhs_expression(f), delta)};
275 const Expression rhs{Visit(get_rhs_expression(f), delta)};
276 if (is_variable(rhs)) {
281 return (lhs - delta > rhs);
283 return (lhs > rhs + delta);
286 [[nodiscard]] Formula VisitGreaterThanOrEqualTo(
const Formula &f,
const double delta)
const {
291 const Expression lhs{Visit(get_lhs_expression(f), delta)};
292 const Expression rhs{Visit(get_rhs_expression(f), delta)};
293 if (is_variable(rhs)) {
298 return (lhs - delta >= rhs);
300 return (lhs >= rhs + delta);
303 [[nodiscard]] Formula VisitLessThan(
const Formula &f,
const double delta)
const {
308 const Expression lhs{Visit(get_lhs_expression(f), delta)};
309 const Expression rhs{Visit(get_rhs_expression(f), delta)};
310 if (is_variable(lhs)) {
315 return (lhs < rhs - delta);
317 return (lhs + delta < rhs);
320 [[nodiscard]] Formula VisitLessThanOrEqualTo(
const Formula &f,
const double delta)
const {
325 const Expression lhs{Visit(get_lhs_expression(f), delta)};
326 const Expression rhs{Visit(get_rhs_expression(f), delta)};
327 if (is_variable(lhs)) {
332 return (lhs <= rhs - delta);
334 return (lhs + delta <= rhs);
337 [[nodiscard]] Formula VisitConjunction(
const Formula &f,
const double delta)
const {
338 Formula ret{Formula::True()};
339 for (
const auto &f_i : get_operands(f)) {
340 ret = std::move(ret) && this->Visit(f_i, delta);
344 [[nodiscard]] Formula VisitDisjunction(
const Formula &f,
const double delta)
const {
345 Formula ret{Formula::False()};
346 for (
const auto &f_i : get_operands(f)) {
347 ret = std::move(ret) || this->Visit(f_i, delta);
351 [[nodiscard]] Formula VisitNegation(
const Formula &f,
const double delta)
const {
352 return !Visit(get_operand(f), -delta);
354 [[nodiscard]] Formula VisitForall(
const Formula &,
const double)
const {
355 DLINEAR_RUNTIME_ERROR(
"DeltaStrengthenVisitor: forall formula is not supported.");
360 friend Expression drake::symbolic::VisitExpression<Expression>(
const DeltaStrengthenVisitor *,
const Expression &,
365 friend Formula drake::symbolic::VisitFormula<Formula>(
const DeltaStrengthenVisitor *,
const Formula &,
370class IsDifferentiableVisitor {
372 IsDifferentiableVisitor() =
default;
373 [[nodiscard]]
bool Visit(
const Formula &f)
const {
return VisitFormula<bool>(
this, f); }
374 [[nodiscard]]
bool Visit(
const Expression &e)
const {
return VisitExpression<bool>(
this, e); }
378 [[nodiscard]]
bool VisitFalse(
const Formula &)
const {
return true; }
379 [[nodiscard]]
bool VisitTrue(
const Formula &)
const {
return true; }
380 [[nodiscard]]
bool VisitVariable(
const Formula &)
const {
return true; }
381 [[nodiscard]]
bool VisitEqualTo(
const Formula &f)
const {
382 return Visit(get_lhs_expression(f)) && Visit(get_rhs_expression(f));
384 [[nodiscard]]
bool VisitNotEqualTo(
const Formula &f)
const {
385 return Visit(get_lhs_expression(f)) && Visit(get_rhs_expression(f));
387 [[nodiscard]]
bool VisitGreaterThan(
const Formula &f)
const {
388 return Visit(get_lhs_expression(f)) && Visit(get_rhs_expression(f));
390 [[nodiscard]]
bool VisitGreaterThanOrEqualTo(
const Formula &f)
const {
391 return Visit(get_lhs_expression(f)) && Visit(get_rhs_expression(f));
393 [[nodiscard]]
bool VisitLessThan(
const Formula &f)
const {
394 return Visit(get_lhs_expression(f)) && Visit(get_rhs_expression(f));
396 [[nodiscard]]
bool VisitLessThanOrEqualTo(
const Formula &f)
const {
397 return Visit(get_lhs_expression(f)) && Visit(get_rhs_expression(f));
399 [[nodiscard]]
bool VisitConjunction(
const Formula &f)
const {
400 std::set<Formula> formulae = get_operands(f);
401 return std::all_of(formulae.begin(), formulae.end(), [
this](
const Formula &formula) { return Visit(formula); });
403 [[nodiscard]]
bool VisitDisjunction(
const Formula &f)
const {
404 std::set<Formula> formulae = get_operands(f);
405 return std::all_of(formulae.begin(), formulae.end(), [
this](
const Formula &formula) { return Visit(formula); });
407 [[nodiscard]]
bool VisitNegation(
const Formula &f)
const {
return Visit(get_operand(f)); }
408 [[nodiscard]]
bool VisitForall(
const Formula &)
const {
return false; }
411 [[nodiscard]]
bool VisitVariable(
const Expression &)
const {
return true; }
412 [[nodiscard]]
bool VisitConstant(
const Expression &)
const {
return true; }
413 [[nodiscard]]
bool VisitAddition(
const Expression &e)
const {
414 for (
const auto &p : get_expr_to_coeff_map_in_addition(e)) {
415 const Expression &e_i{p.first};
422 [[nodiscard]]
bool VisitMultiplication(
const Expression &e)
const {
423 for (
const auto &p : get_base_to_exponent_map_in_multiplication(e)) {
424 const Expression &base{p.first};
425 const Expression &exponent{p.second};
426 if (!Visit(base) || !Visit(exponent)) {
432 [[nodiscard]]
bool VisitDivision(
const Expression &e)
const {
433 return Visit(get_first_argument(e)) && Visit(get_second_argument(e));
435 [[nodiscard]]
bool VisitLog(
const Expression &e)
const {
return Visit(get_argument(e)); }
436 [[nodiscard]]
bool VisitAbs(
const Expression &)
const {
return false; }
437 [[nodiscard]]
bool VisitExp(
const Expression &e)
const {
return Visit(get_argument(e)); }
438 [[nodiscard]]
bool VisitSqrt(
const Expression &e)
const {
return Visit(get_argument(e)); }
439 [[nodiscard]]
bool VisitPow(
const Expression &e)
const {
440 return Visit(get_first_argument(e)) && Visit(get_second_argument(e));
442 [[nodiscard]]
bool VisitSin(
const Expression &e)
const {
return Visit(get_argument(e)); }
443 [[nodiscard]]
bool VisitCos(
const Expression &e)
const {
return Visit(get_argument(e)); }
444 [[nodiscard]]
bool VisitTan(
const Expression &e)
const {
return Visit(get_argument(e)); }
445 [[nodiscard]]
bool VisitAsin(
const Expression &e)
const {
return Visit(get_argument(e)); }
446 [[nodiscard]]
bool VisitAcos(
const Expression &e)
const {
return Visit(get_argument(e)); }
447 [[nodiscard]]
bool VisitAtan(
const Expression &e)
const {
return Visit(get_argument(e)); }
448 [[nodiscard]]
bool VisitAtan2(
const Expression &e)
const {
449 return Visit(get_first_argument(e)) && Visit(get_second_argument(e));
451 [[nodiscard]]
bool VisitSinh(
const Expression &e)
const {
return Visit(get_argument(e)); }
452 [[nodiscard]]
bool VisitCosh(
const Expression &e)
const {
return Visit(get_argument(e)); }
453 [[nodiscard]]
bool VisitTanh(
const Expression &e)
const {
return Visit(get_argument(e)); }
454 [[nodiscard]]
bool VisitMin(
const Expression &)
const {
return false; }
455 [[nodiscard]]
bool VisitMax(
const Expression &)
const {
return false; }
456 [[nodiscard]]
bool VisitIfThenElse(
const Expression &)
const {
return false; }
457 [[nodiscard]]
bool VisitUninterpretedFunction(
const Expression &)
const {
return false; }
461 friend bool drake::symbolic::VisitFormula<bool>(
const IsDifferentiableVisitor *,
const Formula &);
464 friend bool drake::symbolic::VisitExpression<bool>(
const IsDifferentiableVisitor *,
const Expression &);
471 DLINEAR_ASSERT(delta > 0,
"delta must be positive.");
472 return DeltaStrengthenVisitor{}.Strengthen(f, delta);
477 DLINEAR_ASSERT(delta > 0,
"delta must be positive.");
478 return DeltaStrengthenVisitor{}.Strengthen(f, -delta);
487 for (
const auto &f_i : formulas) {
488 ret = std::move(ret) && f_i;
495 for (
const auto &f_i : formulas) {
496 ret = std::move(ret) || f_i;
502 DLINEAR_ASSERT(!prefix.empty(),
"prefix must not be empty.");
503 DLINEAR_ASSERT(size >= 1,
"size must be positive.");
504 std::vector<Variable> v;
506 for (
int i = 0; i < size; ++i) {
507 v.emplace_back(prefix + std::to_string(i), type);
527 DLINEAR_UNREACHABLE();
546 DLINEAR_UNREACHABLE();
550std::ostream &operator<<(std::ostream &os,
const FormulaKind &kind) {
552 case FormulaKind::False:
553 return os <<
"False";
554 case FormulaKind::True:
556 case FormulaKind::Var:
558 case FormulaKind::Eq:
560 case FormulaKind::Neq:
562 case FormulaKind::Gt:
564 case FormulaKind::Geq:
566 case FormulaKind::Lt:
568 case FormulaKind::Leq:
570 case FormulaKind::And:
572 case FormulaKind::Or:
574 case FormulaKind::Not:
576 case FormulaKind::Forall:
577 return os <<
"Forall";
579 DLINEAR_UNREACHABLE();
583std::ostream &operator<<(std::ostream &os,
const ExpressionKind &kind) {
585 case ExpressionKind::Var:
587 case ExpressionKind::Constant:
588 return os <<
"Constant";
589 case ExpressionKind::Infty:
590 return os <<
"Infty";
591 case ExpressionKind::NaN:
593 case ExpressionKind::Add:
594 return os <<
"Addition";
595 case ExpressionKind::Mul:
596 return os <<
"Multiplication";
597 case ExpressionKind::Div:
598 return os <<
"Division";
599 case ExpressionKind::Log:
601 case ExpressionKind::Abs:
603 case ExpressionKind::Exp:
605 case ExpressionKind::Sqrt:
607 case ExpressionKind::Pow:
609 case ExpressionKind::Sin:
611 case ExpressionKind::Cos:
613 case ExpressionKind::Tan:
615 case ExpressionKind::Asin:
617 case ExpressionKind::Acos:
619 case ExpressionKind::Atan:
621 case ExpressionKind::Atan2:
622 return os <<
"Atan2";
623 case ExpressionKind::Sinh:
625 case ExpressionKind::Cosh:
627 case ExpressionKind::Tanh:
629 case ExpressionKind::Min:
631 case ExpressionKind::Max:
633 case ExpressionKind::IfThenElse:
634 return os <<
"IfThenElse";
635 case ExpressionKind::UninterpretedFunction:
636 return os <<
"UninterpretedFunction";
638 DLINEAR_UNREACHABLE();
Represents a symbolic form of an expression.
Represents a symbolic variable.
Type
Supported types of symbolic variables.
Represents a set of variables.
iterator begin()
Returns an iterator to the beginning.
iterator end()
Returns an iterator to the end.
Global namespace for the dlinear library.
Formula make_conjunction(const std::vector< Formula > &formulas)
Make conjunction of formulas.
Formula DeltaWeaken(const Formula &f, const double delta)
Weaken the input formula $p f by delta.
std::set< Formula > map(const std::set< Formula > &formulas, const std::function< Formula(const Formula &)> &func)
Given formulas = {f₁, ..., fₙ} and a func : Formula → Formula, map(formulas, func) returns a set {fun...
std::set< Formula > get_clauses(const Formula &f)
Returns the set of clauses in f.
bool is_cnf(const Formula &f)
Check if f is in CNF form.
RelationalOperator
Relational operators are used in formulas.
bool IsDifferentiable(const Formula &f)
Check if the formula f is symbolic-differentiable.
LpColBound operator!(LpColBound bound)
Invert the bound with delta == 0.
Formula iff(const Formula &f1, const Formula &f2)
Return a formula f1 ⇔ f2.
Formula imply(const Formula &f1, const Formula &f2)
Return a formula f1 ⇒ f2.
Formula DeltaStrengthen(const Formula &f, const double delta)
Strengthen the input formula $p f by delta.
Formula make_disjunction(const std::vector< Formula > &formulas)
Make disjunction of formulas.
LpColBound operator-(LpColBound bound)
Invert the bound with delta > 0.
std::vector< Variable > CreateVector(const std::string &prefix, const int size, const Variable::Type type)
Creates a vector of variables of type whose size is size.
bool is_clause(const Formula &f)
Check if f is a clause.
bool HaveIntersection(const Variables &variables1, const Variables &variables2)
Check if the intersection of variables1 and variables2 is non-empty.
bool is_atomic(const Formula &f)
Check if f is atomic.