Drake
mathematical_program.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <array>
4 #include <cstddef>
5 #include <iostream>
6 #include <limits>
7 #include <list>
8 #include <map>
9 #include <memory>
10 #include <set>
11 #include <stdexcept>
12 #include <string>
13 #include <type_traits>
14 #include <unordered_map>
15 #include <utility>
16 #include <vector>
17 
18 #include <Eigen/Core>
19 
20 #include "drake/common/autodiff.h"
21 #include "drake/common/drake_assert.h"
22 #include "drake/common/drake_copyable.h"
23 #include "drake/common/drake_deprecated.h"
24 #include "drake/common/drake_optional.h"
25 #include "drake/common/eigen_types.h"
26 #include "drake/common/polynomial.h"
27 #include "drake/common/symbolic.h"
28 #include "drake/solvers/binding.h"
29 #include "drake/solvers/constraint.h"
30 #include "drake/solvers/cost.h"
31 #include "drake/solvers/create_constraint.h"
32 #include "drake/solvers/create_cost.h"
33 #include "drake/solvers/decision_variable.h"
34 #include "drake/solvers/function.h"
35 #include "drake/solvers/indeterminate.h"
36 #include "drake/solvers/mathematical_program_solver_interface.h"
37 
38 namespace drake {
39 namespace solvers {
40 
41 /** @defgroup solvers Formulating and Solving Optimization Problems
42  * @{
43  * Drake wraps a number of commercial solvers (+ a few custom solvers) to
44  * provide a common interface for convex optimization, mixed-integer convex
45  * optimization, and other non-convex mathematical programs.
46  *
47  * The MathematicalProgram class handles the coordination of decision variables,
48  * objectives, and constraints. The MathematicalProgram::Solve() method
49  * reflects on the accumulated objectives and constraints and will dispatch to
50  * the most appropriate solver. Alternatively, one can invoke specific solver
51  * by instantiating its MathematicalProgramSolverInterface and passing the
52  * MathematicalProgram directly to the
53  * MathematicalProgramSolverInterface::Solve() method.
54  *
55  * Our solver coverage still has many gaps, but is under active development.
56  *
57  * <b>Closed-form solutions</b>
58  *
59  * The LinearSystemSolver and EqualityConstrainedQPSolver classes provide
60  * efficient closed-form solutions to these special cases.
61  *
62  * <b>Convex Optimization</b>
63  *
64  * <table>
65  * <tr>
66  * <td>Solver</td>
67  * <td><a href="https://en.wikipedia.org/wiki/Linear_programming">LP</a></td>
68  * <td><a href="https://en.wikipedia.org/wiki/Quadratic_programming">
69  * QP</a></td>
70  * <td><a href="https://en.wikipedia.org/wiki/Second-order_cone_programming">
71  * SOCP</a></td>
72  * <td><a href="https://en.wikipedia.org/wiki/Semidefinite_programming">
73  * SDP</a></td>
74  * <td><a href="https://en.wikipedia.org/wiki/Sum-of-squares_optimization">
75  * SOS</a></td>
76  * </tr>
77  * <tr><td>&dagger; <a href="https://www.gurobi.com/products/gurobi-optimizer">
78  * Gurobi</a></td>
79  * <td align="center">&diams;</td>
80  * <td align="center">&diams;</td>
81  * <td align="center">&diams;</td>
82  * <td></td>
83  * <td></td>
84  * </tr>
85  * <tr><td>&dagger; <a href="https://www.mosek.com/products/mosek">
86  * Mosek</a></td>
87  * <td align="center">&diams;</td>
88  * <td align="center">&diams;</td>
89  * <td align="center">&diams;</td>
90  * <td align="center">&diams;</td>
91  * <td align="center">&diams;</td>
92  * </tr>
93  * <tr><td> <a href="https://github.com/cvxgrp/scs">
94  * SCS</a></td>
95  * <td align="center">&diams;</td>
96  * <td align="center">&diams;</td>
97  * <td align="center">&diams;</td>
98  * <td align="center">&diams;</td>
99  * <td align="center">&diams;</td>
100  * </tr>
101  * <tr><td> <a href="https://github.com/oxfordcontrol/osqp">
102  * OSQP</a></td>
103  * <td align="center">&diams;</td>
104  * <td align="center">&diams;</td>
105  * <td></td>
106  * <td></td>
107  * <td></td>
108  * </tr>
109  * </table>
110  *
111  * <b>Mixed-Integer Convex Optimization</b>
112  *
113  * <table>
114  * <tr>
115  * <td>Solver</td>
116  * <td>MILP</a></td>
117  * <td>MIQP</a></td>
118  * <td>MISOCP</a></td>
119  * <td>MISDP</a></td>
120  * </tr>
121  * <tr><td>&dagger; <a href="https://www.gurobi.com/products/gurobi-optimizer">
122  * Gurobi</a></td>
123  * <td align="center">&diams;</td>
124  * <td align="center">&diams;</td>
125  * <td align="center">&diams;</td>
126  * <td></td>
127  * </tr>
128  * <tr><td>&dagger; <a href="https://www.mosek.com/products/mosek">
129  * Mosek</a></td>
130  * <td align="center">&diams;</td>
131  * <td align="center">&diams;</td>
132  * <td align="center">&diams;</td>
133  * <td></td>
134  * </tr>
135  * </table>
136  *
137  * <b>Nonconvex Programming</b>
138  *
139  * <table>
140  * <tr>
141  * <td>Solver</td>
142  * <td><a href="https://en.wikipedia.org/wiki/Nonlinear_programming">
143  * Nonlinear Program</a></td>
144  * <td><a href="https://en.wikipedia.org/wiki/Linear_complementarity_problem">
145  * LCP</a></td>
146  * <td><a href="https://en.wikipedia.org/wiki/Satisfiability_modulo_theories">
147  * SMT</a></td>
148  * </tr>
149  * <tr><td>&dagger;
150  * <a href="http://www.sbsi-sol-optimize.com/asp/sol_product_snopt.htm">
151  * SNOPT</a></td></tr>
152  * <td align="center">&diams;</td>
153  * <td></td>
154  * <td></td>
155  * <tr><td><a href="https://projects.coin-or.org/Ipopt">Ipopt</a></td></tr>
156  * <td align="center">&diams;</td>
157  * <td></td>
158  * <td></td>
159  * <tr><td><a href="http://ab-initio.mit.edu/wiki/index.php/NLopt">
160  * NLopt</a></td></tr>
161  * <td align="center">&diams;</td>
162  * <td></td>
163  * <td></td>
164  * <tr><td><a href="https://github.com/PositronicsLab/Moby">
165  * Moby LCP</a></td>
166  * <td></td>
167  * <td align="center">&diams;</td>
168  * <td></td>
169  * <tr><td><a href="https://dreal.github.io/">dReal</a></td>
170  * <td></td>
171  * <td></td>
172  * <td align="center">&diams;</td>
173  * </tr>
174  * </table>
175  *
176  * &dagger; indicates that this is a commercial solver which requires a license
177  * (note that some have free licenses for academics).
178  * @}
179  */
180 
181 class MathematicalProgram;
182 
185  kError = 1 << 0, ///< Do not use, to avoid & vs. && typos.
186  kGenericCost = 1 << 1,
188  kQuadraticCost = 1 << 3,
190  kLinearCost = 1 << 5,
197  kBinaryVariable = 1 << 12,
198  kCallback = 1 << 13
199 };
200 typedef uint32_t AttributesSet;
201 
202 template <int...>
204 /**
205  * The type of the names for the newly added variables.
206  * @tparam Size If Size is a fixed non-negative integer, then the type of the
207  * name is std::array<std::string, Size>. Otherwise the type is
208  * std::vector<std::string>.
209  */
210 template <int Size>
212  typedef std::array<std::string, Size> type;
213 };
214 
215 template <>
217  typedef std::vector<std::string> type;
218 };
219 
220 template <int Rows, int Cols>
223 
224 template <int Rows>
226  : public NewVariableNames<Rows == Eigen::Dynamic ? Eigen::Dynamic
227  : Rows*(Rows + 1) / 2> {};
228 
229 namespace internal {
230 /**
231  * Return un-initialized new variable names.
232  */
233 template <int Size>
234 typename std::enable_if<Size >= 0, typename NewVariableNames<Size>::type>::type
237  return names;
238 }
239 
240 /**
241  * Return un-initialized new variable names.
242  */
243 template <int Size>
244 typename std::enable_if<Size == Eigen::Dynamic,
245  typename NewVariableNames<Size>::type>::type
248  return names;
249 }
250 /**
251  * Set the names of the newly added variables.
252  * @param name The common name of all new variables.
253  * @param rows The number of rows in the new variables.
254  * @param cols The number of columns in the new variables.
255  * @pre The size of @p names is @p rows * @p cols.
256  */
257 template <typename Derived>
258 void SetVariableNames(const std::string& name, int rows, int cols,
259  Derived* names) {
260  DRAKE_DEMAND(static_cast<int>(names->size()) == rows * cols);
261  if (cols == 1) {
262  for (int i = 0; i < rows; ++i) {
263  (*names)[i] = name + "(" + std::to_string(i) + ")";
264  }
265  } else {
266  for (int j = 0; j < cols; ++j) {
267  for (int i = 0; i < rows; ++i) {
268  (*names)[j * rows + i] =
269  name + "(" + std::to_string(i) + "," + std::to_string(j) + ")";
270  }
271  }
272  }
273 }
274 } // namespace internal
275 
276 namespace detail {
277 /**
278  * Template condition to only catch when Constraints are inadvertently passed
279  * as an argument. If the class is binding-compatible with a Constraint, then
280  * this will provide a static assertion to enable easier debugging of which
281  * type failed.
282  * @tparam F The type to be tested.
283  * @see http://stackoverflow.com/a/13366183/7829525
284  */
285 template <typename F>
288  // Use deferred evaluation
289  static_assert(
290  !value,
291  "You cannot pass a Constraint to create a Cost object from a function. "
292  "Please ensure you are passing a Cost.");
293 };
294 } // namespace detail
295 
296 /**
297  * MathematicalProgram stores the decision variables, the constraints and costs
298  * of an optimization problem. The user can solve the problem by calling Solve()
299  * function, and obtain the results of the optimization.
300  */
302  public:
304  using VarType = symbolic::Variable::Type;
305 
306  /// The optimal cost is +∞ when the problem is globally infeasible.
307  static constexpr double kGlobalInfeasibleCost =
308  std::numeric_limits<double>::infinity();
309  /// The optimal cost is -∞ when the problem is unbounded.
310  static constexpr double kUnboundedCost =
311  -std::numeric_limits<double>::infinity();
312 
314  virtual ~MathematicalProgram();
315 
316  /** Clones an optimization program.
317  * The clone will be functionally equivalent to the source program with the
318  * same:
319  * - decision variables
320  * - constraints
321  * - costs
322  * - solver settings
323  * - initial guess
324  * However, the clone's x values will be initialized to NaN, and all internal
325  * solvers will be freshly constructed.
326  * @retval new_prog. The newly constructed mathematical program.
327  */
328  std::unique_ptr<MathematicalProgram> Clone() const;
329 
330  /**
331  * Adds continuous variables, appending them to an internal vector of any
332  * existing vars.
333  * The initial guess values for the new variables are set to NaN, to
334  * indicate that an initial guess has not been assigned.
335  * Callers are expected to add costs
336  * and/or constraints to have any effect during optimization.
337  * Callers can also set the initial guess of the decision variables through
338  * SetInitialGuess() or SetInitialGuessForAllVariables().
339  * @param rows The number of rows in the new variables.
340  * @param name The name of the newly added variables
341  * @return The VectorDecisionVariable of size rows x 1, containing the new
342  * vars (not all the vars stored).
343  *
344  * Example:
345  * @code{.cc}
346  * MathematicalProgram prog;
347  * auto x = prog.NewContinuousVariables(2, "x");
348  * @endcode
349  * This adds a 2 x 1 vector containing decision variables into the program.
350  * The names of the variables are "x(0)" and "x(1)".
351  *
352  * The name of the variable is only used for the user in order to ease
353  * readability.
354  */
355  VectorXDecisionVariable NewContinuousVariables(
356  int rows, const std::string& name = "x") {
357  return NewContinuousVariables<Eigen::Dynamic, 1>(rows, 1, name);
358  }
359 
360  /**
361  * Adds continuous variables, appending them to an internal vector of any
362  * existing vars.
363  * The initial guess values for the new variables are set to NaN, to
364  * indicate that an initial guess has not been assigned.
365  * Callers are expected to add costs
366  * and/or constraints to have any effect during optimization.
367  * Callers can also set the initial guess of the decision variables through
368  * SetInitialGuess() or SetInitialGuessForAllVariables().
369  * @tparam Rows The number of rows of the new variables, in the compile time.
370  * @tparam Cols The number of columns of the new variables, in the compile
371  * time.
372  * @param rows The number of rows in the new variables. When Rows is not
373  * Eigen::Dynamic, rows is ignored.
374  * @param cols The number of columns in the new variables. When Cols is not
375  * Eigen::Dynamic, cols is ignored.
376  * @param name All variables will share the same name, but different index.
377  * @return The MatrixDecisionVariable of size Rows x Cols, containing the new
378  * vars (not all the vars stored).
379  *
380  * Example:
381  * @code{.cc}
382  * MathematicalProgram prog;
383  * auto x = prog.NewContinuousVariables(2, 3, "X");
384  * auto y = prog.NewContinuousVariables<2, 3>(2, 3, "X");
385  * @endcode
386  * This adds a 2 x 3 matrix decision variables into the program.
387  *
388  * The name of the variable is only used for the user in order to ease
389  * readability.
390  */
391  template <int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
393  int rows, int cols, const std::string& name) {
394  rows = Rows == Eigen::Dynamic ? rows : Rows;
395  cols = Cols == Eigen::Dynamic ? cols : Cols;
396  auto names =
398  rows * cols);
399  internal::SetVariableNames(name, rows, cols, &names);
400  return NewVariables<Rows, Cols>(VarType::CONTINUOUS, names, rows, cols);
401  }
402 
403  /**
404  * Adds continuous variables, appending them to an internal vector of any
405  * existing vars.
406  * The initial guess values for the new variables are set to NaN, to
407  * indicate that an initial guess has not been assigned.
408  * Callers are expected to add costs
409  * and/or constraints to have any effect during optimization.
410  * Callers can also set the initial guess of the decision variables through
411  * SetInitialGuess() or SetInitialGuessForAllVariables().
412  * @tparam Rows The number of rows in the new variables.
413  * @tparam Cols The number of columns in the new variables. The default is 1.
414  * @param name All variables will share the same name, but different index.
415  * @return The MatrixDecisionVariable of size rows x cols, containing the new
416  * vars (not all the vars stored).
417  *
418  * Example:
419  * @code{.cc}
420  * MathematicalProgram prog;
421  * auto x = prog.NewContinuousVariables<2, 3>("X");
422  * @endcode
423  * This adds a 2 x 3 matrix decision variables into the program.
424  *
425  * The name of the variable is only used for the user in order to ease
426  * readability.
427  */
428  template <int Rows, int Cols = 1>
430  const std::string& name = "X") {
431  return NewContinuousVariables<Rows, Cols>(Rows, Cols, name);
432  }
433 
434  /**
435  * Adds binary variables, appending them to an internal vector of any
436  * existing vars.
437  * The initial guess values for the new variables are set to NaN, to
438  * indicate that an initial guess has not been assigned.
439  * Callers are expected to add costs
440  * and/or constraints to have any effect during optimization.
441  * Callers can also set the initial guess of the decision variables through
442  * SetInitialGuess() or SetInitialGuessForAllVariables().
443  * @tparam Rows The number of rows in the new variables.
444  * @tparam Cols The number of columns in the new variables.
445  * @param rows The number of rows in the new variables.
446  * @param cols The number of columns in the new variables.
447  * @param name The commonly shared name of the new variables.
448  * @return The MatrixDecisionVariable of size rows x cols, containing the new
449  * vars (not all the vars stored).
450  *
451  * Example:
452  * @code{.cc}
453  * MathematicalProgram prog;
454  * auto b = prog.NewBinaryVariables(2, 3, "b");
455  * @endcode
456  * This adds a 2 x 3 matrix decision variables into the program.
457  *
458  * The name of the variable is only used for the user in order to ease
459  * readability.
460  */
461  template <int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
463  int rows, int cols, const std::string& name) {
464  rows = Rows == Eigen::Dynamic ? rows : Rows;
465  cols = Cols == Eigen::Dynamic ? cols : Cols;
466  auto names =
468  rows * cols);
469  internal::SetVariableNames(name, rows, cols, &names);
470  return NewVariables<Rows, Cols>(VarType::BINARY, names, rows, cols);
471  }
472 
473  /**
474  * Adds a matrix of binary variables into the optimization program.
475  * @tparam Rows The number of rows in the newly added binary variables.
476  * @tparam Cols The number of columns in the new variables. The default is 1.
477  * @param name Each newly added binary variable will share the same name. The
478  * default name is "b".
479  * @return A matrix containing the newly added variables.
480  */
481  template <int Rows, int Cols = 1>
483  const std::string& name = "b") {
484  return NewBinaryVariables<Rows, Cols>(Rows, Cols, name);
485  }
486 
487  /**
488  * Adds binary variables to this MathematicalProgram. The new variables are
489  * viewed as a column vector, with size @p rows x 1.
490  * @see NewBinaryVariables(int rows, int cols, const
491  * std::vector<std::string>& names);
492  */
494  const std::string& name = "b") {
495  return NewBinaryVariables<Eigen::Dynamic, 1>(rows, 1, name);
496  }
497 
498  /**
499  * Adds a runtime sized symmetric matrix as decision variables to
500  * this MathematicalProgram.
501  * The optimization will only use the stacked columns of the
502  * lower triangular part of the symmetric matrix as decision variables.
503  * @param rows The number of rows in the symmetric matrix.
504  * @param name The name of the matrix. It is only used the for user to
505  * understand the optimization program. The default name is "Symmetric", and
506  * each variable will be named as
507  * <pre>
508  * Symmetric(0, 0) Symmetric(1, 0) ... Symmetric(rows-1, 0)
509  * Symmetric(1, 0) Symmetric(1, 1) ... Symmetric(rows-1, 1)
510  * ...
511  * Symmetric(rows-1,0) Symmetric(rows-1,1) ... Symmetric(rows-1, rows-1)
512  * </pre>
513  * Notice that the (i,j)'th entry and (j,i)'th entry has the same name.
514  * @return The newly added decision variables.
515  */
516  MatrixXDecisionVariable NewSymmetricContinuousVariables(
517  int rows, const std::string& name = "Symmetric");
518 
519  /**
520  * Adds a static sized symmetric matrix as decision variables to
521  * this MathematicalProgram.
522  * The optimization will only use the stacked columns of the
523  * lower triangular part of the symmetric matrix as decision variables.
524  * @tparam rows The number of rows in the symmetric matrix.
525  * @param name The name of the matrix. It is only used the for user to
526  * understand the optimization program. The default name is "Symmetric", and
527  * each variable will be named as
528  * <pre>
529  * Symmetric(0, 0) Symmetric(1, 0) ... Symmetric(rows-1, 0)
530  * Symmetric(1, 0) Symmetric(1, 1) ... Symmetric(rows-1, 1)
531  * ...
532  * Symmetric(rows-1,0) Symmetric(rows-1,1) ... Symmetric(rows-1, rows-1)
533  * </pre>
534  * Notice that the (i,j)'th entry and (j,i)'th entry has the same name.
535  * @return The newly added decision variables.
536  */
537  template <int rows>
539  const std::string& name = "Symmetric") {
541  int var_count = 0;
542  for (int j = 0; j < static_cast<int>(rows); ++j) {
543  for (int i = j; i < static_cast<int>(rows); ++i) {
544  names[var_count] =
545  name + "(" + std::to_string(i) + "," + std::to_string(j) + ")";
546  ++var_count;
547  }
548  }
549  return NewSymmetricVariables<rows>(VarType::CONTINUOUS, names);
550  }
551 
552  /** Appends new variables to the end of the existing variables.
553  * @param decision_variables The newly added decision_variables.
554  * @pre `decision_variables` should not intersect with the existing variables
555  * or indeterminates in the optimization program.
556  * @pre Each entry in `decision_variables` should not be a dummy variable.
557  * @throw runtime_error if the preconditions are not satisfied.
558  */
559  void AddDecisionVariables(
560  const Eigen::Ref<const VectorXDecisionVariable>& decision_variables);
561 
562  /**
563  * Returns a free polynomial in a monomial basis over @p indeterminates of a
564  * given @p degree. It uses @p coeff_name to make new decision variables and
565  * use them as coefficients. For example, `NewFreePolynomial({x₀, x₁}, 2)`
566  * returns a₀x₁² + a₁x₀x₁ + a₂x₀² + a₃x₁ + a₄x₀ + a₅.
567  */
568  symbolic::Polynomial NewFreePolynomial(
569  const symbolic::Variables& indeterminates, int degree,
570  const std::string& coeff_name = "a");
571 
572  /** Returns a pair of a SOS polynomial p = mᵀQm and a PSD constraint for
573  * a new coefficients matrix Q, where m is the @p monomial basis.
574  * For example, `NewSosPolynomial(Vector2<Monomial>{x,y})` returns a
575  * polynomial
576  * p = Q₍₀,₀₎x² + 2Q₍₁,₀₎xy + Q₍₁,₁₎y²
577  * and a PSD constraint over Q.
578  * Note: Q is a symmetric monomial_basis.rows() x monomial_basis.rows()
579  * matrix.
580  */
581  std::pair<symbolic::Polynomial, Binding<PositiveSemidefiniteConstraint>>
582  NewSosPolynomial(
583  const Eigen::Ref<const VectorX<symbolic::Monomial>>& monomial_basis);
584 
585  /** Returns a pair of a SOS polynomial p = m(x)ᵀQm(x) of degree @p degree
586  * and a PSD constraint for the coefficients matrix Q, where m(x) is the
587  * result of calling `MonomialBasis(indeterminates, degree/2)`. For example,
588  * `NewSosPolynomial({x}, 4)` returns a pair of a polynomial
589  * p = Q₍₀,₀₎x⁴ + 2Q₍₁,₀₎ x³ + (2Q₍₂,₀₎ + Q₍₁,₁₎)x² + 2Q₍₂,₁₎x + Q₍₂,₂₎
590  * and a PSD constraint over Q.
591  *
592  * @throws std::runtime_error if @p degree is not a positive even integer.
593  * @see MonomialBasis.
594  */
595  std::pair<symbolic::Polynomial, Binding<PositiveSemidefiniteConstraint>>
596  NewSosPolynomial(const symbolic::Variables& indeterminates, int degree);
597 
598 
599  /**
600  * Adds indeterminates, appending them to an internal vector of any
601  * existing indeterminates.
602  * @tparam rows The number of rows in the new indeterminates.
603  * @tparam cols The number of columns in the new indeterminates.
604  * @param names A vector of strings containing the name for each variable.
605  * @return The MatrixIndeterminate of size rows x cols, containing the
606  * new vars (not all the vars stored).
607  *
608  * Example:
609  * @code{.cc}
610  * MathematicalProgram prog;
611  * std::array<std::string, 6> names = {"x1", "x2", "x3", "x4", "x5", "x6"};
612  * auto x = prog.NewIndeterminates<2, 3>(names);
613  * @endcode
614  * This adds a 2 x 3 matrix indeterminates into the program.
615  *
616  * The name of the indeterminates is only used for the user in order to ease
617  * readability.
618  */
619  template <int rows, int cols>
621  const std::array<std::string, rows * cols>& names) {
622  MatrixIndeterminate<rows, cols> indeterminates_matrix;
623  NewIndeterminates_impl(names, indeterminates_matrix);
624  return indeterminates_matrix;
625  }
626 
627  /**
628  * Adds indeterminates, appending them to an internal vector of any
629  * existing indeterminates.
630  * @tparam rows The number of rows in the new indeterminates.
631  * @tparam cols The number of columns in the new indeterminates.
632  * @param names A vector of strings containing the name for each variable.
633  * @return The MatrixIndeterminate of size rows x cols, containing the
634  * new vars (not all the vars stored).
635  *
636  * Example:
637  * @code{.cc}
638  * MathematicalProgram prog;
639  * std::array<std::string, 2> names = {"x1", "x2"};
640  * auto x = prog.NewIndeterminates<2>(names);
641  * @endcode
642  * This adds a 2 vector indeterminates into the program.
643  *
644  * The name of the indeterminates is only used for the user in order to ease
645  * readability.
646  */
647  template <int rows>
649  const std::array<std::string, rows>& names) {
650  return NewIndeterminates<rows, 1>(names);
651  }
652 
653  /**
654  * Adds indeterminates, appending them to an internal vector of any
655  * existing indeterminates.
656  * @tparam rows The number of rows in the new indeterminates.
657  * @tparam cols The number of columns in the new indeterminates.
658  * @param names A vector of strings containing the name for each variable.
659  * @return The MatrixIndeterminate of size rows x cols, containing the
660  * new vars (not all the vars stored).
661  *
662  * Example:
663  * @code{.cc}
664  * MathematicalProgram prog;
665  * auto x = prog.NewIndeterminates<2, 3>("X");
666  * @endcode
667  * This adds a 2 x 3 matrix indeterminates into the program.
668  *
669  * The name of the indeterminates is only used for the user in order to ease
670  * readability.
671  */
672 
673  template <int rows, int cols>
675  const std::string& name = "X") {
676  std::array<std::string, rows * cols> names;
677  for (int j = 0; j < cols; ++j) {
678  for (int i = 0; i < rows; ++i) {
679  names[j * rows + i] =
680  name + "(" + std::to_string(i) + "," + std::to_string(j) + ")";
681  }
682  }
683  return NewIndeterminates<rows, cols>(names);
684  }
685 
686  /**
687  * Adds indeterminates to the program.
688  * The name for all newly added indeterminates are set to @p name. The default
689  * name is "x"
690  * @see NewIndeterminates(const std::array<std::string, rows>& names)
691  */
692  template <int rows>
693  VectorIndeterminate<rows> NewIndeterminates(const std::string& name = "x") {
694  std::array<std::string, rows> names;
695  int offset = (name.compare("x") == 0) ? num_vars() : 0;
696  for (int i = 0; i < rows; ++i) {
697  names[i] = name + "(" + std::to_string(offset + i) + ")";
698  }
699  return NewIndeterminates<rows>(names);
700  }
701 
702  /**
703  * Adds indeterminates to this MathematicalProgram.
704  * @see NewIndeterminates(int rows, int cols, const
705  * std::vector<std::string>& names);
706  */
707  VectorXIndeterminate NewIndeterminates(int rows,
708  const std::vector<std::string>& names);
709 
710  /**
711  * Adds indeterminates to this MathematicalProgram, with default name
712  * "x".
713  * @see NewIndeterminates(int rows, int cols, const
714  * std::vector<std::string>& names);
715  */
716  VectorXIndeterminate NewIndeterminates(int rows,
717  const std::string& name = "x");
718 
719  /**
720  * Adds indeterminates, appending them to an internal vector of any
721  * existing vars.
722  * @param rows The number of rows in the new indeterminates.
723  * @param cols The number of columns in the new indeterminates.
724  * @param names A vector of strings containing the name for each variable.
725  * @return The MatrixIndeterminate of size rows x cols, containing the
726  * new vars (not all the vars stored).
727  *
728  * Example:
729  * @code{.cc}
730  * MathematicalProgram prog;
731  * auto x = prog.NewIndeterminates(2, 3, {"x1", "x2", "x3", "x4",
732  * "x5", "x6"});
733  * @endcode
734  * This adds a 2 x 3 matrix indeterminates into the program.
735  *
736  * The name of the variable is only used for the user in order to ease
737  * readability.
738  */
739  MatrixXIndeterminate NewIndeterminates(int rows, int cols,
740  const std::vector<std::string>& names);
741 
742  /**
743  * Adds indeterminates to this MathematicalProgram, with default name
744  * "X". The new variables are returned and viewed as a matrix, with size
745  * @p rows x @p cols.
746  * @see NewIndeterminates(int rows, int cols, const
747  * std::vector<std::string>& names);
748  */
749  MatrixXIndeterminate NewIndeterminates(int rows, int cols,
750  const std::string& name = "X");
751 
752  /** Adds indeterminates.
753  * This method appends some indeterminates to the end of the program's old
754  * indeterminates.
755  * @param new_indeterminates The indeterminates to be appended to the
756  * program's old indeterminates.
757  * @pre `new_indeterminates` should not intersect with the program's old
758  * indeterminates or decision variables.
759  * @pre Each entry in new_indeterminates should not be dummy.
760  * @pre Each entry in new_indeterminates should be of CONTINUOUS type.
761  */
762  void AddIndeterminates(
763  const Eigen::Ref<const VectorXIndeterminate>& new_indeterminates);
764 
765  /**
766  * Adds a callback method to visualize intermediate results of the
767  * optimization.
768  *
769  * Note: Just like other costs/constraints, not all solvers support callbacks.
770  * Adding a callback here will force MathematicalProgram::Solve to select a
771  * solver that support callbacks. For instance, adding a visualization
772  * callback to a quadratic programming problem may result in using a nonlinear
773  * programming solver as the default solver.
774  *
775  * @param callback a std::function that accepts an Eigen::Vector of doubles
776  * representing the bound decision variables.
777  * @param vars the decision variables that should be passed to the callback.
778  */
779  Binding<VisualizationCallback> AddVisualizationCallback(
781  const Eigen::Ref<const VectorXDecisionVariable>& vars);
782 
783  /**
784  * Adds a callback method to visualize intermediate results of the
785  * optimization.
786  *
787  * Note: Just like other costs/constraints, not all solvers support callbacks.
788  * Adding a callback here will force MathematicalProgram::Solve to select a
789  * solver that support callbacks. For instance, adding a visualization
790  * callback to a quadratic programming problem may result in using a nonlinear
791  * programming solver as the default solver.
792  *
793  * @param callback a std::function that accepts an Eigen::Vector of doubles
794  * representing the for the bound decision variables.
795  * @param vars the decision variables that should be passed to the callback.
796  */
799  const VariableRefList& vars) {
800  return AddVisualizationCallback(callback,
802  }
803 
804  /**
805  * Adds a generic cost to the optimization program.
806  */
807  Binding<Cost> AddCost(const Binding<Cost>& binding);
808 
809  /**
810  * Adds a cost type to the optimization program.
811  * @param obj The added objective.
812  * @param vars The decision variables on which the cost depend.
813  */
814  template <typename C>
815  auto AddCost(const std::shared_ptr<C>& obj,
816  const Eigen::Ref<const VectorXDecisionVariable>& vars) {
817  // Redirect to the appropriate type
818  // Use auto to enable the overloading method to upcast if needed
819  return AddCost(internal::CreateBinding(obj, vars));
820  }
821 
822  /**
823  * Adds a generic cost to the optimization program.
824  * @param obj The added objective.
825  * @param vars The decision variables on which the cost depend.
826  */
827  template <typename C>
828  auto AddCost(const std::shared_ptr<C>& obj, const VariableRefList& vars) {
829  return AddCost(obj, ConcatenateVariableRefList(vars));
830  }
831 
832  /**
833  * Convert an input of type @p F to a FunctionCost object.
834  * @tparam F This class should have functions numInputs(), numOutputs and
835  * eval(x, y).
836  * @see drake::solvers::detail::FunctionTraits.
837  */
838  template <typename F>
839  static std::shared_ptr<Cost> MakeCost(F&& f) {
840  return MakeFunctionCost(f);
841  }
842 
843  /**
844  * Adds a cost to the optimization program on a list of variables.
845  * @tparam F it should define functions numInputs, numOutputs and eval. Check
846  * drake::solvers::detail::FunctionTraits for more detail.
847  */
848  template <typename F>
850  Binding<Cost>>::type
851  AddCost(F&& f, const VariableRefList& vars) {
852  return AddCost(f, ConcatenateVariableRefList(vars));
853  }
854 
855  /**
856  * Adds a cost to the optimization program on an Eigen::Vector containing
857  * decision variables.
858  * @tparam F Type that defines functions numInputs, numOutputs and eval.
859  * @see drake::solvers::detail::FunctionTraits.
860  */
861  template <typename F>
863  Binding<Cost>>::type
864  AddCost(F&& f, const Eigen::Ref<const VectorXDecisionVariable>& vars) {
865  auto c = MakeFunctionCost(std::forward<F>(f));
866  return AddCost(c, vars);
867  }
868 
869  /**
870  * Statically assert if a user inadvertently passes a
871  * binding-compatible Constraint.
872  * @tparam F The type to check.
873  */
874  template <typename F, typename Vars>
876  Binding<Cost>>::type
877  AddCost(F&&, Vars&&) {
878  throw std::runtime_error("This will assert at compile-time.");
879  }
880 
881  /**
882  * Adds a cost term of the form c'*x.
883  * Applied to a subset of the variables and pushes onto
884  * the linear cost data structure.
885  */
886  Binding<LinearCost> AddCost(const Binding<LinearCost>& binding);
887 
888  /**
889  * Adds a linear cost term of the form a'*x + b.
890  * @param e A linear symbolic expression.
891  * @pre e is a linear expression a'*x + b, where each entry of x is a decision
892  * variable in the mathematical program.
893  * @return The newly added linear constraint, together with the bound
894  * variables.
895  */
896  Binding<LinearCost> AddLinearCost(const symbolic::Expression& e);
897 
898  /**
899  * Adds a linear cost term of the form a'*x + b.
900  * Applied to a subset of the variables and pushes onto
901  * the linear cost data structure.
902  */
903  Binding<LinearCost> AddLinearCost(const Eigen::Ref<const Eigen::VectorXd>& a,
904  double b, const VariableRefList& vars) {
905  return AddLinearCost(a, b, ConcatenateVariableRefList((vars)));
906  }
907 
908  /**
909  * Adds a linear cost term of the form a'*x + b.
910  * Applied to a subset of the variables and pushes onto
911  * the linear cost data structure.
912  */
913  Binding<LinearCost> AddLinearCost(
914  const Eigen::Ref<const Eigen::VectorXd>& a, double b,
915  const Eigen::Ref<const VectorXDecisionVariable>& vars);
916 
917  /**
918  * Adds a linear cost term of the form a'*x.
919  * Applied to a subset of the variables and pushes onto
920  * the linear cost data structure.
921  */
922  template <typename VarType>
923  Binding<LinearCost> AddLinearCost(const Eigen::Ref<const Eigen::VectorXd>& a,
924  const VarType& vars) {
925  const double b = 0.;
926  return AddLinearCost(a, b, vars);
927  }
928 
929  /**
930  * Adds a cost term of the form 0.5*x'*Q*x + b'x.
931  * Applied to subset of the variables and pushes onto
932  * the quadratic cost data structure.
933  */
934  Binding<QuadraticCost> AddCost(const Binding<QuadraticCost>& binding);
935 
936  /**
937  * Add a quadratic cost term of the form 0.5*x'*Q*x + b'*x + c.
938  * Notice that in the optimization program, the constant term `c` in the cost
939  * is ignored.
940  * @param e A quadratic symbolic expression. Throws a runtime error if the
941  * expression is not quadratic.
942  * @return The newly added cost together with the bound variables.
943  */
944  Binding<QuadraticCost> AddQuadraticCost(const symbolic::Expression& e);
945 
946  /**
947  * Adds a cost term of the form (x-x_desired)'*Q*(x-x_desired).
948  */
950  const Eigen::Ref<const Eigen::MatrixXd>& Q,
951  const Eigen::Ref<const Eigen::VectorXd>& x_desired,
952  const VariableRefList& vars) {
953  return AddQuadraticErrorCost(Q, x_desired,
955  }
956 
957  /**
958  * Adds a cost term of the form (x-x_desired)'*Q*(x-x_desired).
959  */
960  Binding<QuadraticCost> AddQuadraticErrorCost(
961  const Eigen::Ref<const Eigen::MatrixXd>& Q,
962  const Eigen::Ref<const Eigen::VectorXd>& x_desired,
963  const Eigen::Ref<const VectorXDecisionVariable>& vars);
964 
965  /**
966  * Adds a cost term of the form | Ax - b |^2.
967  */
969  const Eigen::Ref<const Eigen::MatrixXd>& A,
970  const Eigen::Ref<const Eigen::VectorXd>& b, const VariableRefList& vars) {
971  return AddL2NormCost(A, b, ConcatenateVariableRefList(vars));
972  }
973 
974  /**
975  * Adds a cost term of the form | Ax - b |^2.
976  */
978  const Eigen::Ref<const Eigen::MatrixXd>& A,
979  const Eigen::Ref<const Eigen::VectorXd>& b,
980  const Eigen::Ref<const VectorXDecisionVariable>& vars) {
981  return AddCost(MakeL2NormCost(A, b), vars);
982  }
983 
984  /**
985  * Adds a cost term of the form 0.5*x'*Q*x + b'x.
986  * Applied to subset of the variables.
987  */
989  const Eigen::Ref<const Eigen::MatrixXd>& Q,
990  const Eigen::Ref<const Eigen::VectorXd>& b, const VariableRefList& vars) {
991  return AddQuadraticCost(Q, b, ConcatenateVariableRefList(vars));
992  }
993 
994  /**
995  * Adds a cost term of the form 0.5*x'*Q*x + b'x + c
996  * Applied to subset of the variables.
997  */
998  Binding<QuadraticCost> AddQuadraticCost(
999  const Eigen::Ref<const Eigen::MatrixXd>& Q,
1000  const Eigen::Ref<const Eigen::VectorXd>& b, double c,
1001  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1002 
1003  /**
1004  * Adds a cost term of the form 0.5*x'*Q*x + b'x
1005  * Applied to subset of the variables.
1006  */
1007  Binding<QuadraticCost> AddQuadraticCost(
1008  const Eigen::Ref<const Eigen::MatrixXd>& Q,
1009  const Eigen::Ref<const Eigen::VectorXd>& b,
1010  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1011 
1012  /**
1013  * Adds a cost term in the polynomial form.
1014  * @param e A symbolic expression in the polynomial form.
1015  * @return The newly created cost and the bound variables.
1016  */
1017  Binding<PolynomialCost> AddPolynomialCost(const symbolic::Expression& e);
1018 
1019  /**
1020  * Adds a cost in the symbolic form.
1021  * Note that the constant part of the cost is ignored. So if you set
1022  * `e = x + 2`, then only the cost on `x` is added, the constant term 2 is
1023  * ignored.
1024  * @param e The linear or quadratic expression of the cost.
1025  * @pre `e` is linear or `e` is quadratic. Otherwise throws a runtime error.
1026  * @return The newly created cost, together with the bound variables.
1027  */
1028  Binding<Cost> AddCost(const symbolic::Expression& e);
1029 
1030  /**
1031  * Adds a generic constraint to the program. This should
1032  * only be used if a more specific type of constraint is not
1033  * available, as it may require the use of a significantly more
1034  * expensive solver.
1035  */
1036  Binding<Constraint> AddConstraint(const Binding<Constraint>& binding);
1037 
1038  /**
1039  * Adds one row of constraint lb <= e <= ub where @p e is a symbolic
1040  * expression. Throws an exception if
1041  * 1. <tt>lb <= e <= ub</tt> is a trivial constraint such as 1 <= 2 <= 3.
1042  * 2. <tt>lb <= e <= ub</tt> is unsatisfiable such as 1 <= -5 <= 3
1043  *
1044  * @param e A symbolic expression of the the decision variables.
1045  * @param lb A scalar, the lower bound.
1046  * @param ub A scalar, the upper bound.
1047  *
1048  * The resulting constraint may be a BoundingBoxConstraint, LinearConstraint,
1049  * LinearEqualityConstraint, or ExpressionConstraint, depending on the
1050  * arguments. Constraints of the form x == 1 (which could be created as a
1051  * BoundingBoxConstraint or LinearEqualityConstraint) will be
1052  * constructed as a LinearEqualityConstraint.
1053  */
1054  Binding<Constraint> AddConstraint(const symbolic::Expression& e, double lb,
1055  double ub);
1056 
1057  /**
1058  * Adds constraints represented by symbolic expressions to the program. It
1059  * throws if <tt>lb <= v <= ub</tt> includes trivial/unsatisfiable
1060  * constraints.
1061  *
1062  * @overload Binding<Constraint> AddConstraint(const symbolic::Expression& e,
1063  * double lb, double ub)
1064  */
1065  Binding<Constraint> AddConstraint(
1066  const Eigen::Ref<const VectorX<symbolic::Expression>>& v,
1067  const Eigen::Ref<const Eigen::VectorXd>& lb,
1068  const Eigen::Ref<const Eigen::VectorXd>& ub);
1069 
1070  /**
1071  * Add a constraint represented by a symbolic formula to the program. The
1072  * input formula @p f can be of the following forms:
1073  *
1074  * 1. e1 <= e2
1075  * 2. e1 >= e2
1076  * 3. e1 == e2
1077  * 4. A conjunction of relational formulas where each conjunct is
1078  * a relational formula matched by 1, 2, or 3.
1079  *
1080  * Note that first two cases might return an object of
1081  * Binding<BoundingBoxConstraint>, Binding<LinearConstraint>, or
1082  * Binding<ExpressionConstraint>, depending
1083  * on @p f. Also the third case might return an object of
1084  * Binding<LinearEqualityConstraint> or Binding<ExpressionConstraint>.
1085  *
1086  * It throws an exception if
1087  * 1. @p f is not matched with one of the above patterns. Especially, strict
1088  * inequalities (<, >) are not allowed.
1089  * 2. @p f is either a trivial constraint such as "1 <= 2" or an
1090  * unsatisfiable constraint such as "2 <= 1".
1091  * 3. It is not possible to find numerical bounds of `e1` and `e2` where @p f
1092  * = e1 ≃ e2. We allow `e1` and `e2` to be infinite but only if there are
1093  * no other terms. For example, `x <= ∞` is allowed. However, `x - ∞ <= 0`
1094  * is not allowed because `x ↦ ∞` introduces `nan` in the evaluation.
1095  */
1096  Binding<Constraint> AddConstraint(const symbolic::Formula& f);
1097 
1098  /**
1099  * Add a constraint represented by an Eigen::Array<symbolic::Formula>
1100  * to the program. A common use-case of this function is to add a constraint
1101  * with the element-wise comparison between two Eigen matrices,
1102  * using `A.array() <= B.array()`. See the following example.
1103  *
1104  * @code
1105  * MathematicalProgram prog;
1106  * Eigen::Matrix<double, 2, 2> A;
1107  * auto x = prog.NewContinuousVariables(2, "x");
1108  * Eigen::Vector2d b;
1109  * ... // set up A and b
1110  * prog.AddConstraint((A * x).array() <= b.array());
1111  * @endcode
1112  *
1113  * A formula in @p formulas can be of the following forms:
1114  *
1115  * 1. e1 <= e2
1116  * 2. e1 >= e2
1117  * 3. e1 == e2
1118  *
1119  * It throws an exception if AddConstraint(const symbolic::Formula& f)
1120  * throws an exception for f ∈ @p formulas.
1121  *
1122  * @overload Binding<Constraint> AddConstraint(const symbolic::Formula& f)
1123  *
1124  * @tparam Derived An Eigen Array type of Formula.
1125  */
1126  template <typename Derived>
1127  typename std::enable_if<
1129  Binding<Constraint>>::type
1130  AddConstraint(const Eigen::ArrayBase<Derived>& formulas) {
1131  return AddConstraint(internal::ParseConstraint(formulas));
1132  }
1133 
1134  /**
1135  * Adds a generic constraint to the program. This should
1136  * only be used if a more specific type of constraint is not
1137  * available, as it may require the use of a significantly more
1138  * expensive solver.
1139  */
1140  template <typename C>
1141  auto AddConstraint(std::shared_ptr<C> con, const VariableRefList& vars) {
1142  return AddConstraint(con, ConcatenateVariableRefList(vars));
1143  }
1144 
1145  /**
1146  * Adds a generic constraint to the program. This should
1147  * only be used if a more specific type of constraint is not
1148  * available, as it may require the use of a significantly more
1149  * expensive solver.
1150  */
1151  template <typename C>
1152  auto AddConstraint(std::shared_ptr<C> con,
1153  const Eigen::Ref<const VectorXDecisionVariable>& vars) {
1154  return AddConstraint(internal::CreateBinding(con, vars));
1155  }
1156 
1157  /**
1158  * Adds linear constraints referencing potentially a subset
1159  * of the decision variables (defined in the vars parameter).
1160  */
1161  Binding<LinearConstraint> AddConstraint(
1162  const Binding<LinearConstraint>& binding);
1163 
1164  /**
1165  * Adds linear constraints referencing potentially a subset
1166  * of the decision variables (defined in the vars parameter).
1167  */
1169  const Eigen::Ref<const Eigen::MatrixXd>& A,
1170  const Eigen::Ref<const Eigen::VectorXd>& lb,
1171  const Eigen::Ref<const Eigen::VectorXd>& ub,
1172  const VariableRefList& vars) {
1173  return AddLinearConstraint(A, lb, ub, ConcatenateVariableRefList(vars));
1174  }
1175 
1176  /**
1177  * Adds linear constraints referencing potentially a subset
1178  * of the decision variables (defined in the vars parameter).
1179  */
1180  Binding<LinearConstraint> AddLinearConstraint(
1181  const Eigen::Ref<const Eigen::MatrixXd>& A,
1182  const Eigen::Ref<const Eigen::VectorXd>& lb,
1183  const Eigen::Ref<const Eigen::VectorXd>& ub,
1184  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1185 
1186  /**
1187  * Adds one row of linear constraint referencing potentially a
1188  * subset of the decision variables (defined in the vars parameter).
1189  * lb <= a*vars <= ub
1190  * @param a A row vector.
1191  * @param lb A scalar, the lower bound.
1192  * @param ub A scalar, the upper bound.
1193  * @param vars The decision variables on which to impose the linear
1194  * constraint.
1195  */
1197  const Eigen::Ref<const Eigen::RowVectorXd>& a, double lb, double ub,
1198  const VariableRefList& vars) {
1199  return AddLinearConstraint(a, lb, ub, ConcatenateVariableRefList(vars));
1200  }
1201 
1202  /**
1203  * Adds one row of linear constraint referencing potentially a
1204  * subset of the decision variables (defined in the vars parameter).
1205  * lb <= a*vars <= ub
1206  * @param a A row vector.
1207  * @param lb A scalar, the lower bound.
1208  * @param ub A scalar, the upper bound.
1209  * @param vars The decision variables on which to impose the linear
1210  * constraint.
1211  */
1213  const Eigen::Ref<const Eigen::RowVectorXd>& a, double lb, double ub,
1214  const Eigen::Ref<const VectorXDecisionVariable>& vars) {
1215  return AddLinearConstraint(a, Vector1d(lb), Vector1d(ub), vars);
1216  }
1217 
1218  /**
1219  * Adds one row of linear constraint lb <= e <= ub where @p e is a symbolic
1220  * expression. Throws an exception if
1221  * 1. @p e is a non-linear expression.
1222  * 2. <tt>lb <= e <= ub</tt> is a trivial constraint such as 1 <= 2 <= 3.
1223  * 3. <tt>lb <= e <= ub</tt> is unsatisfiable such as 1 <= -5 <= 3
1224  *
1225  * @param e A linear symbolic expression in the form of <tt>c0 + c1 * v1 +
1226  * ... + cn * vn</tt> where @c c_i is a constant and @v_i is a variable.
1227  * @param lb A scalar, the lower bound.
1228  * @param ub A scalar, the upper bound.
1229  */
1230  Binding<LinearConstraint> AddLinearConstraint(const symbolic::Expression& e,
1231  double lb, double ub);
1232 
1233  /**
1234  * Adds linear constraints represented by symbolic expressions to the
1235  * program. It throws if @v includes a non-linear expression or <tt>lb <= v <=
1236  * ub</tt> includes trivial/unsatisfiable constraints.
1237  */
1238  Binding<LinearConstraint> AddLinearConstraint(
1239  const Eigen::Ref<const VectorX<symbolic::Expression>>& v,
1240  const Eigen::Ref<const Eigen::VectorXd>& lb,
1241  const Eigen::Ref<const Eigen::VectorXd>& ub);
1242 
1243  /**
1244  * Add a linear constraint represented by a symbolic formula to the
1245  * program. The input formula @p f can be of the following forms:
1246  *
1247  * 1. e1 <= e2
1248  * 2. e1 >= e2
1249  * 3. e1 == e2
1250  * 4. A conjunction of relational formulas where each conjunct is
1251  * a relational formula matched by 1, 2, or 3.
1252  *
1253  * Note that first two cases might return an object of
1254  * Binding<BoundingBoxConstraint> depending on @p f. Also the third case
1255  * returns an object of Binding<LinearEqualityConstraint>.
1256  *
1257  * It throws an exception if
1258  * 1. @p f is not matched with one of the above patterns. Especially, strict
1259  * inequalities (<, >) are not allowed.
1260  * 2. @p f includes a non-linear expression.
1261  * 3. @p f is either a trivial constraint such as "1 <= 2" or an
1262  * unsatisfiable constraint such as "2 <= 1".
1263  * 4. It is not possible to find numerical bounds of `e1` and `e2` where @p f
1264  * = e1 ≃ e2. We allow `e1` and `e2` to be infinite but only if there are
1265  * no other terms. For example, `x <= ∞` is allowed. However, `x - ∞ <= 0`
1266  * is not allowed because `x ↦ ∞` introduces `nan` in the evaluation.
1267  */
1268  Binding<LinearConstraint> AddLinearConstraint(const symbolic::Formula& f);
1269 
1270  /**
1271  * Add a linear constraint represented by an Eigen::Array<symbolic::Formula>
1272  * to the program. A common use-case of this function is to add a linear
1273  * constraint with the element-wise comparison between two Eigen matrices,
1274  * using `A.array() <= B.array()`. See the following example.
1275  *
1276  * @code
1277  * MathematicalProgram prog;
1278  * Eigen::Matrix<double, 2, 2> A;
1279  * auto x = prog.NewContinuousVariables(2, "x");
1280  * Eigen::Vector2d b;
1281  * ... // set up A and b
1282  * prog.AddLinearConstraint((A * x).array() <= b.array());
1283  * @endcode
1284  *
1285  * A formula in @p formulas can be of the following forms:
1286  *
1287  * 1. e1 <= e2
1288  * 2. e1 >= e2
1289  * 3. e1 == e2
1290  *
1291  * It throws an exception if AddLinearConstraint(const symbolic::Formula& f)
1292  * throws an exception for f ∈ @p formulas.
1293  * @tparam Derived An Eigen Array type of Formula.
1294  */
1295  template <typename Derived>
1296  typename std::enable_if<
1297  is_eigen_scalar_same<Derived, symbolic::Formula>::value,
1299  AddLinearConstraint(const Eigen::ArrayBase<Derived>& formulas) {
1300  Binding<Constraint> binding = internal::ParseConstraint(formulas);
1301  Constraint* constraint = binding.evaluator().get();
1302  if (dynamic_cast<LinearConstraint*>(constraint)) {
1303  return AddConstraint(
1304  internal::BindingDynamicCast<LinearConstraint>(binding));
1305  } else {
1306  std::stringstream oss;
1307  oss << "Formulas are non-linear.";
1308  throw std::runtime_error("AddLinearConstraint called but formulas are "
1309  "non-linear");
1310  }
1311  }
1312 
1313  /**
1314  * Adds linear equality constraints referencing potentially a
1315  * subset of the decision variables.
1316  */
1317  Binding<LinearEqualityConstraint> AddConstraint(
1318  const Binding<LinearEqualityConstraint>& binding);
1319 
1320  /**
1321  * Adds one row of linear constraint e = b where @p e is a symbolic
1322  * expression. Throws an exception if
1323  * 1. @p e is a non-linear expression.
1324  * 2. @p e is a constant.
1325  *
1326  * @param e A linear symbolic expression in the form of <tt>c0 + c1 * x1 +
1327  * ... + cn * xn</tt> where @c c_i is a constant and @x_i is a variable.
1328  * @param b A scalar.
1329  * @return The newly added linear equality constraint, together with the
1330  * bound variable.
1331  */
1332  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
1333  const symbolic::Expression& e, double b);
1334 
1335  /**
1336  * Adds a linear equality constraint represented by a symbolic formula to the
1337  * program. The input formula @p f is either an equality formula (`e1 == e2`)
1338  * or a conjunction of equality formulas.
1339  *
1340  * It throws an exception if
1341  * 1. @p f is neither an equality formula nor a conjunction of equalities.
1342  * 2. @p f includes a non-linear expression.
1343  */
1344  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
1345  const symbolic::Formula& f);
1346 
1347  /**
1348  * Adds linear equality constraints \f$ v = b \f$, where \p v(i) is a symbolic
1349  * linear expression. Throws an exception if
1350  * 1. @p v(i) is a non-linear expression.
1351  * 2. @p v(i) is a constant.
1352  * @tparam DerivedV An Eigen Matrix type of Expression. A column vector.
1353  * @tparam DerivedB An Eigen Matrix type of double. A column vector.
1354  * @param v v(i) is a linear symbolic expression in the form of
1355  * <tt> c0 + c1 * x1 + ... + cn * xn </tt> where ci is a constant and @xi is
1356  * a variable.
1357  * @param b A vector of doubles.
1358  * @return The newly added linear equality constraint, together with the
1359  * bound variables.
1360  */
1361  template <typename DerivedV, typename DerivedB>
1362  typename std::enable_if<
1365  AddLinearEqualityConstraint(const Eigen::MatrixBase<DerivedV>& v,
1366  const Eigen::MatrixBase<DerivedB>& b) {
1367  return AddConstraint(internal::ParseLinearEqualityConstraint(v, b));
1368  }
1369 
1370  /**
1371  * Adds a linear equality constraint for a matrix of linear expression @p V,
1372  * such that V(i, j) = B(i, j). If V is a symmetric matrix, then the user
1373  * may only want to constrain the lower triangular part of V.
1374  * This function is meant to provide convenience to the user, it incurs
1375  * additional copy and memory allocation. For faster speed, add each column
1376  * of the matrix equality in a for loop.
1377  * @tparam DerivedV An Eigen Matrix type of Expression. The number of columns
1378  * at compile time should not be 1.
1379  * @tparam DerivedB An Eigen Matrix type of double.
1380  * @param V An Eigen Matrix of symbolic expressions. V(i, j) should be a
1381  * linear expression.
1382  * @param B An Eigen Matrix of doubles.
1383  * @param lower_triangle If true, then only the lower triangular part of @p V
1384  * is constrained, otherwise the whole matrix V is constrained. @default is
1385  * false.
1386  * @return The newly added linear equality constraint, together with the
1387  * bound variables.
1388  */
1389  template <typename DerivedV, typename DerivedB>
1390  typename std::enable_if<
1393  AddLinearEqualityConstraint(const Eigen::MatrixBase<DerivedV>& V,
1394  const Eigen::MatrixBase<DerivedB>& B,
1395  bool lower_triangle = false) {
1396  return AddConstraint(
1397  internal::ParseLinearEqualityConstraint(V, B, lower_triangle));
1398  }
1399 
1400  /** AddLinearEqualityConstraint
1401  *
1402  * Adds linear equality constraints referencing potentially a subset of
1403  * the decision variables.
1404  *
1405  * Example: to add two equality constraints which only depend on two of the
1406  * elements of x, you could use
1407  * @code{.cc}
1408  * auto x = prog.NewContinuousDecisionVariable(6,"myvar");
1409  * Eigen::Matrix2d Aeq;
1410  * Aeq << -1, 2,
1411  * 1, 1;
1412  * Eigen::Vector2d beq(1, 3);
1413  * prog.AddLinearEqualityConstraint(Aeq, beq, {x.segment<1>(2),
1414  * x.segment<1>(5)});
1415  * @endcode
1416  * The code above imposes constraints
1417  * @f[-x(2) + 2x(5) = 1 @f]
1418  * @f[ x(2) + x(5) = 3 @f]
1419  */
1421  const Eigen::Ref<const Eigen::MatrixXd>& Aeq,
1422  const Eigen::Ref<const Eigen::VectorXd>& beq,
1423  const VariableRefList& vars) {
1424  return AddLinearEqualityConstraint(Aeq, beq,
1426  }
1427 
1428  /** AddLinearEqualityConstraint
1429  *
1430  * Adds linear equality constraints referencing potentially a subset of
1431  * the decision variables.
1432  *
1433  * Example: to add two equality constraints which only depend on two of the
1434  * elements of x, you could use
1435  * @code{.cc}
1436  * auto x = prog.NewContinuousDecisionVariable(6,"myvar");
1437  * Eigen::Matrix2d Aeq;
1438  * Aeq << -1, 2,
1439  * 1, 1;
1440  * Eigen::Vector2d beq(1, 3);
1441  * // Imposes constraint
1442  * // -x(0) + 2x(1) = 1
1443  * // x(0) + x(1) = 3
1444  * prog.AddLinearEqualityConstraint(Aeq, beq, x.head<2>());
1445  * @endcode
1446  */
1447  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
1448  const Eigen::Ref<const Eigen::MatrixXd>& Aeq,
1449  const Eigen::Ref<const Eigen::VectorXd>& beq,
1450  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1451 
1452  /**
1453  * Adds one row of linear equality constraint referencing potentially a subset
1454  * of decision variables.
1455  * @f[
1456  * ax = beq
1457  * @f]
1458  * @param a A row vector.
1459  * @param beq A scalar.
1460  * @param vars The decision variables on which the constraint is imposed.
1461  */
1463  const Eigen::Ref<const Eigen::RowVectorXd>& a, double beq,
1464  const VariableRefList& vars) {
1465  return AddConstraint(std::make_shared<LinearEqualityConstraint>(a, beq),
1467  }
1468 
1469  /**
1470  * Adds one row of linear equality constraint referencing potentially a subset
1471  * of decision variables.
1472  * @f[
1473  * ax = beq
1474  * @f]
1475  * @param a A row vector.
1476  * @param beq A scalar.
1477  * @param vars The decision variables on which the constraint is imposed.
1478  */
1480  const Eigen::Ref<const Eigen::RowVectorXd>& a, double beq,
1481  const Eigen::Ref<const VectorXDecisionVariable>& vars) {
1482  return AddLinearEqualityConstraint(a, Vector1d(beq), vars);
1483  }
1484 
1485  /**
1486  * Adds bounding box constraints referencing potentially a subest of the
1487  * decision variables.
1488  * @param binding Binds a BoundingBoxConstraint with some decision variables,
1489  * such that
1490  * binding.evaluator()->lower_bound()(i) <= binding.variables()(i)
1491  * <= binding.evaluator().upper_bound()(i)
1492  */
1493  Binding<BoundingBoxConstraint> AddConstraint(
1494  const Binding<BoundingBoxConstraint>& binding);
1495 
1496  /** AddBoundingBoxConstraint
1497  *
1498  * Adds bounding box constraints referencing potentially a
1499  * subset of the decision variables (defined in the vars parameter).
1500  * Example
1501  * \code{.cc}
1502  * MathematicalProgram prog;
1503  * auto x = prog.NewContinuousDecisionVariables<2>("x");
1504  * auto y = prog.NewContinuousDecisionVariables<1>("y");
1505  * Eigen::Vector3d lb(0, 1, 2);
1506  * Eigen::Vector3d ub(1, 2, 3);
1507  * // Imposes the constraint
1508  * // 0 ≤ x(0) ≤ 1
1509  * // 1 ≤ x(1) ≤ 2
1510  * // 2 ≤ y ≤ 3
1511  * prog.AddBoundingBoxConstraint(lb, ub, {x, y});
1512  * \endcode
1513  */
1515  const Eigen::Ref<const Eigen::VectorXd>& lb,
1516  const Eigen::Ref<const Eigen::VectorXd>& ub,
1517  const VariableRefList& vars) {
1518  return AddBoundingBoxConstraint(lb, ub, ConcatenateVariableRefList(vars));
1519  }
1520 
1521  /**
1522  * Adds bounding box constraints referencing potentially a subset of the
1523  * decision variables.
1524  * @param lb The lower bound.
1525  * @param ub The upper bound.
1526  * @param vars Will imposes constraint lb(i) <= vars(i) <= ub(i).
1527  * @return The newly constructed BoundingBoxConstraint.
1528  */
1529  Binding<BoundingBoxConstraint> AddBoundingBoxConstraint(
1530  const Eigen::Ref<const Eigen::VectorXd>& lb,
1531  const Eigen::Ref<const Eigen::VectorXd>& ub,
1532  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1533 
1534  /**
1535  * Adds bounds for a single variable.
1536  * @param lb Lower bound.
1537  * @param ub Upper bound.
1538  * @param var The decision variable.
1539  */
1541  double lb, double ub, const symbolic::Variable& var) {
1542  MatrixDecisionVariable<1, 1> var_matrix(var);
1543  return AddBoundingBoxConstraint(Vector1d(lb), Vector1d(ub), var_matrix);
1544  }
1545 
1546  /**
1547  * Adds the same scalar lower and upper bound to every variable in the list.
1548  * @param lb Lower bound.
1549  * @param ub Upper bound.
1550  * @param vars The decision variables.
1551  */
1553  double lb, double ub, const VariableRefList& vars) {
1554  return AddBoundingBoxConstraint(lb, ub, ConcatenateVariableRefList(vars));
1555  }
1556 
1557  /**
1558  * Adds the same scalar lower and upper bound to every variable in @p vars.
1559  * @tparam Derived An Eigen Vector type with Variable as the scalar
1560  * type.
1561  * @param lb Lower bound.
1562  * @param ub Upper bound.
1563  * @param vars The decision variables.
1564  */
1565  template <typename Derived>
1566  typename std::enable_if<
1568  Derived::ColsAtCompileTime == 1,
1570  AddBoundingBoxConstraint(double lb, double ub,
1571  const Eigen::MatrixBase<Derived>& vars) {
1572  const int kSize = Derived::RowsAtCompileTime;
1573  return AddBoundingBoxConstraint(
1574  Eigen::Matrix<double, kSize, 1>::Constant(vars.size(), lb),
1575  Eigen::Matrix<double, kSize, 1>::Constant(vars.size(), ub), vars);
1576  }
1577 
1578  /**
1579  * Adds the same scalar lower and upper bound to every variable in @p vars.
1580  * @tparam Derived An Eigen::Matrix with Variable as the scalar
1581  * type. The matrix has unknown number of columns at compile time, or has
1582  * more than one column.
1583  * @param lb Lower bound.
1584  * @param ub Upper bound.
1585  * @param vars The decision variables.
1586  */
1587  template <typename Derived>
1588  typename std::enable_if<
1590  Derived::ColsAtCompileTime != 1,
1592  AddBoundingBoxConstraint(double lb, double ub,
1593  const Eigen::MatrixBase<Derived>& vars) {
1594  const int kSize =
1595  Derived::RowsAtCompileTime != Eigen::Dynamic &&
1596  Derived::ColsAtCompileTime != Eigen::Dynamic
1597  ? Derived::RowsAtCompileTime * Derived::ColsAtCompileTime
1598  : Eigen::Dynamic;
1599  Eigen::Matrix<symbolic::Variable, kSize, 1> flat_vars(vars.size());
1600  for (int j = 0; j < vars.cols(); ++j) {
1601  for (int i = 0; i < vars.rows(); ++i) {
1602  flat_vars(j * vars.rows() + i) = vars(i, j);
1603  }
1604  }
1605  return AddBoundingBoxConstraint(
1606  Eigen::Matrix<double, kSize, 1>::Constant(vars.size(), lb),
1607  Eigen::Matrix<double, kSize, 1>::Constant(vars.size(), ub), flat_vars);
1608  }
1609 
1610  /**
1611  * Adds Lorentz cone constraint referencing potentially a subset
1612  * of the decision variables.
1613  * The linear expression @f$ z=Ax+b @f$ is in the Lorentz cone.
1614  * A vector \f$ z \in\mathbb{R}^n \f$ is in the Lorentz cone, if
1615  * <!--
1616  * z(0) >= sqrt{z(1)² + ... + z(n-1)²}
1617  * -->
1618  * @f[
1619  * z_0 \ge \sqrt{z_1^2 + ... + z_{n-1}^2}
1620  * @f]
1621  */
1622  Binding<LorentzConeConstraint> AddConstraint(
1623  const Binding<LorentzConeConstraint>& binding);
1624 
1625  /**
1626  * Adds Lorentz cone constraint referencing potentially a subset of the
1627  * decision variables.
1628  * @param v An Eigen::Vector of symbolic::Expression. Constraining that
1629  * \f[
1630  * v_0 \ge \sqrt{v_1^2 + ... + v_{n-1}^2}
1631  * \f]
1632  * @return The newly constructed Lorentz cone constraint with the bounded
1633  * variables.
1634  */
1635  Binding<LorentzConeConstraint> AddLorentzConeConstraint(
1636  const Eigen::Ref<const VectorX<symbolic::Expression>>& v);
1637 
1638  /**
1639  * Adds Lorentz cone constraint on the linear expression v1 and quadratic
1640  * expression v2, such that v1 >= sqrt(v2)
1641  * @param linear_expression The linear expression v1.
1642  * @param quadratic_expression The quadratic expression v2.
1643  * @param tol The tolerance to determine if the matrix in v2 is positive
1644  * semidefinite or not. @see DecomposePositiveQuadraticForm for more
1645  * explanation. @default is 0.
1646  * @retval binding The newly added Lorentz cone constraint, together with the
1647  * bound variables.
1648  * @pre
1649  * 1. `v1` is a linear expression, in the form of c'*x + d.
1650  * 2. `v2` is a quadratic expression, in the form of
1651  * <pre>
1652  * x'*Q*x + b'x + a
1653  * </pre>
1654  * Also the quadratic expression has to be convex, namely Q is a
1655  * positive semidefinite matrix, and the quadratic expression needs
1656  * to be non-negative for any x.
1657  * Throws a runtime_error if the preconditions are not satisfied.
1658  *
1659  * Notice this constraint is equivalent to the vector [z;y] is within a
1660  * Lorentz cone, where
1661  * <pre>
1662  * z = v1
1663  * y = R * x + d
1664  * </pre>
1665  * while (R, d) satisfies y'*y = x'*Q*x + b'*x + a
1666  */
1667  Binding<LorentzConeConstraint> AddLorentzConeConstraint(
1668  const symbolic::Expression& linear_expression,
1669  const symbolic::Expression& quadratic_expression, double tol = 0);
1670 
1671  /**
1672  * Adds Lorentz cone constraint referencing potentially a subset of the
1673  * decision variables (defined in the vars parameter).
1674  * The linear expression @f$ z=Ax+b @f$ is in the Lorentz cone.
1675  * A vector \f$ z \in\mathbb{R}^n \f$ is in the Lorentz cone, if
1676  * <!--
1677  * z(0) >= sqrt{z(1)² + ... + z(n-1)²}
1678  * -->
1679  * @f[
1680  * z_0 \ge \sqrt{z_1^2 + ... + z_{n-1}^2}
1681  * @f]
1682  * @param A A @f$\mathbb{R}^{n\times m}@f$ matrix, whose number of columns
1683  * equals to the size of the decision variables.
1684  * @param b A @f$\mathbb{R}^n@f$ vector, whose number of rows equals to the
1685  * size of the decision variables.
1686  * @param vars The list of @f$ m @f$ decision variables.
1687  * @return The newly added Lorentz cone constraint.
1688  */
1690  const Eigen::Ref<const Eigen::MatrixXd>& A,
1691  const Eigen::Ref<const Eigen::VectorXd>& b, const VariableRefList& vars) {
1692  return AddLorentzConeConstraint(A, b, ConcatenateVariableRefList(vars));
1693  }
1694 
1695  /**
1696  * Adds Lorentz cone constraint referencing potentially a subset of the
1697  * decision variables (defined in the vars parameter).
1698  * The linear expression @f$ z=Ax+b @f$ is in the Lorentz cone.
1699  * A vector \f$ z \in\mathbb{R}^n \f$ is in the Lorentz cone, if
1700  * <!--
1701  * z(0) >= sqrt{z(1)² + ... + z(n-1)²}
1702  * -->
1703  * @f[
1704  * z_0 \ge \sqrt{z_1^2 + ... + z_{n-1}^2}
1705  * @f]
1706  * @param A A @f$\mathbb{R}^{n\times m}@f$ matrix, whose number of columns
1707  * equals to the size of the decision variables.
1708  * @param b A @f$\mathbb{R}^n@f$ vector, whose number of rows equals to the
1709  * size of the decision variables.
1710  * @param vars The Eigen vector of @f$ m @f$ decision variables.
1711  * @return The newly added Lorentz cone constraint.
1712  */
1713  Binding<LorentzConeConstraint> AddLorentzConeConstraint(
1714  const Eigen::Ref<const Eigen::MatrixXd>& A,
1715  const Eigen::Ref<const Eigen::VectorXd>& b,
1716  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1717 
1718  /**
1719  * Imposes that a vector @f$ x\in\mathbb{R}^m @f$ lies in Lorentz cone. Namely
1720  * @f[
1721  * x_0 \ge \sqrt{x_1^2 + .. + x_{m-1}^2}
1722  * @f]
1723  * <!-->
1724  * x(0) >= sqrt(x(1)² + ... + x(m-1)²)
1725  * <-->
1726  * @param vars The stacked column of vars should lie within the Lorentz cone.
1727  * @return The newly added Lorentz cone constraint.
1728  */
1730  const VariableRefList& vars) {
1731  return AddLorentzConeConstraint(ConcatenateVariableRefList(vars));
1732  }
1733 
1734  /**
1735  * Imposes that a vector @f$ x\in\mathbb{R}^m @f$ lies in Lorentz cone. Namely
1736  * @f[
1737  * x_0 \ge \sqrt{x_1^2 + .. + x_{m-1}^2}
1738  * @f]
1739  * <!-->
1740  * x(0) >= sqrt(x(1)² + ... + x(m-1)²)
1741  * <-->
1742  * @param vars The stacked column of vars should lie within the Lorentz cone.
1743  * @return The newly added Lorentz cone constraint.
1744  */
1745  template <int rows>
1747  const Eigen::MatrixBase<VectorDecisionVariable<rows>>& vars) {
1748  Eigen::Matrix<double, rows, rows> A(vars.rows(), vars.rows());
1749  A.setIdentity();
1750  Eigen::Matrix<double, rows, 1> b(vars.rows());
1751  b.setZero();
1752  return AddLorentzConeConstraint(A, b, vars);
1753  }
1754 
1755  /**
1756  * Adds a rotated Lorentz cone constraint referencing potentially a subset
1757  * of decision variables. The linear expression @f$ z=Ax+b @f$ is in rotated
1758  * Lorentz cone.
1759  * A vector \f$ z \in\mathbb{R}^n \f$ is in the rotated Lorentz cone, if
1760  * <!--
1761  * z(0)*z(1) >= z(2)² + ... + z(n-1)²
1762  * -->
1763  * @f[
1764  * z_0z_1 \ge z_2^2 + ... + z_{n-1}^2
1765  * @f]
1766  */
1768  const Binding<RotatedLorentzConeConstraint>& binding);
1769 
1770  /**
1771  * Adds rotated Lorentz cone constraint on the linear expression v1, v2 and
1772  * quadratic expression u, such that v1 * v2 >= u, v1 >= 0, v2 >= 0
1773  * @param linear_expression1 The linear expression v1.
1774  * @param linear_expression2 The linear expression v2.
1775  * @param quadratic_expression The quadratic expression u.
1776  * @param tol The tolerance to determine if the matrix in v2 is positive
1777  * semidefinite or not. @see DecomposePositiveQuadraticForm for more
1778  * explanation. @default is 0.
1779  * @retval binding The newly added rotated Lorentz cone constraint, together
1780  * with the bound variables.
1781  * @pre
1782  * 1. `linear_expression1` is a linear (affine) expression, in the form of
1783  * v1 = c1'*x + d1.
1784  * 2. `linear_expression2` is a linear (affine) expression, in the form of
1785  * v2 = c2'*x + d2.
1786  * 2. `quadratic_expression` is a quadratic expression, in the form of
1787  * <pre>
1788  * u = x'*Q*x + b'x + a
1789  * </pre>
1790  * Also the quadratic expression has to be convex, namely Q is a
1791  * positive semidefinite matrix, and the quadratic expression needs
1792  * to be non-negative for any x.
1793  * Throws a runtime_error if the preconditions are not satisfied.
1794  */
1795  Binding<RotatedLorentzConeConstraint> AddRotatedLorentzConeConstraint(
1796  const symbolic::Expression& linear_expression1,
1797  const symbolic::Expression& linear_expression2,
1798  const symbolic::Expression& quadratic_expression, double tol = 0);
1799 
1800  /**
1801  * Adds a constraint that a symbolic expression @param v is in the rotated
1802  * Lorentz cone, i.e.,
1803  * \f[
1804  * v_0v_1 \ge v_2^2 + ... + v_{n-1}^2\\
1805  * v_0 \ge 0, v_1 \ge 0
1806  * \f]
1807  * @param v A linear expression of variables, \f$ v = A x + b\f$, where \f$ A,
1808  * b \f$ are given matrices of the correct size, \f$ x \f$ is the vector of
1809  * decision variables.
1810  * @retval binding The newly added rotated Lorentz cone constraint, together
1811  * with the bound variables.
1812  */
1813  Binding<RotatedLorentzConeConstraint> AddRotatedLorentzConeConstraint(
1814  const Eigen::Ref<const VectorX<symbolic::Expression>>& v);
1815 
1816  /**
1817  * Adds a rotated Lorentz cone constraint referencing potentially a subset
1818  * of decision variables, The linear expression @f$ z=Ax+b @f$ is in rotated
1819  * Lorentz cone.
1820  * A vector \f$ z \in\mathbb{R}^n \f$ is in the rotated Lorentz cone, if
1821  * <!--
1822  * z(0)*z(1) >= z(2)² + ... + z(n-1)²
1823  * -->
1824  * @f[
1825  * z_0z_1 \ge z_2^2 + ... + z_{n-1}^2
1826  * @f]
1827  * where @f$ A\in\mathbb{R}^{n\times m}, b\in\mathbb{R}^n@f$ are given
1828  * matrices.
1829  * @param A A matrix whose number of columns equals to the size of the
1830  * decision variables.
1831  * @param b A vector whose number of rows equals to the size fo the decision
1832  * variables.
1833  * @param vars The decision variables on which the constraint is imposed.
1834  */
1836  const Eigen::Ref<const Eigen::MatrixXd>& A,
1837  const Eigen::Ref<const Eigen::VectorXd>& b, const VariableRefList& vars) {
1838  return AddRotatedLorentzConeConstraint(A, b,
1840  }
1841 
1842  /**
1843  * Adds a rotated Lorentz cone constraint referencing potentially a subset
1844  * of decision variables, The linear expression @f$ z=Ax+b @f$ is in rotated
1845  * Lorentz cone.
1846  * A vector \f$ z \in\mathbb{R}^n \f$ is in the rotated Lorentz cone, if
1847  * <!--
1848  * z(0)*z(1) >= z(2)² + ... + z(n-1)²
1849  * -->
1850  * @f[
1851  * z_0z_1 \ge z_2^2 + ... + z_{n-1}^2
1852  * @f]
1853  * where @f$ A\in\mathbb{R}^{n\times m}, b\in\mathbb{R}^n@f$ are given
1854  * matrices.
1855  * @param A A matrix whose number of columns equals to the size of the
1856  * decision variables.
1857  * @param b A vector whose number of rows equals to the size fo the decision
1858  * variables.
1859  * @param vars The decision variables on which the constraint is imposed.
1860  */
1861  Binding<RotatedLorentzConeConstraint> AddRotatedLorentzConeConstraint(
1862  const Eigen::Ref<const Eigen::MatrixXd>& A,
1863  const Eigen::Ref<const Eigen::VectorXd>& b,
1864  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1865 
1866  /**
1867  * Impose that a vector @f$ x\in\mathbb{R}^m @f$ is in rotated Lorentz cone.
1868  * Namely
1869  * @f[
1870  * x_0 x_1 \ge x_2^2 + ... + x_{m-1}^2\\
1871  * x_0 \ge 0, x_1 \ge 0
1872  * @f]
1873  * <!-->
1874  * x(0)*x(1) >= x(2)^2 + ... x(m-1)^2
1875  * x(0) >= 0, x(1) >= 0
1876  * <-->
1877  * @param vars The stacked column of vars lies in the rotated Lorentz cone.
1878  * @return The newly added rotated Lorentz cone constraint.
1879  */
1881  const VariableRefList& vars) {
1882  return AddRotatedLorentzConeConstraint(ConcatenateVariableRefList(vars));
1883  }
1884 
1885  /**
1886  * Impose that a vector @f$ x\in\mathbb{R}^m @f$ is in rotated Lorentz cone.
1887  * Namely
1888  * @f[
1889  * x_0 x_1 \ge x_2^2 + ... + x_{m-1}^2\\
1890  * x_0 \ge 0, x_1 \ge 0
1891  * @f]
1892  * <!-->
1893  * x(0)*x(1) >= x(2)^2 + ... x(m-1)^2
1894  * x(0) >= 0, x(1) >= 0
1895  * <-->
1896  * @param vars The stacked column of vars lies in the rotated Lorentz cone.
1897  * @return The newly added rotated Lorentz cone constraint.
1898  */
1899  template <int rows>
1901  const Eigen::MatrixBase<VectorDecisionVariable<rows>>& vars) {
1902  Eigen::Matrix<double, rows, rows> A(vars.rows(), vars.rows());
1903  A.setIdentity();
1904  Eigen::Matrix<double, rows, 1> b(vars.rows());
1905  b.setZero();
1906  return AddRotatedLorentzConeConstraint(A, b, vars);
1907  }
1908 
1909  /**
1910  * Adds a linear complementarity constraints referencing a subset of
1911  * the decision variables.
1912  */
1915 
1916  /**
1917  * Adds a linear complementarity constraints referencing a subset of
1918  * the decision variables.
1919  */
1921  const Eigen::Ref<const Eigen::MatrixXd>& M,
1922  const Eigen::Ref<const Eigen::VectorXd>& q, const VariableRefList& vars) {
1923  return AddLinearComplementarityConstraint(M, q,
1925  }
1926 
1927  /**
1928  * Adds a linear complementarity constraints referencing a subset of
1929  * the decision variables.
1930  */
1931  Binding<LinearComplementarityConstraint> AddLinearComplementarityConstraint(
1932  const Eigen::Ref<const Eigen::MatrixXd>& M,
1933  const Eigen::Ref<const Eigen::VectorXd>& q,
1934  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1935 
1936  /**
1937  * Adds a polynomial constraint to the program referencing a subset
1938  * of the decision variables (defined in the vars parameter).
1939  */
1941  const VectorXPoly& polynomials,
1942  const std::vector<Polynomiald::VarType>& poly_vars,
1943  const Eigen::VectorXd& lb, const Eigen::VectorXd& ub,
1944  const VariableRefList& vars) {
1945  return AddPolynomialConstraint(polynomials, poly_vars, lb, ub,
1947  }
1948 
1949  /**
1950  * Adds a polynomial constraint to the program referencing a subset
1951  * of the decision variables (defined in the vars parameter).
1952  */
1953  Binding<Constraint> AddPolynomialConstraint(
1954  const VectorXPoly& polynomials,
1955  const std::vector<Polynomiald::VarType>& poly_vars,
1956  const Eigen::VectorXd& lb, const Eigen::VectorXd& ub,
1957  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1958 
1959  /**
1960  * Adds a positive semidefinite constraint on a symmetric matrix.
1961  */
1964 
1965  /**
1966  * Adds a positive semidefinite constraint on a symmetric matrix.
1967  */
1969  std::shared_ptr<PositiveSemidefiniteConstraint> con,
1970  const Eigen::Ref<const MatrixXDecisionVariable>& symmetric_matrix_var);
1971 
1972  /**
1973  * Adds a positive semidefinite constraint on a symmetric matrix.
1974  * In Debug mode, @throws error if
1975  * @p symmetric_matrix_var is not symmetric.
1976  * @param symmetric_matrix_var A symmetric MatrixDecisionVariable object.
1977  */
1978  Binding<PositiveSemidefiniteConstraint> AddPositiveSemidefiniteConstraint(
1979  const Eigen::Ref<const MatrixXDecisionVariable>& symmetric_matrix_var);
1980 
1981  /**
1982  * Adds a positive semidefinite constraint on a symmetric matrix of symbolic
1983  * expressions @p e. We create a new symmetric matrix of variables M being
1984  * positive semidefinite, with the linear equality constraint e == M.
1985  * @tparam Derived An Eigen Matrix of symbolic expressions.
1986  * @param e Imposes constraint "e is positive semidefinite".
1987  * @pre{1. e is symmetric.
1988  * 2. e(i, j) is linear for all i, j
1989  * }
1990  * @return The newly added positive semidefinite constraint, with the bound
1991  * variable M that are also newly added.
1992  */
1993  template <typename Derived>
1994  typename std::enable_if<
1997  AddPositiveSemidefiniteConstraint(const Eigen::MatrixBase<Derived>& e) {
1998  DRAKE_DEMAND(e.rows() == e.cols());
1999  DRAKE_ASSERT(e == e.transpose());
2000  const int e_rows = Derived::RowsAtCompileTime;
2002  if (e_rows == Eigen::Dynamic) {
2003  M = NewSymmetricContinuousVariables(e.rows());
2004  } else {
2005  M = NewSymmetricContinuousVariables<e_rows>();
2006  }
2007  // Adds the linear equality constraint that M = e.
2008  AddLinearEqualityConstraint(
2009  e - M, Eigen::Matrix<double, e_rows, e_rows>::Zero(e.rows(), e.rows()),
2010  true);
2011  const int M_flat_size =
2012  e_rows == Eigen::Dynamic ? Eigen::Dynamic : e_rows * e_rows;
2013  const Eigen::Map<Eigen::Matrix<symbolic::Variable, M_flat_size, 1>> M_flat(
2014  &M(0, 0), e.size());
2015  return AddPositiveSemidefiniteConstraint(M);
2016  }
2017 
2018  /**
2019  * Adds a linear matrix inequality constraint to the program.
2020  */
2023 
2024  /**
2025  * Adds a linear matrix inequality constraint to the program.
2026  */
2028  const std::vector<Eigen::Ref<const Eigen::MatrixXd>>& F,
2029  const VariableRefList& vars) {
2030  return AddLinearMatrixInequalityConstraint(
2031  F, ConcatenateVariableRefList(vars));
2032  }
2033 
2034  /**
2035  * Adds a linear matrix inequality constraint to the program.
2036  */
2037  Binding<LinearMatrixInequalityConstraint> AddLinearMatrixInequalityConstraint(
2038  const std::vector<Eigen::Ref<const Eigen::MatrixXd>>& F,
2039  const Eigen::Ref<const VectorXDecisionVariable>& vars);
2040 
2041  /**
2042  * Adds constraints that a given polynomial @p p is a sums-of-squares (SOS),
2043  * that is, @p p can be decomposed into `mᵀQm`, where m is the @p
2044  * monomial_basis. It returns a pair of constraint bindings expressing:
2045  * - The coefficients matrix Q is PSD (positive semidefinite).
2046  * - The coefficients matching conditions in linear equality constraint.
2047  */
2048  std::pair<Binding<PositiveSemidefiniteConstraint>,
2050  AddSosConstraint(
2051  const symbolic::Polynomial& p,
2052  const Eigen::Ref<const VectorX<symbolic::Monomial>>& monomial_basis);
2053 
2054  /**
2055  * Adds constraints that a given polynomial @p p is a sums-of-squares (SOS),
2056  * that is, @p p can be decomposed into `mᵀQm`, where m is the monomial
2057  * basis of all indeterminates in the program with degree equal to half the
2058  * TotalDegree of @p p. It returns a pair of constraint bindings expressing:
2059  * - The coefficients matrix Q is PSD (positive semidefinite).
2060  * - The coefficients matching conditions in linear equality constraint.
2061  */
2062  std::pair<Binding<PositiveSemidefiniteConstraint>,
2064  AddSosConstraint(const symbolic::Polynomial& p);
2065 
2066  /**
2067  * Adds constraints that a given symbolic expression @p e is a
2068  * sums-of-squares (SOS), that is, @p p can be decomposed into `mᵀQm`,
2069  * where m is the @p monomial_basis. Note that it decomposes @p e into a
2070  * polynomial with respect to `indeterminates()` in this mathematical
2071  * program. It returns a pair of constraint bindings expressing:
2072  * - The coefficients matrix Q is PSD (positive semidefinite).
2073  * - The coefficients matching conditions in linear equality constraint.
2074  */
2075  std::pair<Binding<PositiveSemidefiniteConstraint>,
2077  AddSosConstraint(
2078  const symbolic::Expression& e,
2079  const Eigen::Ref<const VectorX<symbolic::Monomial>>& monomial_basis);
2080 
2081  /**
2082  * Adds constraints that a given symbolic expression @p e is a sums-of-squares
2083  * (SOS), that is, @p e can be decomposed into `mTQm`. Note that it decomposes
2084  * @p e into a polynomial with respect to `indeterminates()` in this
2085  * mathematical program. It returns a pair of
2086  * constraint bindings expressing:
2087  * - The coefficients matrix Q is PSD (positive semidefinite).
2088  * - The coefficients matching conditions in linear equality constraint.
2089  */
2090  std::pair<Binding<PositiveSemidefiniteConstraint>,
2092  AddSosConstraint(const symbolic::Expression& e);
2093 
2094  // template <typename FunctionType>
2095  // void AddCost(std::function..);
2096  // void AddLinearCost(const Eigen::MatrixBase<Derived>& c, const vector<const
2097  // DecisionVariable&>& vars)
2098  // void addQuadraticCost ...
2099 
2100  /**
2101  * Gets the initial guess for a single variable.
2102  * @pre @p decision_variable has been registered in the optimization program.
2103  * @throw runtime error if the pre condition is not satisfied.
2104  */
2105  double GetInitialGuess(const symbolic::Variable& decision_variable) const;
2106 
2107  /**
2108  * Gets the initial guess for some variables.
2109  * @pre Each variable in @p decision_variable_mat has been registered in the
2110  * optimization program.
2111  * @throw runtime error if the pre condition is not satisfied.
2112  */
2113  template <typename Derived>
2114  typename std::enable_if<
2116  Eigen::Matrix<double, Derived::RowsAtCompileTime,
2117  Derived::ColsAtCompileTime>>::type
2119  const Eigen::MatrixBase<Derived>& decision_variable_mat) const {
2120  Eigen::Matrix<double, Derived::RowsAtCompileTime,
2121  Derived::ColsAtCompileTime>
2122  decision_variable_values(decision_variable_mat.rows(),
2123  decision_variable_mat.cols());
2124  for (int i = 0; i < decision_variable_mat.rows(); ++i) {
2125  for (int j = 0; j < decision_variable_mat.cols(); ++j) {
2126  decision_variable_values(i, j) =
2127  GetInitialGuess(decision_variable_mat(i, j));
2128  }
2129  }
2130  return decision_variable_values;
2131  }
2132 
2133  /**
2134  * Sets the initial guess for a single variable @p decision_variable.
2135  * @pre decision_variable is a registered decision variable in the program.
2136  * @throw a runtime error if precondition is not satisfied.
2137  */
2138  void SetInitialGuess(const symbolic::Variable& decision_variable,
2139  double variable_guess_value);
2140 
2141  /**
2142  * Sets the initial guess for the decision variables stored in
2143  * @p decision_variable_mat to be @p x0. Variables begin with a default
2144  * initial guess of NaN to indicate that no guess is available.
2145  */
2146  template <typename DerivedA, typename DerivedB>
2147  void SetInitialGuess(const Eigen::MatrixBase<DerivedA>& decision_variable_mat,
2148  const Eigen::MatrixBase<DerivedB>& x0) {
2149  DRAKE_ASSERT(decision_variable_mat.rows() == x0.rows());
2150  DRAKE_ASSERT(decision_variable_mat.cols() == x0.cols());
2151  for (int i = 0; i < decision_variable_mat.rows(); ++i) {
2152  for (int j = 0; j < decision_variable_mat.cols(); ++j) {
2153  SetInitialGuess(decision_variable_mat(i, j), x0(i, j));
2154  }
2155  }
2156  }
2157 
2158  /**
2159  * Set the intial guess for ALL decision variables.
2160  * Note that variables begin with a default initial guess of NaN to indicate
2161  * that no guess is available.
2162  * @param x0 A vector of appropriate size (num_vars() x 1).
2163  */
2164  template <typename Derived>
2165  void SetInitialGuessForAllVariables(const Eigen::MatrixBase<Derived>& x0) {
2166  DRAKE_ASSERT(x0.rows() == num_vars() && x0.cols() == 1);
2167  x_initial_guess_ = x0;
2168  }
2169 
2170  /**
2171  * Solve the MathematicalProgram.
2172  *
2173  * @return SolutionResult indicating if the solution was successful.
2174  */
2175  SolutionResult Solve();
2176  // TODO(naveenoid) : add argument for options
2177 
2178  // template <typename Derived>
2179  // bool solve(const Eigen::MatrixBase<Derived>& x0);
2180 
2181  // getCostValue();
2182  // getExitFlag();
2183  // getInfeasibleConstraintNames();
2184 
2185  void PrintSolution() {
2186  for (int i = 0; i < num_vars(); ++i) {
2187  std::cout << decision_variables_(i).get_name() << " = "
2188  << GetSolution(decision_variables_(i)) << std::endl;
2189  }
2190  }
2191 
2192  /**
2193  * Set an option for a particular solver. This interface does not
2194  * do any verification of solver parameters beyond what an
2195  * individual solver does for itself. It does not even verify that
2196  * the specified solver exists. Use this only when you have
2197  * particular knowledge of what solver is being invoked, and exactly
2198  * what tuning is required.
2199  *
2200  * Supported solver names/options:
2201  *
2202  * "SNOPT" -- Paramater names and values as specified in SNOPT
2203  * User's Guide section 7.7 "Description of the optional parameters",
2204  * used as described in section 7.5 for snSet().
2205  *
2206  * "IPOPT" -- Paramater names and values as specified in IPOPT users
2207  * guide section "Options Reference"
2208  * http://www.coin-or.org/Ipopt/documentation/node40.html
2209  *
2210  * "GUROBI" -- Parameter name and values as specified in Gurobi Reference
2211  * Manual, section 10.2 "Parameter Descriptions"
2212  * https://www.gurobi.com/documentation/7.5/refman/parameters.html
2213  */
2214  void SetSolverOption(const SolverId& solver_id,
2215  const std::string& solver_option, double option_value) {
2216  solver_options_double_[solver_id][solver_option] = option_value;
2217  }
2218 
2219  void SetSolverOption(const SolverId& solver_id,
2220  const std::string& solver_option, int option_value) {
2221  solver_options_int_[solver_id][solver_option] = option_value;
2222  }
2223 
2224  void SetSolverOption(const SolverId& solver_id,
2225  const std::string& solver_option,
2226  const std::string& option_value) {
2227  solver_options_str_[solver_id][solver_option] = option_value;
2228  }
2229 
2230  const std::map<std::string, double>& GetSolverOptionsDouble(
2231  const SolverId& solver_id) {
2232  return solver_options_double_[solver_id];
2233  }
2234 
2235  const std::map<std::string, int>& GetSolverOptionsInt(
2236  const SolverId& solver_id) {
2237  return solver_options_int_[solver_id];
2238  }
2239 
2240  const std::map<std::string, std::string>& GetSolverOptionsStr(
2241  const SolverId& solver_id) {
2242  return solver_options_str_[solver_id];
2243  }
2244 
2245  /**
2246  * Returns the ID of the solver that was used to solve this program.
2247  * Returns empty if Solve() has not been called.
2248  */
2249  optional<SolverId> GetSolverId() const { return solver_id_; }
2250 
2251  /**
2252  * Getter for optimal cost at the solution.
2253  * If the solver finds an optimal solution, then we return the cost evaluated
2254  * at this solution.
2255  * If the program is unbounded, then the optimal cost is -∞.
2256  * If the program is globally infeasible, then the optimal cost is +∞.
2257  * If the program is locally infeasible, then the solver (e.g. SNOPT) might
2258  * return some finite value as the optimal cost.
2259  * Otherwise, the optimal cost is NaN.
2260  */
2261  double GetOptimalCost() const { return optimal_cost_; }
2262 
2263  /**
2264  * Getter for lower bound on optimal cost. Defaults to -Infinity
2265  * if a lower bound has not been found.
2266  */
2267  double GetLowerBoundCost() const { return lower_bound_cost_; }
2268 
2269  /**
2270  * Getter for all callbacks.
2271  */
2272  const std::vector<Binding<VisualizationCallback>>& visualization_callbacks()
2273  const {
2274  return visualization_callbacks_;
2275  }
2276 
2277  /**
2278  * Getter for all generic costs.
2279  */
2280  const std::vector<Binding<Cost>>& generic_costs() const {
2281  return generic_costs_;
2282  } // e.g. for snopt_user_fun
2283 
2284  /**
2285  * Getter for all generic constraints
2286  */
2287  const std::vector<Binding<Constraint>>& generic_constraints() const {
2288  return generic_constraints_;
2289  } // e.g. for snopt_user_fun
2290 
2291  /**
2292  * Getter for linear equality constraints.
2293  */
2294  const std::vector<Binding<LinearEqualityConstraint>>&
2296  return linear_equality_constraints_;
2297  }
2298 
2299  /** Getter for linear costs. */
2300  const std::vector<Binding<LinearCost>>& linear_costs() const {
2301  return linear_costs_;
2302  }
2303 
2304  /** Getter for quadratic costs. */
2305  const std::vector<Binding<QuadraticCost>>& quadratic_costs() const {
2306  return quadratic_costs_;
2307  }
2308 
2309  /** Getter for linear constraints. */
2310  const std::vector<Binding<LinearConstraint>>& linear_constraints() const {
2311  return linear_constraints_;
2312  }
2313 
2314  /** Getter for Lorentz cone constraint */
2315  const std::vector<Binding<LorentzConeConstraint>>& lorentz_cone_constraints()
2316  const {
2317  return lorentz_cone_constraint_;
2318  }
2319 
2320  /** Getter for rotated Lorentz cone constraint */
2321  const std::vector<Binding<RotatedLorentzConeConstraint>>&
2323  return rotated_lorentz_cone_constraint_;
2324  }
2325 
2326  /** Getter for positive semidefinite constraint */
2327  const std::vector<Binding<PositiveSemidefiniteConstraint>>&
2329  return positive_semidefinite_constraint_;
2330  }
2331 
2332  /** Getter for linear matrix inequality constraint */
2333  const std::vector<Binding<LinearMatrixInequalityConstraint>>&
2335  return linear_matrix_inequality_constraint_;
2336  }
2337 
2338  /**
2339  * Getter returning all costs (for now linear costs appended to
2340  * generic costs, then quadratic costs appended to
2341  * generic costs).
2342  */
2343  std::vector<Binding<Cost>> GetAllCosts() const {
2344  auto costlist = generic_costs_;
2345  costlist.insert(costlist.end(), linear_costs_.begin(), linear_costs_.end());
2346  costlist.insert(costlist.end(), quadratic_costs_.begin(),
2347  quadratic_costs_.end());
2348  return costlist;
2349  }
2350 
2351  /**
2352  * Getter returning all linear constraints (both linear equality and
2353  * inequality constraints).
2354  */
2355  std::vector<Binding<LinearConstraint>> GetAllLinearConstraints() const {
2356  std::vector<Binding<LinearConstraint>> conlist = linear_constraints_;
2357  conlist.insert(conlist.end(), linear_equality_constraints_.begin(),
2358  linear_equality_constraints_.end());
2359  return conlist;
2360  }
2361 
2362  /** Getter for all bounding box constraints */
2363  const std::vector<Binding<BoundingBoxConstraint>>& bounding_box_constraints()
2364  const {
2365  return bbox_constraints_;
2366  }
2367 
2368  /** Getter for all linear complementarity constraints.*/
2369  const std::vector<Binding<LinearComplementarityConstraint>>&
2371  return linear_complementarity_constraints_;
2372  }
2373 
2374  // Base class for solver-specific data. A solver implementation may derive
2375  // a helper class from this for use with getSolverData.
2376  struct SolverData {
2378  SolverData() = default;
2379  virtual ~SolverData() = default;
2380  };
2381 
2382  // Call from solver implementations to get a persistently-stored
2383  // helper structure of type T (derived from SolverData). If no
2384  // data of type T is already stored then a new one will be created
2385  // and stored, replacing data from any other solver in this problem
2386  // instance.
2387  template <typename T>
2388  std::shared_ptr<T> GetSolverData() {
2389  auto p = std::dynamic_pointer_cast<T>(solver_data_);
2390  if (!p) {
2391  p = std::make_shared<T>();
2392  solver_data_ = p;
2393  }
2394  return p;
2395  }
2396 
2397  /** Getter for number of variables in the optimization program */
2398  int num_vars() const { return decision_variables_.rows(); }
2399 
2400  /** Getter for the initial guess */
2401  const Eigen::VectorXd& initial_guess() const { return x_initial_guess_; }
2402 
2403  /** Returns the index of the decision variable. Internally the solvers thinks
2404  * all variables are stored in an array, and it acceses each individual
2405  * variable using its index. This index is used when adding constraints
2406  * and costs for each solver.
2407  * @pre{@p var is a decision variable in the mathematical program, otherwise
2408  * this function throws a runtime error.}
2409  */
2410  int FindDecisionVariableIndex(const symbolic::Variable& var) const;
2411 
2412  /**
2413  * Returns the indices of the decision variables. Internally the solvers
2414  * thinks all variables are stored in an array, and it acceses each individual
2415  * variable using its index. This index is used when adding constraints
2416  * and costs for each solver.
2417  * @pre{@p vars are decision variables in the mathematical program, otherwise
2418  * this function throws a runtime error.}
2419  */
2420  std::vector<int> FindDecisionVariableIndices(
2421  const Eigen::Ref<const VectorXDecisionVariable>& vars) const;
2422 
2423  /** Gets the number of indeterminates in the optimization program */
2424  int num_indeterminates() const { return indeterminates_.rows(); }
2425 
2426  /** Returns the index of the indeterminate. Internally a solver
2427  * thinks all indeterminates are stored in an array, and it acceses each
2428  * individual indeterminate using its index. This index is used when adding
2429  * constraints and costs for each solver.
2430  * @pre @p var is a indeterminate in the mathematical program,
2431  * otherwise this function throws a runtime error.
2432  */
2433  size_t FindIndeterminateIndex(const symbolic::Variable& var) const;
2434 
2435  /**
2436  * Gets the solution of an Eigen matrix of decision variables.
2437  * @tparam Derived An Eigen matrix containing Variable.
2438  * @param var The decision variables.
2439  * @return The value of the decision variable after solving the problem.
2440  */
2441  template <typename Derived>
2442  typename std::enable_if<
2443  std::is_same<typename Derived::Scalar, symbolic::Variable>::value,
2444  Eigen::Matrix<double, Derived::RowsAtCompileTime,
2445  Derived::ColsAtCompileTime>>::type
2446  GetSolution(const Eigen::MatrixBase<Derived>& var) const {
2447  Eigen::Matrix<double, Derived::RowsAtCompileTime,
2448  Derived::ColsAtCompileTime>
2449  value(var.rows(), var.cols());
2450  for (int i = 0; i < var.rows(); ++i) {
2451  for (int j = 0; j < var.cols(); ++j) {
2452  value(i, j) = GetSolution(var(i, j));
2453  }
2454  }
2455  return value;
2456  }
2457 
2458  /**
2459  * Gets the value of a single decision variable.
2460  */
2461  double GetSolution(const symbolic::Variable& var) const;
2462 
2463  /**
2464  * Replaces the variables in an expression with the solutions to the
2465  * variables, returns the expression after substitution.
2466  * @throw runtime error if some variables in the expression @p e are NOT
2467  * decision variables or indeterminates in the optimization program.
2468  * @note If the expression @p e contains both decision variables and
2469  * indeterminates of the optimization program, then the decision variables
2470  * will be substituted by its solutions in double values, but not the
2471  * indeterminates.
2472  */
2473  symbolic::Expression SubstituteSolution(const symbolic::Expression& e) const;
2474 
2475  /**
2476  * Replaces the decision variables in a polynomial with the solutions to the
2477  * variables, returns the polynomial after substitution.
2478  * @throw runtime error if some decision variables in the polynomial @p p are
2479  * NOT decision variables in the optimization program.
2480  * @note If the polynomial @p p contains both decision variables and
2481  * indeterminates of the optimization program, then the decision variables
2482  * will be substituted by its solutions in double values, but not the
2483  * indeterminates.
2484  */
2485  symbolic::Polynomial SubstituteSolution(const symbolic::Polynomial& p) const;
2486 
2487  /**
2488  * Evaluates the value of some binding, for some input value for all
2489  * decision variables.
2490  * @param binding A Binding whose variables are decision variables in this
2491  * program.
2492  * @param prog_var_vals The value of all the decision variables in this
2493  * program. @throw a logic error if the size does not match.
2494  */
2495  template <typename C, typename DerivedX>
2498  EvalBinding(const Binding<C>& binding,
2499  const Eigen::MatrixBase<DerivedX>& prog_var_vals) const {
2500  using Scalar = typename DerivedX::Scalar;
2501  if (prog_var_vals.rows() != num_vars()) {
2502  std::ostringstream oss;
2503  oss << "The input binding variable is not in the right size. Expects "
2504  << num_vars() << " rows, but it actually has " << prog_var_vals.rows()
2505  << " rows.\n";
2506  throw std::logic_error(oss.str());
2507  }
2508  VectorX<Scalar> binding_x(binding.GetNumElements());
2509  VectorX<Scalar> binding_y(binding.evaluator()->num_outputs());
2510  for (int i = 0; i < static_cast<int>(binding.GetNumElements()); ++i) {
2511  binding_x(i) =
2512  prog_var_vals(FindDecisionVariableIndex(binding.variables()(i)));
2513  }
2514  binding.evaluator()->Eval(binding_x, binding_y);
2515  return binding_y;
2516  }
2517 
2518  /** Evaluates all visualization callbacks registered with the
2519  * MathematicalProgram.
2520  *
2521  * @param prog_var_vals The value of all the decision variables in this
2522  * program. @throw a logic error if the size does not match.
2523  **/
2525  const Eigen::Ref<const Eigen::VectorXd>& prog_var_vals) const {
2526  if (prog_var_vals.rows() != num_vars()) {
2527  std::ostringstream oss;
2528  oss << "The input binding variable is not in the right size. Expects "
2529  << num_vars() << " rows, but it actually has " << prog_var_vals.rows()
2530  << " rows.\n";
2531  throw std::logic_error(oss.str());
2532  }
2533 
2534  Eigen::VectorXd this_x;
2535 
2536  for (auto const& binding : visualization_callbacks_) {
2537  auto const& obj = binding.evaluator();
2538 
2539  const int num_v_variables = binding.GetNumElements();
2540  this_x.resize(num_v_variables);
2541  for (int j = 0; j < num_v_variables; ++j) {
2542  this_x(j) =
2543  prog_var_vals(FindDecisionVariableIndex(binding.variables()(j)));
2544  }
2545 
2546  obj->EvalCallback(this_x);
2547  }
2548  }
2549 
2550  /**
2551  * Evaluates the evaluator in @p binding at the solution value.
2552  * @return The value of @p binding at the solution value.
2553  */
2554  template <typename C>
2555  Eigen::VectorXd EvalBindingAtSolution(const Binding<C>& binding) const {
2556  return EvalBinding(binding, x_values_);
2557  }
2558 
2559  /**
2560  * Evaluates the evaluator in @p binding at the initial guess.
2561  * @return The value of @p binding at the initial guess.
2562  */
2563  template <typename C>
2564  Eigen::VectorXd EvalBindingAtInitialGuess(const Binding<C>& binding) const {
2565  return EvalBinding(binding, x_initial_guess_);
2566  }
2567 
2568  /** Getter for all decision variables in the program. */
2570  return decision_variables_;
2571  }
2572 
2573  /** Getter for the decision variable with index @p i in the program. */
2575  return decision_variables_(i);
2576  }
2577 
2578  /** Getter for all indeterminates in the program. */
2580 
2581  /** Getter for the indeterminate with index @p i in the program. */
2582  const symbolic::Variable& indeterminate(int i) const {
2583  return indeterminates_(i);
2584  }
2585 
2586  /**
2587  * Solver reports its result back to MathematicalProgram, by passing the
2588  * solver_result, which contains the solver result.
2589  * @note This method should only be called by each solver, after it solves the
2590  * optimization problem stored in MathematicalProgram. The user should NOT
2591  * call this method.
2592  */
2593  // This method should be called by the derived classes of
2594  // MathematicalProgramSolverInterface, which is not a friend class of
2595  // MathematicalProgram, as we do not want to leak any of the internal details
2596  // of MathematicalProgram.
2597  void SetSolverResult(const SolverResult& solver_result);
2598 
2599  private:
2600  static void AppendNanToEnd(int new_var_size, Eigen::VectorXd* vector);
2601  // maps the ID of a symbolic variable to the index of the variable stored in
2602  // the optimization program.
2603  std::unordered_map<symbolic::Variable::Id, int> decision_variable_index_{};
2604 
2605  VectorXDecisionVariable decision_variables_;
2606 
2607  std::unordered_map<symbolic::Variable::Id, int> indeterminates_index_;
2609 
2610  std::vector<Binding<VisualizationCallback>> visualization_callbacks_;
2611 
2612  std::vector<Binding<Cost>> generic_costs_;
2613  std::vector<Binding<QuadraticCost>> quadratic_costs_;
2614  std::vector<Binding<LinearCost>> linear_costs_;
2615  // TODO(naveenoid) : quadratic_constraints_
2616 
2617  // note: linear_constraints_ does not include linear_equality_constraints_
2618  std::vector<Binding<Constraint>> generic_constraints_;
2619  std::vector<Binding<LinearConstraint>> linear_constraints_;
2620  std::vector<Binding<LinearEqualityConstraint>> linear_equality_constraints_;
2621  std::vector<Binding<BoundingBoxConstraint>> bbox_constraints_;
2622  std::vector<Binding<LorentzConeConstraint>> lorentz_cone_constraint_;
2623  std::vector<Binding<RotatedLorentzConeConstraint>>
2624  rotated_lorentz_cone_constraint_;
2625  std::vector<Binding<PositiveSemidefiniteConstraint>>
2626  positive_semidefinite_constraint_;
2627  std::vector<Binding<LinearMatrixInequalityConstraint>>
2628  linear_matrix_inequality_constraint_;
2629 
2630  // Invariant: The bindings in this list must be non-overlapping, when calling
2631  // Linear Complementarity solver like Moby. If this constraint is solved
2632  // through a nonlinear optimization solver (like SNOPT) instead, then we allow
2633  // the bindings to be overlapping.
2634  // TODO(ggould-tri) can this constraint be relaxed?
2635  std::vector<Binding<LinearComplementarityConstraint>>
2636  linear_complementarity_constraints_;
2637 
2638  Eigen::VectorXd x_initial_guess_;
2639  Eigen::VectorXd x_values_;
2640  std::shared_ptr<SolverData> solver_data_;
2641  optional<SolverId> solver_id_;
2642  double optimal_cost_{};
2643  // The lower bound of the objective found by the solver, during the
2644  // optimization process.
2645  double lower_bound_cost_{};
2646  std::map<SolverId, std::map<std::string, double>> solver_options_double_;
2647  std::map<SolverId, std::map<std::string, int>> solver_options_int_;
2648  std::map<SolverId, std::map<std::string, std::string>> solver_options_str_;
2649 
2650  AttributesSet required_capabilities_{0};
2651 
2652  std::unique_ptr<MathematicalProgramSolverInterface> ipopt_solver_;
2653  std::unique_ptr<MathematicalProgramSolverInterface> nlopt_solver_;
2654  std::unique_ptr<MathematicalProgramSolverInterface> snopt_solver_;
2655  std::unique_ptr<MathematicalProgramSolverInterface> moby_lcp_solver_;
2656  std::unique_ptr<MathematicalProgramSolverInterface> linear_system_solver_;
2657  std::unique_ptr<MathematicalProgramSolverInterface>
2658  equality_constrained_qp_solver_;
2659  std::unique_ptr<MathematicalProgramSolverInterface> gurobi_solver_;
2660  std::unique_ptr<MathematicalProgramSolverInterface> mosek_solver_;
2661  std::unique_ptr<MathematicalProgramSolverInterface> osqp_solver_;
2662  std::unique_ptr<MathematicalProgramSolverInterface> scs_solver_;
2663 
2664  template <typename T>
2665  void NewVariables_impl(
2666  VarType type, const T& names, bool is_symmetric,
2667  Eigen::Ref<MatrixXDecisionVariable> decision_variable_matrix) {
2668  switch (type) {
2669  case VarType::CONTINUOUS:
2670  break;
2671  case VarType::BINARY:
2672  required_capabilities_ |= kBinaryVariable;
2673  break;
2674  case VarType::INTEGER:
2675  throw std::runtime_error(
2676  "MathematicalProgram does not support integer variables yet.");
2677  case VarType::BOOLEAN:
2678  throw std::runtime_error(
2679  "MathematicalProgram does not support Boolean variables.");
2680  }
2681  int rows = decision_variable_matrix.rows();
2682  int cols = decision_variable_matrix.cols();
2683  DRAKE_ASSERT(!is_symmetric || rows == cols);
2684  int num_new_vars = 0;
2685  if (!is_symmetric) {
2686  num_new_vars = rows * cols;
2687  } else {
2688  num_new_vars = rows * (rows + 1) / 2;
2689  }
2690  DRAKE_ASSERT(static_cast<int>(names.size()) == num_new_vars);
2691  decision_variables_.conservativeResize(num_vars() + num_new_vars,
2692  Eigen::NoChange);
2693  AppendNanToEnd(num_new_vars, &x_values_);
2694  int row_index = 0;
2695  int col_index = 0;
2696  for (int i = 0; i < num_new_vars; ++i) {
2697  decision_variables_(num_vars() - num_new_vars + i) =
2698  symbolic::Variable(names[i], type);
2699  const int new_var_index = num_vars() - num_new_vars + i;
2700  decision_variable_index_.insert(std::pair<int, int>(
2701  decision_variables_(new_var_index).get_id(), new_var_index));
2702  decision_variable_matrix(row_index, col_index) =
2703  decision_variables_(num_vars() - num_new_vars + i);
2704  // If the matrix is not symmetric, then store the variable in column
2705  // major.
2706  if (!is_symmetric) {
2707  if (row_index + 1 < rows) {
2708  ++row_index;
2709  } else {
2710  ++col_index;
2711  row_index = 0;
2712  }
2713  } else {
2714  // If the matrix is symmetric, then the decision variables are the lower
2715  // triangular part of the symmetric matrix, also stored in column major.
2716  if (row_index != col_index) {
2717  decision_variable_matrix(col_index, row_index) =
2718  decision_variable_matrix(row_index, col_index);
2719  }
2720  if (row_index + 1 < rows) {
2721  ++row_index;
2722  } else {
2723  ++col_index;
2724  row_index = col_index;
2725  }
2726  }
2727  }
2728 
2729  AppendNanToEnd(num_new_vars, &x_initial_guess_);
2730  }
2731 
2732  MatrixXDecisionVariable NewVariables(VarType type, int rows, int cols,
2733  bool is_symmetric,
2734  const std::vector<std::string>& names);
2735 
2736  template <typename T>
2737  void NewIndeterminates_impl(
2738  const T& names, Eigen::Ref<MatrixXIndeterminate> indeterminates_matrix) {
2739  int rows = indeterminates_matrix.rows();
2740  int cols = indeterminates_matrix.cols();
2741  int num_new_vars = rows * cols;
2742 
2743  DRAKE_ASSERT(static_cast<int>(names.size()) == num_new_vars);
2744  indeterminates_.conservativeResize(indeterminates_.rows() + num_new_vars,
2745  Eigen::NoChange);
2746  int row_index = 0;
2747  int col_index = 0;
2748  for (int i = 0; i < num_new_vars; ++i) {
2749  indeterminates_(indeterminates_.rows() - num_new_vars + i) =
2750  symbolic::Variable(names[i]);
2751 
2752  const int new_var_index = indeterminates_.rows() - num_new_vars + i;
2753  indeterminates_index_.insert(std::pair<size_t, size_t>(
2754  indeterminates_(new_var_index).get_id(), new_var_index));
2755  indeterminates_matrix(row_index, col_index) =
2756  indeterminates_(indeterminates_.rows() - num_new_vars + i);
2757 
2758  // store the indeterminate in column major.
2759  if (row_index + 1 < rows) {
2760  ++row_index;
2761  } else {
2762  ++col_index;
2763  row_index = 0;
2764  }
2765  }
2766  }
2767 
2768  /*
2769  * Given a matrix of decision variables, checks if every entry in the
2770  * matrix is a decision variable in the program; throws a runtime
2771  * error if any variable is not a decision variable in the program.
2772  * @tparam Derived An Eigen::Matrix type of symbolic Variable.
2773  * @param vars A matrix of variables.
2774  */
2775  template <typename Derived>
2776  typename std::enable_if<
2777  std::is_same<typename Derived::Scalar, symbolic::Variable>::value>::type
2778  CheckIsDecisionVariable(const Eigen::MatrixBase<Derived>& vars) const {
2779  for (int i = 0; i < vars.rows(); ++i) {
2780  for (int j = 0; j < vars.cols(); ++j) {
2781  if (decision_variable_index_.find(vars(i, j).get_id()) ==
2782  decision_variable_index_.end()) {
2783  std::ostringstream oss;
2784  oss << vars(i, j)
2785  << " is not a decision variable of the mathematical program.\n";
2786  throw std::runtime_error(oss.str());
2787  }
2788  }
2789  }
2790  }
2791 
2792  /*
2793  * Ensure a binding is valid *before* adding it to the program.
2794  * @pre The binding has not yet been registered.
2795  * @pre The decision variables have been registered.
2796  * @throws std::runtime_error if the binding is invalid.
2797  */
2798  template <typename C>
2799  void CheckBinding(const Binding<C>& binding) const {
2800  // TODO(eric.cousineau): In addition to identifiers, hash bindings by
2801  // their constraints and their variables, to prevent duplicates.
2802  // TODO(eric.cousineau): Once bindings have identifiers (perhaps
2803  // retrofitting `description`), ensure that they have unique names.
2804  CheckIsDecisionVariable(binding.variables());
2805  }
2806 
2807  // Adds a constraint represented by a set of symbolic formulas to the
2808  // program.
2809  //
2810  // Precondition: ∀ f ∈ formulas, is_relational(f).
2811  Binding<Constraint> AddConstraint(
2812  const std::set<symbolic::Formula>& formulas);
2813 
2814  // Adds a linear-equality constraint represented by a set of symbolic formulas
2815  // to the program.
2816  //
2817  // Precondition: ∀ f ∈ formulas, is_equal_to(f).
2818  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
2819  const std::set<symbolic::Formula>& formulas);
2820 
2821  /**
2822  * Adds new variables to MathematicalProgram.
2823  */
2824  template <int Rows, int Cols>
2826  VarType type, const typename NewVariableNames<Rows, Cols>::type& names,
2827  int rows, int cols) {
2828  DRAKE_DEMAND(rows >= 0 && cols >= 0);
2829  MatrixDecisionVariable<Rows, Cols> decision_variable_matrix;
2830  decision_variable_matrix.resize(rows, cols);
2831  NewVariables_impl(type, names, false, decision_variable_matrix);
2832  return decision_variable_matrix;
2833  }
2834 
2835  /**
2836  * Adds symmetric matrix variables to optimization program. Only the lower
2837  * triangular part of the matrix is used as decision variables.
2838  * @param names The names of the stacked columns of the lower triangular part
2839  * of the matrix.
2840  */
2841  template <int Rows>
2842  MatrixDecisionVariable<Rows, Rows> NewSymmetricVariables(
2843  VarType type, const typename NewSymmetricVariableNames<Rows>::type& names,
2844  int rows = Rows) {
2845  MatrixDecisionVariable<Rows, Rows> decision_variable_matrix(rows, rows);
2846  NewVariables_impl(type, names, true, decision_variable_matrix);
2847  return decision_variable_matrix;
2848  }
2849 };
2850 
2851 } // namespace solvers
2852 } // namespace drake
Binding< QuadraticCost > AddQuadraticErrorCost(const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &x_desired, const VariableRefList &vars)
Adds a cost term of the form (x-x_desired)&#39;Q(x-x_desired).
Definition: mathematical_program.h:949
const std::vector< Binding< QuadraticCost > > & quadratic_costs() const
Getter for quadratic costs.
Definition: mathematical_program.h:2305
Binding< C > CreateBinding(const std::shared_ptr< C > &c, Args &&...args)
Definition: binding.h:84
Binding< RotatedLorentzConeConstraint > AddRotatedLorentzConeConstraint(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
Adds a rotated Lorentz cone constraint referencing potentially a subset of decision variables...
Definition: mathematical_program.h:1835
std::enable_if< detail::assert_if_is_constraint< F >::value, Binding< Cost > >::type AddCost(F &&, Vars &&)
Statically assert if a user inadvertently passes a binding-compatible Constraint. ...
Definition: mathematical_program.h:877
void PrintSolution()
Definition: mathematical_program.h:2185
Eigen::Matrix< symbolic::Variable, rows, cols > MatrixDecisionVariable
Definition: decision_variable.h:12
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Expression >::value, Binding< PositiveSemidefiniteConstraint > >::type AddPositiveSemidefiniteConstraint(const Eigen::MatrixBase< Derived > &e)
Adds a positive semidefinite constraint on a symmetric matrix of symbolic expressions e...
Definition: mathematical_program.h:1997
Definition: eigen_types.h:212
Definition: mathematical_program.h:193
std::vector< snopt::doublereal > F
Definition: snopt_solver.cc:87
VectorXDecisionVariable NewBinaryVariables(int rows, const std::string &name="b")
Adds binary variables to this MathematicalProgram.
Definition: mathematical_program.h:493
shared_ptr< QuadraticCost > MakeL2NormCost(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b)
Creates a cost term of the form | Ax - b |^2.
Definition: cost.cc:46
Definition: mathematical_program.h:2376
const std::vector< Binding< LinearMatrixInequalityConstraint > > & linear_matrix_inequality_constraints() const
Getter for linear matrix inequality constraint.
Definition: mathematical_program.h:2334
Binding< BoundingBoxConstraint > AddBoundingBoxConstraint(double lb, double ub, const VariableRefList &vars)
Adds the same scalar lower and upper bound to every variable in the list.
Definition: mathematical_program.h:1552
int i
Definition: reset_after_move_test.cc:51
std::vector< Binding< LinearConstraint > > GetAllLinearConstraints() const
Getter returning all linear constraints (both linear equality and inequality constraints).
Definition: mathematical_program.h:2355
Represents a symbolic variable.
Definition: symbolic_variable.h:24
Eigen::Matrix< Polynomiald, Eigen::Dynamic, 1 > VectorXPoly
A column vector of polynomials; used in several optimization classes.
Definition: polynomial.h:482
static std::shared_ptr< Cost > MakeCost(F &&f)
Convert an input of type F to a FunctionCost object.
Definition: mathematical_program.h:839
Definition: symbolic_expression.h:951
double value
Definition: wrap_test_util_py.cc:12
const Variables indeterminates_
Definition: symbolic_polynomial_test.cc:26
std::vector< Number > x
Definition: ipopt_solver.cc:150
The type of the names for the newly added variables.
Definition: mathematical_program.h:211
double GetOptimalCost() const
Getter for optimal cost at the solution.
Definition: mathematical_program.h:2261
Binding< RotatedLorentzConeConstraint > AddRotatedLorentzConeConstraint(const VariableRefList &vars)
Impose that a vector is in rotated Lorentz cone.
Definition: mathematical_program.h:1880
std::enable_if< is_eigen_vector_expression_double_pair< DerivedV, DerivedB >::value, Binding< LinearEqualityConstraint > >::type AddLinearEqualityConstraint(const Eigen::MatrixBase< DerivedV > &v, const Eigen::MatrixBase< DerivedB > &b)
Adds linear equality constraints , where v(i) is a symbolic linear expression.
Definition: mathematical_program.h:1365
Definition: automotive_demo.cc:90
int num_indeterminates() const
Gets the number of indeterminates in the optimization program.
Definition: mathematical_program.h:2424
Binding< LinearEqualityConstraint > ParseLinearEqualityConstraint(const set< Formula > &formulas)
Definition: create_constraint.cc:345
auto AddCost(const std::shared_ptr< C > &obj, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost type to the optimization program.
Definition: mathematical_program.h:815
const std::vector< Binding< LinearComplementarityConstraint > > & linear_complementarity_constraints() const
Getter for all linear complementarity constraints.
Definition: mathematical_program.h:2370
std::shared_ptr< Cost > MakeFunctionCost(FF &&f)
Converts an input of type F to a nonlinear cost.
Definition: cost.h:240
Definition: mathematical_program.h:203
Represents a symbolic form of a first-order logic formula.
Definition: symbolic_formula.h:114
Definition: mathematical_program.h:191
Binding< LorentzConeConstraint > AddLorentzConeConstraint(const VariableRefList &vars)
Imposes that a vector lies in Lorentz cone.
Definition: mathematical_program.h:1729
VectorIndeterminate< Eigen::Dynamic > VectorXIndeterminate
VectorXIndeterminate is used as an alias for Eigen::Matrix<symbolic::Variable, Eigen::Dynamic, 1>.
Definition: indeterminate.h:43
Binding< VisualizationCallback > AddVisualizationCallback(const VisualizationCallback::CallbackFunction &callback, const VariableRefList &vars)
Adds a callback method to visualize intermediate results of the optimization.
Definition: mathematical_program.h:797
STL namespace.
const symbolic::Variable & decision_variable(int i) const
Getter for the decision variable with index i in the program.
Definition: mathematical_program.h:2574
Definition: mathematical_program.h:184
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
Definition: mathematical_program.h:194
Type
Supported types of symbolic variables.
Definition: symbolic_variable.h:30
Binding< RotatedLorentzConeConstraint > AddRotatedLorentzConeConstraint(const Eigen::MatrixBase< VectorDecisionVariable< rows >> &vars)
Impose that a vector is in rotated Lorentz cone.
Definition: mathematical_program.h:1900
MatrixDecisionVariable< rows, rows > NewSymmetricContinuousVariables(const std::string &name="Symmetric")
Adds a static sized symmetric matrix as decision variables to this MathematicalProgram.
Definition: mathematical_program.h:538
const std::map< std::string, std::string > & GetSolverOptionsStr(const SolverId &solver_id)
Definition: mathematical_program.h:2240
std::enable_if< is_eigen_scalar_same< Derived, symbolic::Formula >::value, Binding< LinearConstraint > >::type AddLinearConstraint(const Eigen::ArrayBase< Derived > &formulas)
Add a linear constraint represented by an Eigen::Array<symbolic::Formula> to the program.
Definition: mathematical_program.h:1299
Definition: mathematical_program.h:196
const std::vector< Binding< VisualizationCallback > > & visualization_callbacks() const
Getter for all callbacks.
Definition: mathematical_program.h:2272
void SetSolverOption(const SolverId &solver_id, const std::string &solver_option, int option_value)
Definition: mathematical_program.h:2219
Definition: mathematical_program.h:198
Binding< BoundingBoxConstraint > AddBoundingBoxConstraint(double lb, double ub, const symbolic::Variable &var)
Adds bounds for a single variable.
Definition: mathematical_program.h:1540
const VectorXDecisionVariable & decision_variables() const
Getter for all decision variables in the program.
Definition: mathematical_program.h:2569
std::enable_if< is_eigen_vector< DerivedX >::value, VectorX< typename DerivedX::Scalar > >::type EvalBinding(const Binding< C > &binding, const Eigen::MatrixBase< DerivedX > &prog_var_vals) const
Evaluates the value of some binding, for some input value for all decision variables.
Definition: mathematical_program.h:2498
Binding< Constraint > ParseConstraint(const Eigen::Ref< const VectorX< Expression >> &v, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)
Definition: create_constraint.cc:38
Eigen::VectorXd EvalBindingAtInitialGuess(const Binding< C > &binding) const
Evaluates the evaluator in binding at the initial guess.
Definition: mathematical_program.h:2564
Binding< QuadraticCost > AddL2NormCost(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost term of the form | Ax - b |^2.
Definition: mathematical_program.h:977
MatrixDecisionVariable< Rows, Cols > NewContinuousVariables(const std::string &name="X")
Adds continuous variables, appending them to an internal vector of any existing vars.
Definition: mathematical_program.h:429
Definition: mathematical_program.h:195
MatrixIndeterminate< rows, cols > NewIndeterminates(const std::string &name="X")
Adds indeterminates, appending them to an internal vector of any existing indeterminates.
Definition: mathematical_program.h:674
A binding on constraint type C is a mapping of the decision variables onto the inputs of C...
Definition: binding.h:18
Definition: mathematical_program.h:188
const Constraint * constraint
Definition: nlopt_solver.cc:104
const Eigen::VectorXd & initial_guess() const
Getter for the initial guess.
Definition: mathematical_program.h:2401
std::string to_string(const drake::geometry::Identifier< Tag > &id)
Enables use of identifiers with to_string.
Definition: identifier.h:211
std::vector< double > vector
Definition: translator_test.cc:20
Binding< LorentzConeConstraint > AddLorentzConeConstraint(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
Adds Lorentz cone constraint referencing potentially a subset of the decision variables (defined in t...
Definition: mathematical_program.h:1689
std::vector< snopt::doublereal > A
Definition: snopt_solver.cc:93
const std::vector< Binding< Cost > > & generic_costs() const
Getter for all generic costs.
Definition: mathematical_program.h:2280
stx::optional< T > optional
Definition: drake_optional.h:22
MatrixIndeterminate< rows, 1 > VectorIndeterminate
VectorIndeterminate<int> is used as an alias for Eigen::Matrix<symbolic::Variable, int, 1>.
Definition: indeterminate.h:29
const std::vector< Binding< LinearEqualityConstraint > > & linear_equality_constraints() const
Getter for linear equality constraints.
Definition: mathematical_program.h:2295
Binding< LinearConstraint > AddLinearConstraint(const Eigen::Ref< const Eigen::RowVectorXd > &a, double lb, double ub, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds one row of linear constraint referencing potentially a subset of the decision variables (defined...
Definition: mathematical_program.h:1212
uint32_t AttributesSet
Definition: mathematical_program.h:200
Enables us to catch and provide a meaningful assertion if a Constraint is passed in, when we should have a Cost.
Definition: create_cost.h:62
VectorIndeterminate< rows > NewIndeterminates(const std::string &name="x")
Adds indeterminates to the program.
Definition: mathematical_program.h:693
Binding< LinearEqualityConstraint > AddLinearEqualityConstraint(const Eigen::Ref< const Eigen::RowVectorXd > &a, double beq, const VariableRefList &vars)
Adds one row of linear equality constraint referencing potentially a subset of decision variables...
Definition: mathematical_program.h:1462
const std::vector< Binding< LinearCost > > & linear_costs() const
Getter for linear costs.
Definition: mathematical_program.h:2300
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:37
std::enable_if< Size==Eigen::Dynamic, typename NewVariableNames< Size >::type >::type CreateNewVariableNames(int size)
Return un-initialized new variable names.
Definition: mathematical_program.h:246
Definition: mathematical_program.h:216
Binding< LinearCost > AddLinearCost(const Eigen::Ref< const Eigen::VectorXd > &a, const VarType &vars)
Adds a linear cost term of the form a&#39;*x.
Definition: mathematical_program.h:923
const std::vector< Binding< Constraint > > & generic_constraints() const
Getter for all generic constraints.
Definition: mathematical_program.h:2287
const std::vector< Binding< RotatedLorentzConeConstraint > > & rotated_lorentz_cone_constraints() const
Getter for rotated Lorentz cone constraint.
Definition: mathematical_program.h:2322
Definition: mathematical_program.h:187
Binding< LinearEqualityConstraint > AddLinearEqualityConstraint(const Eigen::Ref< const Eigen::RowVectorXd > &a, double beq, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds one row of linear equality constraint referencing potentially a subset of decision variables...
Definition: mathematical_program.h:1479
void SetInitialGuessForAllVariables(const Eigen::MatrixBase< Derived > &x0)
Set the intial guess for ALL decision variables.
Definition: mathematical_program.h:2165
auto AddConstraint(std::shared_ptr< C > con, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a generic constraint to the program.
Definition: mathematical_program.h:1152
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
std::enable_if< is_eigen_nonvector_expression_double_pair< DerivedV, DerivedB >::value, Binding< LinearEqualityConstraint > >::type AddLinearEqualityConstraint(const Eigen::MatrixBase< DerivedV > &V, const Eigen::MatrixBase< DerivedB > &B, bool lower_triangle=false)
Adds a linear equality constraint for a matrix of linear expression V, such that V(i, j) = B(i, j).
Definition: mathematical_program.h:1393
Definition: mathematical_program.h:221
Binding< BoundingBoxConstraint > AddBoundingBoxConstraint(const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const VariableRefList &vars)
AddBoundingBoxConstraint.
Definition: mathematical_program.h:1514
std::vector< Binding< Cost > > GetAllCosts() const
Getter returning all costs (for now linear costs appended to generic costs, then quadratic costs appe...
Definition: mathematical_program.h:2343
Binding< LinearConstraint > AddLinearConstraint(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const VariableRefList &vars)
Adds linear constraints referencing potentially a subset of the decision variables (defined in the va...
Definition: mathematical_program.h:1168
Binding< LinearEqualityConstraint > AddLinearEqualityConstraint(const Eigen::Ref< const Eigen::MatrixXd > &Aeq, const Eigen::Ref< const Eigen::VectorXd > &beq, const VariableRefList &vars)
AddLinearEqualityConstraint.
Definition: mathematical_program.h:1420
MatrixIndeterminate< Eigen::Dynamic, Eigen::Dynamic > MatrixXIndeterminate
MatrixXIndeterminate is used as an alias for Eigen::Matrix<symbolic::Variable, Eigen::Dynamic, Eigen::Dynamic>.
Definition: indeterminate.h:37
SolutionResult
Definition: mathematical_program_solver_interface.h:16
Eigen::VectorXd EvalBindingAtSolution(const Binding< C > &binding) const
Evaluates the evaluator in binding at the solution value.
Definition: mathematical_program.h:2555
auto AddCost(const std::shared_ptr< C > &obj, const VariableRefList &vars)
Adds a generic cost to the optimization program.
Definition: mathematical_program.h:828
Eigen::Matrix< symbolic::Variable, rows, cols > MatrixIndeterminate
MatrixIndeterminate<int, int> is used as an alias for Eigen::Matrix<symbolic::Variable, int, int>.
Definition: indeterminate.h:21
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value, Eigen::Matrix< double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime > >::type GetSolution(const Eigen::MatrixBase< Derived > &var) const
Gets the solution of an Eigen matrix of decision variables.
Definition: mathematical_program.h:2446
std::array< std::string, Size > type
Definition: mathematical_program.h:212
const std::vector< Binding< LinearConstraint > > & linear_constraints() const
Getter for linear constraints.
Definition: mathematical_program.h:2310
Represents a symbolic form of an expression.
Definition: symbolic_expression.h:172
Do not use, to avoid & vs. && typos.
Definition: mathematical_program.h:185
void SetSolverOption(const SolverId &solver_id, const std::string &solver_option, double option_value)
Set an option for a particular solver.
Definition: mathematical_program.h:2214
ProgramAttributes
Definition: mathematical_program.h:183
VectorDecisionVariable< Eigen::Dynamic > VectorXDecisionVariable
Definition: decision_variable.h:17
std::vector< std::string > type
Definition: mathematical_program.h:217
const VectorXDecisionVariable * vars
Definition: nlopt_solver.cc:105
std::enable_if< detail::is_cost_functor_candidate< F >::value, Binding< Cost > >::type AddCost(F &&f, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost to the optimization program on an Eigen::Vector containing decision variables.
Definition: mathematical_program.h:864
Binding< Constraint > AddPolynomialConstraint(const VectorXPoly &polynomials, const std::vector< Polynomiald::VarType > &poly_vars, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const VariableRefList &vars)
Adds a polynomial constraint to the program referencing a subset of the decision variables (defined i...
Definition: mathematical_program.h:1940
VectorIndeterminate< rows > NewIndeterminates(const std::array< std::string, rows > &names)
Adds indeterminates, appending them to an internal vector of any existing indeterminates.
Definition: mathematical_program.h:648
void SetVariableNames(const std::string &name, int rows, int cols, Derived *names)
Set the names of the newly added variables.
Definition: mathematical_program.h:258
const std::map< std::string, double > & GetSolverOptionsDouble(const SolverId &solver_id)
Definition: mathematical_program.h:2230
const symbolic::Variable & indeterminate(int i) const
Getter for the indeterminate with index i in the program.
Definition: mathematical_program.h:2582
void SetSolverOption(const SolverId &solver_id, const std::string &solver_option, const std::string &option_value)
Definition: mathematical_program.h:2224
std::enable_if< detail::is_cost_functor_candidate< F >::value, Binding< Cost > >::type AddCost(F &&f, const VariableRefList &vars)
Adds a cost to the optimization program on a list of variables.
Definition: mathematical_program.h:851
Binding< QuadraticCost > AddQuadraticCost(const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
Adds a cost term of the form 0.5*x&#39;*Q*x + b&#39;x.
Definition: mathematical_program.h:988
std::list< Eigen::Ref< const VectorXDecisionVariable >> VariableRefList
Definition: decision_variable.h:19
std::shared_ptr< T > GetSolverData()
Definition: mathematical_program.h:2388
MatrixDecisionVariable< Rows, Cols > NewBinaryVariables(int rows, int cols, const std::string &name)
Adds binary variables, appending them to an internal vector of any existing vars. ...
Definition: mathematical_program.h:462
MatrixDecisionVariable< Rows, Cols > NewContinuousVariables(int rows, int cols, const std::string &name)
Adds continuous variables, appending them to an internal vector of any existing vars.
Definition: mathematical_program.h:392
const std::map< std::string, int > & GetSolverOptionsInt(const SolverId &solver_id)
Definition: mathematical_program.h:2235
optional< SolverId > GetSolverId() const
Returns the ID of the solver that was used to solve this program.
Definition: mathematical_program.h:2249
Binding< LinearMatrixInequalityConstraint > AddLinearMatrixInequalityConstraint(const std::vector< Eigen::Ref< const Eigen::MatrixXd >> &F, const VariableRefList &vars)
Adds a linear matrix inequality constraint to the program.
Definition: mathematical_program.h:2027
MatrixDecisionVariable< Rows, Cols > NewBinaryVariables(const std::string &name="b")
Adds a matrix of binary variables into the optimization program.
Definition: mathematical_program.h:482
auto AddConstraint(std::shared_ptr< C > con, const VariableRefList &vars)
Adds a generic constraint to the program.
Definition: mathematical_program.h:1141
MatrixDecisionVariable< rows, 1 > VectorDecisionVariable
Definition: decision_variable.h:14
Definition: mathematical_program.h:192
Definition: mathematical_program.h:197
double GetLowerBoundCost() const
Getter for lower bound on optimal cost.
Definition: mathematical_program.h:2267
MatrixDecisionVariable< Eigen::Dynamic, Eigen::Dynamic > MatrixXDecisionVariable
Definition: decision_variable.h:16
Definition: mathematical_program.h:225
Represents symbolic polynomials.
Definition: symbolic_polynomial.h:30
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value &&Derived::ColsAtCompileTime==1, Binding< BoundingBoxConstraint > >::type AddBoundingBoxConstraint(double lb, double ub, const Eigen::MatrixBase< Derived > &vars)
Adds the same scalar lower and upper bound to every variable in vars.
Definition: mathematical_program.h:1570
Eigen::Matrix< double, 1, 1 > Vector1d
A column vector of size 1 of doubles.
Definition: eigen_types.h:26
Represents a set of variables.
Definition: symbolic_variables.h:31
void EvalVisualizationCallbacks(const Eigen::Ref< const Eigen::VectorXd > &prog_var_vals) const
Evaluates all visualization callbacks registered with the MathematicalProgram.
Definition: mathematical_program.h:2524
SolverResult solver_result
Definition: gurobi_solver.cc:52
Definition: mathematical_program.h:190
Binding< QuadraticCost > AddL2NormCost(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
Adds a cost term of the form | Ax - b |^2.
Definition: mathematical_program.h:968
Template condition to only catch when Constraints are inadvertently passed as an argument.
Definition: mathematical_program.h:286
std::vector< std::string > names
Definition: translator_test.cc:19
Definition: mathematical_program.h:189
const std::vector< Binding< PositiveSemidefiniteConstraint > > & positive_semidefinite_constraints() const
Getter for positive semidefinite constraint.
Definition: mathematical_program.h:2328
This class is used by implementations of the class MathematicalProgramSolverInterface to report their...
Definition: mathematical_program_solver_interface.h:39
Definition: mathematical_program.h:186
Definition: symbolic_expression.h:941
VectorXDecisionVariable ConcatenateVariableRefList(const VariableRefList &var_list)
Concatenates each element in var_list into a single Eigen vector of decision variables, returns this concatenated vector.
Definition: decision_variable.cc:5
A constraint is a function + lower and upper bounds.
Definition: constraint.h:43
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value &&Derived::ColsAtCompileTime!=1, Binding< BoundingBoxConstraint > >::type AddBoundingBoxConstraint(double lb, double ub, const Eigen::MatrixBase< Derived > &vars)
Adds the same scalar lower and upper bound to every variable in vars.
Definition: mathematical_program.h:1592
void SetInitialGuess(const Eigen::MatrixBase< DerivedA > &decision_variable_mat, const Eigen::MatrixBase< DerivedB > &x0)
Sets the initial guess for the decision variables stored in decision_variable_mat to be x0...
Definition: mathematical_program.h:2147
std::enable_if< is_eigen_scalar_same< Derived, symbolic::Formula >::value, Binding< Constraint > >::type AddConstraint(const Eigen::ArrayBase< Derived > &formulas)
Definition: mathematical_program.h:1130
MathematicalProgram stores the decision variables, the constraints and costs of an optimization probl...
Definition: mathematical_program.h:301
const int kSize
Definition: diagram_test.cc:540
Binding< LinearCost > AddLinearCost(const Eigen::Ref< const Eigen::VectorXd > &a, double b, const VariableRefList &vars)
Adds a linear cost term of the form a&#39;*x + b.
Definition: mathematical_program.h:903
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value, Eigen::Matrix< double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime > >::type GetInitialGuess(const Eigen::MatrixBase< Derived > &decision_variable_mat) const
Gets the initial guess for some variables.
Definition: mathematical_program.h:2118
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
Binding< LinearComplementarityConstraint > AddLinearComplementarityConstraint(const Eigen::Ref< const Eigen::MatrixXd > &M, const Eigen::Ref< const Eigen::VectorXd > &q, const VariableRefList &vars)
Adds a linear complementarity constraints referencing a subset of the decision variables.
Definition: mathematical_program.h:1920
Binding< LorentzConeConstraint > AddLorentzConeConstraint(const Eigen::MatrixBase< VectorDecisionVariable< rows >> &vars)
Imposes that a vector lies in Lorentz cone.
Definition: mathematical_program.h:1746
const std::vector< Binding< LorentzConeConstraint > > & lorentz_cone_constraints() const
Getter for Lorentz cone constraint.
Definition: mathematical_program.h:2315
Binding< LinearConstraint > AddLinearConstraint(const Eigen::Ref< const Eigen::RowVectorXd > &a, double lb, double ub, const VariableRefList &vars)
Adds one row of linear constraint referencing potentially a subset of the decision variables (defined...
Definition: mathematical_program.h:1196
std::function< void(const Eigen::Ref< const Eigen::VectorXd > &)> CallbackFunction
Definition: evaluator_base.h:260
Identifies a MathematicalProgramSolverInterface implementation.
Definition: solver_id.h:17
MatrixIndeterminate< rows, cols > NewIndeterminates(const std::array< std::string, rows *cols > &names)
Adds indeterminates, appending them to an internal vector of any existing indeterminates.
Definition: mathematical_program.h:620
int num_vars() const
Getter for number of variables in the optimization program.
Definition: mathematical_program.h:2398
const VectorXIndeterminate & indeterminates() const
Getter for all indeterminates in the program.
Definition: mathematical_program.h:2579
const std::vector< Binding< BoundingBoxConstraint > > & bounding_box_constraints() const
Getter for all bounding box constraints.
Definition: mathematical_program.h:2363