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"
27 #include "drake/common/symbolic.h"
28 #include "drake/solvers/binding.h"
30 #include "drake/solvers/cost.h"
34 #include "drake/solvers/function.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,
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>
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>
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>
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<
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) const {
2232  // Aliases for brevity.
2233  const auto& options = solver_options_double_;
2234  const auto& empty = solver_options_double_empty_;
2235  const auto iter = options.find(solver_id);
2236  return (iter != options.end()) ? iter->second : empty;
2237  }
2238 
2239  const std::map<std::string, int>& GetSolverOptionsInt(
2240  const SolverId& solver_id) const {
2241  // Aliases for brevity.
2242  const auto& options = solver_options_int_;
2243  const auto& empty = solver_options_int_empty_;
2244  const auto iter = options.find(solver_id);
2245  return (iter != options.end()) ? iter->second : empty;
2246  }
2247 
2248  const std::map<std::string, std::string>& GetSolverOptionsStr(
2249  const SolverId& solver_id) const {
2250  // Aliases for brevity.
2251  const auto& options = solver_options_str_;
2252  const auto& empty = solver_options_str_empty_;
2253  const auto iter = options.find(solver_id);
2254  return (iter != options.end()) ? iter->second : empty;
2255  }
2256 
2257  /**
2258  * Returns the ID of the solver that was used to solve this program.
2259  * Returns empty if Solve() has not been called.
2260  */
2261  optional<SolverId> GetSolverId() const { return solver_id_; }
2262 
2263  /**
2264  * Getter for optimal cost at the solution.
2265  * If the solver finds an optimal solution, then we return the cost evaluated
2266  * at this solution.
2267  * If the program is unbounded, then the optimal cost is -∞.
2268  * If the program is globally infeasible, then the optimal cost is +∞.
2269  * If the program is locally infeasible, then the solver (e.g. SNOPT) might
2270  * return some finite value as the optimal cost.
2271  * Otherwise, the optimal cost is NaN.
2272  */
2273  double GetOptimalCost() const { return optimal_cost_; }
2274 
2275  /**
2276  * Getter for lower bound on optimal cost. Defaults to -Infinity
2277  * if a lower bound has not been found.
2278  */
2279  double GetLowerBoundCost() const { return lower_bound_cost_; }
2280 
2281  /**
2282  * Getter for all callbacks.
2283  */
2284  const std::vector<Binding<VisualizationCallback>>& visualization_callbacks()
2285  const {
2286  return visualization_callbacks_;
2287  }
2288 
2289  /**
2290  * Getter for all generic costs.
2291  */
2292  const std::vector<Binding<Cost>>& generic_costs() const {
2293  return generic_costs_;
2294  } // e.g. for snopt_user_fun
2295 
2296  /**
2297  * Getter for all generic constraints
2298  */
2299  const std::vector<Binding<Constraint>>& generic_constraints() const {
2300  return generic_constraints_;
2301  } // e.g. for snopt_user_fun
2302 
2303  /**
2304  * Getter for linear equality constraints.
2305  */
2306  const std::vector<Binding<LinearEqualityConstraint>>&
2308  return linear_equality_constraints_;
2309  }
2310 
2311  /** Getter for linear costs. */
2312  const std::vector<Binding<LinearCost>>& linear_costs() const {
2313  return linear_costs_;
2314  }
2315 
2316  /** Getter for quadratic costs. */
2317  const std::vector<Binding<QuadraticCost>>& quadratic_costs() const {
2318  return quadratic_costs_;
2319  }
2320 
2321  /** Getter for linear constraints. */
2322  const std::vector<Binding<LinearConstraint>>& linear_constraints() const {
2323  return linear_constraints_;
2324  }
2325 
2326  /** Getter for Lorentz cone constraint */
2327  const std::vector<Binding<LorentzConeConstraint>>& lorentz_cone_constraints()
2328  const {
2329  return lorentz_cone_constraint_;
2330  }
2331 
2332  /** Getter for rotated Lorentz cone constraint */
2333  const std::vector<Binding<RotatedLorentzConeConstraint>>&
2335  return rotated_lorentz_cone_constraint_;
2336  }
2337 
2338  /** Getter for positive semidefinite constraint */
2339  const std::vector<Binding<PositiveSemidefiniteConstraint>>&
2341  return positive_semidefinite_constraint_;
2342  }
2343 
2344  /** Getter for linear matrix inequality constraint */
2345  const std::vector<Binding<LinearMatrixInequalityConstraint>>&
2347  return linear_matrix_inequality_constraint_;
2348  }
2349 
2350  /**
2351  * Getter returning all costs (for now linear costs appended to
2352  * generic costs, then quadratic costs appended to
2353  * generic costs).
2354  */
2355  std::vector<Binding<Cost>> GetAllCosts() const {
2356  auto costlist = generic_costs_;
2357  costlist.insert(costlist.end(), linear_costs_.begin(), linear_costs_.end());
2358  costlist.insert(costlist.end(), quadratic_costs_.begin(),
2359  quadratic_costs_.end());
2360  return costlist;
2361  }
2362 
2363  /**
2364  * Getter returning all linear constraints (both linear equality and
2365  * inequality constraints).
2366  */
2367  std::vector<Binding<LinearConstraint>> GetAllLinearConstraints() const {
2368  std::vector<Binding<LinearConstraint>> conlist = linear_constraints_;
2369  conlist.insert(conlist.end(), linear_equality_constraints_.begin(),
2370  linear_equality_constraints_.end());
2371  return conlist;
2372  }
2373 
2374  /** Getter for all bounding box constraints */
2375  const std::vector<Binding<BoundingBoxConstraint>>& bounding_box_constraints()
2376  const {
2377  return bbox_constraints_;
2378  }
2379 
2380  /** Getter for all linear complementarity constraints.*/
2381  const std::vector<Binding<LinearComplementarityConstraint>>&
2383  return linear_complementarity_constraints_;
2384  }
2385 
2386  // Base class for solver-specific data. A solver implementation may derive
2387  // a helper class from this for use with getSolverData.
2388  struct SolverData {
2390  SolverData() = default;
2391  virtual ~SolverData() = default;
2392  };
2393 
2394  // Call from solver implementations to get a persistently-stored
2395  // helper structure of type T (derived from SolverData). If no
2396  // data of type T is already stored then a new one will be created
2397  // and stored, replacing data from any other solver in this problem
2398  // instance.
2399  template <typename T>
2400  std::shared_ptr<T> GetSolverData() {
2401  auto p = std::dynamic_pointer_cast<T>(solver_data_);
2402  if (!p) {
2403  p = std::make_shared<T>();
2404  solver_data_ = p;
2405  }
2406  return p;
2407  }
2408 
2409  /** Getter for number of variables in the optimization program */
2410  int num_vars() const { return decision_variables_.rows(); }
2411 
2412  /** Getter for the initial guess */
2413  const Eigen::VectorXd& initial_guess() const { return x_initial_guess_; }
2414 
2415  /** Returns the index of the decision variable. Internally the solvers thinks
2416  * all variables are stored in an array, and it acceses each individual
2417  * variable using its index. This index is used when adding constraints
2418  * and costs for each solver.
2419  * @pre{@p var is a decision variable in the mathematical program, otherwise
2420  * this function throws a runtime error.}
2421  */
2422  int FindDecisionVariableIndex(const symbolic::Variable& var) const;
2423 
2424  /**
2425  * Returns the indices of the decision variables. Internally the solvers
2426  * thinks all variables are stored in an array, and it acceses each individual
2427  * variable using its index. This index is used when adding constraints
2428  * and costs for each solver.
2429  * @pre{@p vars are decision variables in the mathematical program, otherwise
2430  * this function throws a runtime error.}
2431  */
2432  std::vector<int> FindDecisionVariableIndices(
2433  const Eigen::Ref<const VectorXDecisionVariable>& vars) const;
2434 
2435  /** Gets the number of indeterminates in the optimization program */
2436  int num_indeterminates() const { return indeterminates_.rows(); }
2437 
2438  /** Returns the index of the indeterminate. Internally a solver
2439  * thinks all indeterminates are stored in an array, and it acceses each
2440  * individual indeterminate using its index. This index is used when adding
2441  * constraints and costs for each solver.
2442  * @pre @p var is a indeterminate in the mathematical program,
2443  * otherwise this function throws a runtime error.
2444  */
2445  size_t FindIndeterminateIndex(const symbolic::Variable& var) const;
2446 
2447  /**
2448  * Gets the solution of an Eigen matrix of decision variables.
2449  * @tparam Derived An Eigen matrix containing Variable.
2450  * @param var The decision variables.
2451  * @return The value of the decision variable after solving the problem.
2452  */
2453  template <typename Derived>
2454  typename std::enable_if<
2455  std::is_same<typename Derived::Scalar, symbolic::Variable>::value,
2456  Eigen::Matrix<double, Derived::RowsAtCompileTime,
2457  Derived::ColsAtCompileTime>>::type
2458  GetSolution(const Eigen::MatrixBase<Derived>& var) const {
2459  Eigen::Matrix<double, Derived::RowsAtCompileTime,
2460  Derived::ColsAtCompileTime>
2461  value(var.rows(), var.cols());
2462  for (int i = 0; i < var.rows(); ++i) {
2463  for (int j = 0; j < var.cols(); ++j) {
2464  value(i, j) = GetSolution(var(i, j));
2465  }
2466  }
2467  return value;
2468  }
2469 
2470  /**
2471  * Gets the value of a single decision variable.
2472  */
2473  double GetSolution(const symbolic::Variable& var) const;
2474 
2475  /**
2476  * Replaces the variables in an expression with the solutions to the
2477  * variables, returns the expression after substitution.
2478  * @throw runtime error if some variables in the expression @p e are NOT
2479  * decision variables or indeterminates in the optimization program.
2480  * @note If the expression @p e 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::Expression SubstituteSolution(const symbolic::Expression& e) const;
2486 
2487  /**
2488  * Replaces the decision variables in a polynomial with the solutions to the
2489  * variables, returns the polynomial after substitution.
2490  * @throw runtime error if some decision variables in the polynomial @p p are
2491  * NOT decision variables in the optimization program.
2492  * @note If the polynomial @p p contains both decision variables and
2493  * indeterminates of the optimization program, then the decision variables
2494  * will be substituted by its solutions in double values, but not the
2495  * indeterminates.
2496  */
2497  symbolic::Polynomial SubstituteSolution(const symbolic::Polynomial& p) const;
2498 
2499  /**
2500  * Evaluates the value of some binding, for some input value for all
2501  * decision variables.
2502  * @param binding A Binding whose variables are decision variables in this
2503  * program.
2504  * @param prog_var_vals The value of all the decision variables in this
2505  * program. @throw a logic error if the size does not match.
2506  */
2507  template <typename C, typename DerivedX>
2510  EvalBinding(const Binding<C>& binding,
2511  const Eigen::MatrixBase<DerivedX>& prog_var_vals) const {
2512  using Scalar = typename DerivedX::Scalar;
2513  if (prog_var_vals.rows() != num_vars()) {
2514  std::ostringstream oss;
2515  oss << "The input binding variable is not in the right size. Expects "
2516  << num_vars() << " rows, but it actually has " << prog_var_vals.rows()
2517  << " rows.\n";
2518  throw std::logic_error(oss.str());
2519  }
2520  VectorX<Scalar> binding_x(binding.GetNumElements());
2521  VectorX<Scalar> binding_y(binding.evaluator()->num_outputs());
2522  for (int i = 0; i < static_cast<int>(binding.GetNumElements()); ++i) {
2523  binding_x(i) =
2524  prog_var_vals(FindDecisionVariableIndex(binding.variables()(i)));
2525  }
2526  binding.evaluator()->Eval(binding_x, &binding_y);
2527  return binding_y;
2528  }
2529 
2530  /** Evaluates all visualization callbacks registered with the
2531  * MathematicalProgram.
2532  *
2533  * @param prog_var_vals The value of all the decision variables in this
2534  * program. @throw a logic error if the size does not match.
2535  **/
2537  const Eigen::Ref<const Eigen::VectorXd>& prog_var_vals) const {
2538  if (prog_var_vals.rows() != num_vars()) {
2539  std::ostringstream oss;
2540  oss << "The input binding variable is not in the right size. Expects "
2541  << num_vars() << " rows, but it actually has " << prog_var_vals.rows()
2542  << " rows.\n";
2543  throw std::logic_error(oss.str());
2544  }
2545 
2546  Eigen::VectorXd this_x;
2547 
2548  for (auto const& binding : visualization_callbacks_) {
2549  auto const& obj = binding.evaluator();
2550 
2551  const int num_v_variables = binding.GetNumElements();
2552  this_x.resize(num_v_variables);
2553  for (int j = 0; j < num_v_variables; ++j) {
2554  this_x(j) =
2555  prog_var_vals(FindDecisionVariableIndex(binding.variables()(j)));
2556  }
2557 
2558  obj->EvalCallback(this_x);
2559  }
2560  }
2561 
2562  /**
2563  * Evaluates the evaluator in @p binding at the solution value.
2564  * @return The value of @p binding at the solution value.
2565  */
2566  template <typename C>
2567  Eigen::VectorXd EvalBindingAtSolution(const Binding<C>& binding) const {
2568  return EvalBinding(binding, x_values_);
2569  }
2570 
2571  /**
2572  * Evaluates the evaluator in @p binding at the initial guess.
2573  * @return The value of @p binding at the initial guess.
2574  */
2575  template <typename C>
2576  Eigen::VectorXd EvalBindingAtInitialGuess(const Binding<C>& binding) const {
2577  return EvalBinding(binding, x_initial_guess_);
2578  }
2579 
2580  /** Getter for all decision variables in the program. */
2582  return decision_variables_;
2583  }
2584 
2585  /** Getter for the decision variable with index @p i in the program. */
2587  return decision_variables_(i);
2588  }
2589 
2590  /** Getter for all indeterminates in the program. */
2592 
2593  /** Getter for the indeterminate with index @p i in the program. */
2594  const symbolic::Variable& indeterminate(int i) const {
2595  return indeterminates_(i);
2596  }
2597 
2598  /**
2599  * Solver reports its result back to MathematicalProgram, by passing the
2600  * solver_result, which contains the solver result.
2601  * @note This method should only be called by each solver, after it solves the
2602  * optimization problem stored in MathematicalProgram. The user should NOT
2603  * call this method.
2604  */
2605  // This method should be called by the derived classes of
2606  // MathematicalProgramSolverInterface, which is not a friend class of
2607  // MathematicalProgram, as we do not want to leak any of the internal details
2608  // of MathematicalProgram.
2609  void SetSolverResult(const SolverResult& solver_result);
2610 
2611  private:
2612  static void AppendNanToEnd(int new_var_size, Eigen::VectorXd* vector);
2613  // maps the ID of a symbolic variable to the index of the variable stored in
2614  // the optimization program.
2615  std::unordered_map<symbolic::Variable::Id, int> decision_variable_index_{};
2616 
2617  VectorXDecisionVariable decision_variables_;
2618 
2619  std::unordered_map<symbolic::Variable::Id, int> indeterminates_index_;
2621 
2622  std::vector<Binding<VisualizationCallback>> visualization_callbacks_;
2623 
2624  std::vector<Binding<Cost>> generic_costs_;
2625  std::vector<Binding<QuadraticCost>> quadratic_costs_;
2626  std::vector<Binding<LinearCost>> linear_costs_;
2627  // TODO(naveenoid) : quadratic_constraints_
2628 
2629  // note: linear_constraints_ does not include linear_equality_constraints_
2630  std::vector<Binding<Constraint>> generic_constraints_;
2631  std::vector<Binding<LinearConstraint>> linear_constraints_;
2632  std::vector<Binding<LinearEqualityConstraint>> linear_equality_constraints_;
2633  std::vector<Binding<BoundingBoxConstraint>> bbox_constraints_;
2634  std::vector<Binding<LorentzConeConstraint>> lorentz_cone_constraint_;
2635  std::vector<Binding<RotatedLorentzConeConstraint>>
2636  rotated_lorentz_cone_constraint_;
2637  std::vector<Binding<PositiveSemidefiniteConstraint>>
2638  positive_semidefinite_constraint_;
2639  std::vector<Binding<LinearMatrixInequalityConstraint>>
2640  linear_matrix_inequality_constraint_;
2641 
2642  // Invariant: The bindings in this list must be non-overlapping, when calling
2643  // Linear Complementarity solver like Moby. If this constraint is solved
2644  // through a nonlinear optimization solver (like SNOPT) instead, then we allow
2645  // the bindings to be overlapping.
2646  // TODO(ggould-tri) can this constraint be relaxed?
2647  std::vector<Binding<LinearComplementarityConstraint>>
2648  linear_complementarity_constraints_;
2649 
2650  Eigen::VectorXd x_initial_guess_;
2651  Eigen::VectorXd x_values_;
2652  std::shared_ptr<SolverData> solver_data_;
2653  optional<SolverId> solver_id_;
2654  double optimal_cost_{};
2655  // The lower bound of the objective found by the solver, during the
2656  // optimization process.
2657  double lower_bound_cost_{};
2658 
2659  // The actual per-solver customization options.
2660  std::map<SolverId, std::map<std::string, double>> solver_options_double_;
2661  std::map<SolverId, std::map<std::string, int>> solver_options_int_;
2662  std::map<SolverId, std::map<std::string, std::string>> solver_options_str_;
2663  // Dummy (empty) options, for when the solver_id is not in the above maps.
2664  const std::map<std::string, double> solver_options_double_empty_;
2665  const std::map<std::string, int> solver_options_int_empty_;
2666  const std::map<std::string, std::string> solver_options_str_empty_;
2667 
2668  AttributesSet required_capabilities_{0};
2669 
2670  std::unique_ptr<MathematicalProgramSolverInterface> ipopt_solver_;
2671  std::unique_ptr<MathematicalProgramSolverInterface> nlopt_solver_;
2672  std::unique_ptr<MathematicalProgramSolverInterface> snopt_solver_;
2673  std::unique_ptr<MathematicalProgramSolverInterface> moby_lcp_solver_;
2674  std::unique_ptr<MathematicalProgramSolverInterface> linear_system_solver_;
2675  std::unique_ptr<MathematicalProgramSolverInterface>
2676  equality_constrained_qp_solver_;
2677  std::unique_ptr<MathematicalProgramSolverInterface> gurobi_solver_;
2678  std::unique_ptr<MathematicalProgramSolverInterface> mosek_solver_;
2679  std::unique_ptr<MathematicalProgramSolverInterface> osqp_solver_;
2680  std::unique_ptr<MathematicalProgramSolverInterface> scs_solver_;
2681 
2682  template <typename T>
2683  void NewVariables_impl(
2684  VarType type, const T& names, bool is_symmetric,
2685  Eigen::Ref<MatrixXDecisionVariable> decision_variable_matrix) {
2686  switch (type) {
2687  case VarType::CONTINUOUS:
2688  break;
2689  case VarType::BINARY:
2690  required_capabilities_ |= kBinaryVariable;
2691  break;
2692  case VarType::INTEGER:
2693  throw std::runtime_error(
2694  "MathematicalProgram does not support integer variables yet.");
2695  case VarType::BOOLEAN:
2696  throw std::runtime_error(
2697  "MathematicalProgram does not support Boolean variables.");
2698  }
2699  int rows = decision_variable_matrix.rows();
2700  int cols = decision_variable_matrix.cols();
2701  DRAKE_ASSERT(!is_symmetric || rows == cols);
2702  int num_new_vars = 0;
2703  if (!is_symmetric) {
2704  num_new_vars = rows * cols;
2705  } else {
2706  num_new_vars = rows * (rows + 1) / 2;
2707  }
2708  DRAKE_ASSERT(static_cast<int>(names.size()) == num_new_vars);
2709  decision_variables_.conservativeResize(num_vars() + num_new_vars,
2710  Eigen::NoChange);
2711  AppendNanToEnd(num_new_vars, &x_values_);
2712  int row_index = 0;
2713  int col_index = 0;
2714  for (int i = 0; i < num_new_vars; ++i) {
2715  decision_variables_(num_vars() - num_new_vars + i) =
2716  symbolic::Variable(names[i], type);
2717  const int new_var_index = num_vars() - num_new_vars + i;
2718  decision_variable_index_.insert(std::pair<int, int>(
2719  decision_variables_(new_var_index).get_id(), new_var_index));
2720  decision_variable_matrix(row_index, col_index) =
2721  decision_variables_(num_vars() - num_new_vars + i);
2722  // If the matrix is not symmetric, then store the variable in column
2723  // major.
2724  if (!is_symmetric) {
2725  if (row_index + 1 < rows) {
2726  ++row_index;
2727  } else {
2728  ++col_index;
2729  row_index = 0;
2730  }
2731  } else {
2732  // If the matrix is symmetric, then the decision variables are the lower
2733  // triangular part of the symmetric matrix, also stored in column major.
2734  if (row_index != col_index) {
2735  decision_variable_matrix(col_index, row_index) =
2736  decision_variable_matrix(row_index, col_index);
2737  }
2738  if (row_index + 1 < rows) {
2739  ++row_index;
2740  } else {
2741  ++col_index;
2742  row_index = col_index;
2743  }
2744  }
2745  }
2746 
2747  AppendNanToEnd(num_new_vars, &x_initial_guess_);
2748  }
2749 
2750  MatrixXDecisionVariable NewVariables(VarType type, int rows, int cols,
2751  bool is_symmetric,
2752  const std::vector<std::string>& names);
2753 
2754  template <typename T>
2755  void NewIndeterminates_impl(
2756  const T& names, Eigen::Ref<MatrixXIndeterminate> indeterminates_matrix) {
2757  int rows = indeterminates_matrix.rows();
2758  int cols = indeterminates_matrix.cols();
2759  int num_new_vars = rows * cols;
2760 
2761  DRAKE_ASSERT(static_cast<int>(names.size()) == num_new_vars);
2762  indeterminates_.conservativeResize(indeterminates_.rows() + num_new_vars,
2763  Eigen::NoChange);
2764  int row_index = 0;
2765  int col_index = 0;
2766  for (int i = 0; i < num_new_vars; ++i) {
2767  indeterminates_(indeterminates_.rows() - num_new_vars + i) =
2768  symbolic::Variable(names[i]);
2769 
2770  const int new_var_index = indeterminates_.rows() - num_new_vars + i;
2771  indeterminates_index_.insert(std::pair<size_t, size_t>(
2772  indeterminates_(new_var_index).get_id(), new_var_index));
2773  indeterminates_matrix(row_index, col_index) =
2774  indeterminates_(indeterminates_.rows() - num_new_vars + i);
2775 
2776  // store the indeterminate in column major.
2777  if (row_index + 1 < rows) {
2778  ++row_index;
2779  } else {
2780  ++col_index;
2781  row_index = 0;
2782  }
2783  }
2784  }
2785 
2786  /*
2787  * Given a matrix of decision variables, checks if every entry in the
2788  * matrix is a decision variable in the program; throws a runtime
2789  * error if any variable is not a decision variable in the program.
2790  * @tparam Derived An Eigen::Matrix type of symbolic Variable.
2791  * @param vars A matrix of variables.
2792  */
2793  template <typename Derived>
2794  typename std::enable_if<
2795  std::is_same<typename Derived::Scalar, symbolic::Variable>::value>::type
2796  CheckIsDecisionVariable(const Eigen::MatrixBase<Derived>& vars) const {
2797  for (int i = 0; i < vars.rows(); ++i) {
2798  for (int j = 0; j < vars.cols(); ++j) {
2799  if (decision_variable_index_.find(vars(i, j).get_id()) ==
2800  decision_variable_index_.end()) {
2801  std::ostringstream oss;
2802  oss << vars(i, j)
2803  << " is not a decision variable of the mathematical program.\n";
2804  throw std::runtime_error(oss.str());
2805  }
2806  }
2807  }
2808  }
2809 
2810  /*
2811  * Ensure a binding is valid *before* adding it to the program.
2812  * @pre The binding has not yet been registered.
2813  * @pre The decision variables have been registered.
2814  * @throws std::runtime_error if the binding is invalid.
2815  */
2816  template <typename C>
2817  void CheckBinding(const Binding<C>& binding) const {
2818  // TODO(eric.cousineau): In addition to identifiers, hash bindings by
2819  // their constraints and their variables, to prevent duplicates.
2820  // TODO(eric.cousineau): Once bindings have identifiers (perhaps
2821  // retrofitting `description`), ensure that they have unique names.
2822  CheckIsDecisionVariable(binding.variables());
2823  }
2824 
2825  // Adds a constraint represented by a set of symbolic formulas to the
2826  // program.
2827  //
2828  // Precondition: ∀ f ∈ formulas, is_relational(f).
2829  Binding<Constraint> AddConstraint(
2830  const std::set<symbolic::Formula>& formulas);
2831 
2832  // Adds a linear-equality constraint represented by a set of symbolic formulas
2833  // to the program.
2834  //
2835  // Precondition: ∀ f ∈ formulas, is_equal_to(f).
2836  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
2837  const std::set<symbolic::Formula>& formulas);
2838 
2839  /**
2840  * Adds new variables to MathematicalProgram.
2841  */
2842  template <int Rows, int Cols>
2844  VarType type, const typename NewVariableNames<Rows, Cols>::type& names,
2845  int rows, int cols) {
2846  DRAKE_DEMAND(rows >= 0 && cols >= 0);
2847  MatrixDecisionVariable<Rows, Cols> decision_variable_matrix;
2848  decision_variable_matrix.resize(rows, cols);
2849  NewVariables_impl(type, names, false, decision_variable_matrix);
2850  return decision_variable_matrix;
2851  }
2852 
2853  /**
2854  * Adds symmetric matrix variables to optimization program. Only the lower
2855  * triangular part of the matrix is used as decision variables.
2856  * @param names The names of the stacked columns of the lower triangular part
2857  * of the matrix.
2858  */
2859  template <int Rows>
2860  MatrixDecisionVariable<Rows, Rows> NewSymmetricVariables(
2861  VarType type, const typename NewSymmetricVariableNames<Rows>::type& names,
2862  int rows = Rows) {
2863  MatrixDecisionVariable<Rows, Rows> decision_variable_matrix(rows, rows);
2864  NewVariables_impl(type, names, true, decision_variable_matrix);
2865  return decision_variable_matrix;
2866  }
2867 };
2868 
2869 } // namespace solvers
2870 } // 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:2317
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:64
Definition: mathematical_program.h:2388
const std::vector< Binding< LinearMatrixInequalityConstraint > > & linear_matrix_inequality_constraints() const
Getter for linear matrix inequality constraint.
Definition: mathematical_program.h:2346
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:2367
Represents a symbolic variable.
Definition: symbolic_variable.h:24
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
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:983
double value
Definition: wrap_test_util_py.cc:12
const Variables indeterminates_
Definition: symbolic_polynomial_test.cc:26
Provides a portable macro for use in generating compile-time warnings for use of code that is permitt...
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:2273
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:2436
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:2382
std::shared_ptr< Cost > MakeFunctionCost(FF &&f)
Converts an input of type F to a nonlinear cost.
Definition: cost.h:257
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:2586
std::unique_ptr< T > dynamic_pointer_cast(std::unique_ptr< U > &&other) noexcept
Casts the object owned by the std::unique_ptr other from type U to T; if the cast fails...
Definition: pointer_cast.h:38
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
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:2284
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:2581
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:2510
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:2576
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:2413
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:2292
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:2307
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:2312
#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:2299
const std::vector< Binding< RotatedLorentzConeConstraint > > & rotated_lorentz_cone_constraints() const
Getter for rotated Lorentz cone constraint.
Definition: mathematical_program.h:2334
Definition: mathematical_program.h:187
Provides Drake&#39;s assertion implementation.
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:2355
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
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
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:2567
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:2458
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:2322
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 symbolic::Variable & indeterminate(int i) const
Getter for the indeterminate with index i in the program.
Definition: mathematical_program.h:2594
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:2400
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
optional< SolverId > GetSolverId() const
Returns the ID of the solver that was used to solve this program.
Definition: mathematical_program.h:2261
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
const std::map< std::string, double > & GetSolverOptionsDouble(const SolverId &solver_id) const
Definition: mathematical_program.h:2230
double GetLowerBoundCost() const
Getter for lower bound on optimal cost.
Definition: mathematical_program.h:2279
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:2536
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:2340
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:973
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
Provides public header files of Drake&#39;s symbolic library.
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
ReferenceType type
Definition: loader.cc:34
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:536
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::map< std::string, std::string > & GetSolverOptionsStr(const SolverId &solver_id) const
Definition: mathematical_program.h:2248
const std::vector< Binding< LorentzConeConstraint > > & lorentz_cone_constraints() const
Getter for Lorentz cone constraint.
Definition: mathematical_program.h:2327
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:289
Identifies a MathematicalProgramSolverInterface implementation.
Definition: solver_id.h:17
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
const std::map< std::string, int > & GetSolverOptionsInt(const SolverId &solver_id) const
Definition: mathematical_program.h:2239
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:2410
const VectorXIndeterminate & indeterminates() const
Getter for all indeterminates in the program.
Definition: mathematical_program.h:2591
const std::vector< Binding< BoundingBoxConstraint > > & bounding_box_constraints() const
Getter for all bounding box constraints.
Definition: mathematical_program.h:2375