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