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