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