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 
27 #include "drake/common/symbolic.h"
28 #include "drake/solvers/binding.h"
30 #include "drake/solvers/cost.h"
34 #include "drake/solvers/function.h"
37 
38 namespace drake {
39 namespace solvers {
40 
172 class MathematicalProgram;
173 
176  kError = 1 << 0,
177  kGenericCost = 1 << 1,
179  kQuadraticCost = 1 << 3,
181  kLinearCost = 1 << 5,
188  kBinaryVariable = 1 << 12
189 };
190 typedef uint32_t AttributesSet;
191 
192 template<int ...>
200 template <int Size>
202  typedef std::array<std::string, Size> type;
203 };
204 
205 template <>
207  typedef std::vector<std::string> type;
208 };
209 
210 template <int Rows, int Cols>
213 
214 template <int Rows>
216  : public NewVariableNames<Rows == Eigen::Dynamic ? Eigen::Dynamic
217  : Rows*(Rows + 1) / 2> {};
218 
219 namespace internal {
223 template <int Size>
224 typename std::enable_if< Size >= 0, typename NewVariableNames<Size>::type>::type
227  return names;
228 }
229 
233 template <int Size>
234 typename std::enable_if<Size == Eigen::Dynamic,
235  typename NewVariableNames<Size>::type>::type
238  return names;
239 }
247 template <typename Derived>
248 void SetVariableNames(const std::string& name, int rows, int cols,
249  Derived* names) {
250  DRAKE_DEMAND(static_cast<int>(names->size()) == rows * cols);
251  if (cols == 1) {
252  for (int i = 0; i < rows; ++i) {
253  (*names)[i] = name + "(" + std::to_string(i) + ")";
254  }
255  } else {
256  for (int j = 0; j < cols; ++j) {
257  for (int i = 0; i < rows; ++i) {
258  (*names)[j * rows + i] =
259  name + "(" + std::to_string(i) + "," + std::to_string(j) + ")";
260  }
261  }
262  }
263 }
264 } // namespace internal
265 
266 namespace detail {
275 template <typename F>
278  // Use deferred evaluation
279  static_assert(
280  !value,
281  "You cannot pass a Constraint to create a Cost object from a function. "
282  "Please ensure you are passing a Cost.");
283 };
284 } // namespace detail
285 
287  public:
289 
290  using VarType = symbolic::Variable::Type;
291 
293  virtual ~MathematicalProgram() {}
294 
321  int rows, const std::string& name = "x") {
322  return NewContinuousVariables<Eigen::Dynamic, 1>(rows, 1, name);
323  }
324 
325 
357  template <int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
359  NewContinuousVariables(int rows, int cols, const std::string& name) {
360  rows = Rows == Eigen::Dynamic? rows : Rows;
361  cols = Cols == Eigen::Dynamic? cols : Cols;
362  auto names =
364  rows * cols);
365  internal::SetVariableNames(name, rows, cols, &names);
366  return NewVariables<Rows, Cols>(VarType::CONTINUOUS, names, rows, cols);
367  }
368 
394  template <int Rows, int Cols = 1>
396  const std::string& name = "X") {
397  return NewContinuousVariables<Rows, Cols>(Rows, Cols, name);
398  }
399 
427  template <int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
429  NewBinaryVariables(int rows, int cols, const std::string& name) {
430  rows = Rows == Eigen::Dynamic ? rows : Rows;
431  cols = Cols == Eigen::Dynamic ? cols : Cols;
432  auto names =
434  rows * cols);
435  internal::SetVariableNames(name, rows, cols, &names);
436  return NewVariables<Rows, Cols>(VarType::BINARY, names, rows, cols);
437  }
438 
447  template <int Rows, int Cols = 1>
449  const std::string& name = "b") {
450  return NewBinaryVariables<Rows, Cols>(Rows, Cols, name);
451  }
452 
460  const std::string& name = "b") {
461  return NewBinaryVariables<Eigen::Dynamic, 1>(rows, 1, name);
462  }
463 
482  MatrixXDecisionVariable NewSymmetricContinuousVariables(
483  int rows, const std::string& name = "Symmetric");
484 
503  template <int rows>
505  const std::string& name = "Symmetric") {
506  std::array<std::string, rows*(rows + 1) / 2> names;
507  int var_count = 0;
508  for (int j = 0; j < static_cast<int>(rows); ++j) {
509  for (int i = j; i < static_cast<int>(rows); ++i) {
510  names[var_count] =
511  name + "(" + std::to_string(i) + "," + std::to_string(j) + ")";
512  ++var_count;
513  }
514  }
515  return NewSymmetricVariables<rows>(VarType::CONTINUOUS, names);
516  }
517 
524  symbolic::Polynomial NewFreePolynomial(
525  const symbolic::Variables& indeterminates, int degree,
526  const std::string& coeff_name = "a");
527 
537  std::pair<symbolic::Polynomial, Binding<PositiveSemidefiniteConstraint>>
538  NewSosPolynomial(const symbolic::Variables& indeterminates, int degree);
539 
560  template <int rows, int cols>
562  const std::array<std::string, rows * cols>& names) {
563  MatrixIndeterminate<rows, cols> indeterminates_matrix;
564  NewIndeterminates_impl(names, indeterminates_matrix);
565  return indeterminates_matrix;
566  }
567 
588  template <int rows>
590  const std::array<std::string, rows>& names) {
591  return NewIndeterminates<rows, 1>(names);
592  }
593 
614  template <int rows, int cols>
616  const std::string& name = "X") {
617  std::array<std::string, rows * cols> names;
618  for (int j = 0; j < cols; ++j) {
619  for (int i = 0; i < rows; ++i) {
620  names[j * rows + i] =
621  name + "(" + std::to_string(i) + "," + std::to_string(j) + ")";
622  }
623  }
624  return NewIndeterminates<rows, cols>(names);
625  }
626 
633  template <int rows>
634  VectorIndeterminate<rows> NewIndeterminates(const std::string& name = "x") {
635  std::array<std::string, rows> names;
636  int offset = (name.compare("x") == 0) ? num_vars() : 0;
637  for (int i = 0; i < rows; ++i) {
638  names[i] = name + "(" + std::to_string(offset + i) + ")";
639  }
640  return NewIndeterminates<rows>(names);
641  }
642 
648  VectorXIndeterminate NewIndeterminates(int rows,
649  const std::vector<std::string>& names);
650 
657  VectorXIndeterminate NewIndeterminates(int rows,
658  const std::string& name = "x");
659 
680  MatrixXIndeterminate NewIndeterminates(int rows, int cols,
681  const std::vector<std::string>& names);
682 
690  MatrixXIndeterminate NewIndeterminates(int rows, int cols,
691  const std::string& name = "X");
692 
696  Binding<Cost> AddCost(const Binding<Cost>& binding);
697 
703  template <typename C>
704  auto AddCost(const std::shared_ptr<C>& obj,
705  const Eigen::Ref<const VectorXDecisionVariable>& vars) {
706  // Redirect to the appropriate type
707  // Use auto to enable the overloading method to upcast if needed
708  return AddCost(internal::CreateBinding(obj, vars));
709  }
710 
716  template <typename C>
717  auto AddCost(const std::shared_ptr<C>& obj, const VariableRefList& vars) {
718  return AddCost(obj, ConcatenateVariableRefList(vars));
719  }
720 
727  template <typename F>
728  static std::shared_ptr<Cost> MakeCost(F&& f) {
729  return MakeFunctionCost(f);
730  }
731 
737  template <typename F>
739  Binding<Cost>>::type
740  AddCost(F&& f, const VariableRefList& vars) {
741  return AddCost(f, ConcatenateVariableRefList(vars));
742  }
743 
750  template <typename F>
752  Binding<Cost>>::type
753  AddCost(F&& f, const Eigen::Ref<const VectorXDecisionVariable>& vars) {
754  auto c = MakeFunctionCost(std::forward<F>(f));
755  return AddCost(c, vars);
756  }
757 
763  template <typename F, typename Vars>
765  Binding<Cost>>::type
766  AddCost(F&&, Vars&&) {
767  throw std::runtime_error("This will assert at compile-time.");
768  }
769 
775  Binding<LinearCost> AddCost(const Binding<LinearCost>& binding);
776 
785  Binding<LinearCost> AddLinearCost(const symbolic::Expression& e);
786 
792  Binding<LinearCost> AddLinearCost(const Eigen::Ref<const Eigen::VectorXd>& a,
793  double b, const VariableRefList& vars) {
794  return AddLinearCost(a, b, ConcatenateVariableRefList((vars)));
795  }
796 
802  Binding<LinearCost> AddLinearCost(
803  const Eigen::Ref<const Eigen::VectorXd>& a, double b,
804  const Eigen::Ref<const VectorXDecisionVariable>& vars);
805 
811  template <typename VarType>
812  Binding<LinearCost> AddLinearCost(const Eigen::Ref<const Eigen::VectorXd>& a,
813  const VarType& vars) {
814  const double b = 0.;
815  return AddLinearCost(a, b, vars);
816  }
817 
823  Binding<QuadraticCost> AddCost(const Binding<QuadraticCost>& binding);
824 
833  Binding<QuadraticCost> AddQuadraticCost(const symbolic::Expression& e);
834 
839  const Eigen::Ref<const Eigen::MatrixXd>& Q,
840  const Eigen::Ref<const Eigen::VectorXd>& x_desired,
841  const VariableRefList& vars) {
842  return AddQuadraticErrorCost(Q, x_desired,
844  }
845 
849  Binding<QuadraticCost> AddQuadraticErrorCost(
850  const Eigen::Ref<const Eigen::MatrixXd>& Q,
851  const Eigen::Ref<const Eigen::VectorXd>& x_desired,
852  const Eigen::Ref<const VectorXDecisionVariable>& vars);
853 
858  const Eigen::Ref<const Eigen::MatrixXd>& A,
859  const Eigen::Ref<const Eigen::VectorXd>& b, const VariableRefList& vars) {
860  return AddL2NormCost(A, b, ConcatenateVariableRefList(vars));
861  }
862 
867  const Eigen::Ref<const Eigen::MatrixXd>& A,
868  const Eigen::Ref<const Eigen::VectorXd>& b,
869  const Eigen::Ref<const VectorXDecisionVariable>& vars) {
870  return AddCost(MakeL2NormCost(A, b), vars);
871  }
872 
878  const Eigen::Ref<const Eigen::MatrixXd>& Q,
879  const Eigen::Ref<const Eigen::VectorXd>& b, const VariableRefList& vars) {
880  return AddQuadraticCost(Q, b, ConcatenateVariableRefList(vars));
881  }
882 
887  Binding<QuadraticCost> AddQuadraticCost(
888  const Eigen::Ref<const Eigen::MatrixXd>& Q,
889  const Eigen::Ref<const Eigen::VectorXd>& b, double c,
890  const Eigen::Ref<const VectorXDecisionVariable>& vars);
891 
896  Binding<QuadraticCost> AddQuadraticCost(
897  const Eigen::Ref<const Eigen::MatrixXd>& Q,
898  const Eigen::Ref<const Eigen::VectorXd>& b,
899  const Eigen::Ref<const VectorXDecisionVariable>& vars);
900 
906  Binding<PolynomialCost> AddPolynomialCost(const symbolic::Expression& e);
907 
917  Binding<Cost> AddCost(const symbolic::Expression& e);
918 
925  Binding<Constraint> AddConstraint(const Binding<Constraint>& binding);
926 
933  template <typename C>
934  auto AddConstraint(std::shared_ptr<C> con, const VariableRefList& vars) {
935  return AddConstraint(con, ConcatenateVariableRefList(vars));
936  }
937 
944  template <typename C>
945  auto AddConstraint(std::shared_ptr<C> con,
946  const Eigen::Ref<const VectorXDecisionVariable>& vars) {
947  return AddConstraint(internal::CreateBinding(con, vars));
948  }
949 
954  Binding<LinearConstraint> AddConstraint(
955  const Binding<LinearConstraint>& binding);
956 
962  const Eigen::Ref<const Eigen::MatrixXd>& A,
963  const Eigen::Ref<const Eigen::VectorXd>& lb,
964  const Eigen::Ref<const Eigen::VectorXd>& ub,
965  const VariableRefList& vars) {
966  return AddLinearConstraint(A, lb, ub, ConcatenateVariableRefList(vars));
967  }
968 
973  Binding<LinearConstraint> AddLinearConstraint(
974  const Eigen::Ref<const Eigen::MatrixXd>& A,
975  const Eigen::Ref<const Eigen::VectorXd>& lb,
976  const Eigen::Ref<const Eigen::VectorXd>& ub,
977  const Eigen::Ref<const VectorXDecisionVariable>& vars);
978 
990  const Eigen::Ref<const Eigen::RowVectorXd>& a, double lb, double ub,
991  const VariableRefList& vars) {
992  return AddLinearConstraint(a, lb, ub, ConcatenateVariableRefList(vars));
993  }
994 
1006  const Eigen::Ref<const Eigen::RowVectorXd>& a, double lb, double ub,
1007  const Eigen::Ref<const VectorXDecisionVariable>& vars) {
1008  return AddLinearConstraint(a, Vector1d(lb), Vector1d(ub), vars);
1009  }
1010 
1023  Binding<LinearConstraint> AddLinearConstraint(const symbolic::Expression& e,
1024  double lb, double ub);
1025 
1031  Binding<LinearConstraint> AddLinearConstraint(
1032  const Eigen::Ref<const VectorX<symbolic::Expression>>& v,
1033  const Eigen::Ref<const Eigen::VectorXd>& lb,
1034  const Eigen::Ref<const Eigen::VectorXd>& ub);
1035 
1061  Binding<LinearConstraint> AddLinearConstraint(const symbolic::Formula& f);
1062 
1088  template <typename Derived>
1089  typename std::enable_if<
1092  AddLinearConstraint(const Eigen::ArrayBase<Derived>& formulas) {
1093  return AddConstraint(internal::ParseLinearConstraint(formulas));
1094  }
1095 
1100  Binding<LinearEqualityConstraint> AddConstraint(
1101  const Binding<LinearEqualityConstraint>& binding);
1102 
1115  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
1116  const symbolic::Expression& e, double b);
1117 
1127  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
1128  const symbolic::Formula& f);
1129 
1144  template <typename DerivedV, typename DerivedB>
1145  typename std::enable_if<
1148  AddLinearEqualityConstraint(const Eigen::MatrixBase<DerivedV>& v,
1149  const Eigen::MatrixBase<DerivedB>& b) {
1150  return AddConstraint(internal::ParseLinearEqualityConstraint(v, b));
1151  }
1152 
1172  template <typename DerivedV, typename DerivedB>
1173  typename std::enable_if<
1176  AddLinearEqualityConstraint(const Eigen::MatrixBase<DerivedV>& V,
1177  const Eigen::MatrixBase<DerivedB>& B,
1178  bool lower_triangle = false) {
1179  return AddConstraint(
1180  internal::ParseLinearEqualityConstraint(V, B, lower_triangle));
1181  }
1182 
1204  const Eigen::Ref<const Eigen::MatrixXd>& Aeq,
1205  const Eigen::Ref<const Eigen::VectorXd>& beq,
1206  const VariableRefList& vars) {
1207  return AddLinearEqualityConstraint(Aeq, beq,
1209  }
1210 
1230  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
1231  const Eigen::Ref<const Eigen::MatrixXd>& Aeq,
1232  const Eigen::Ref<const Eigen::VectorXd>& beq,
1233  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1234 
1246  const Eigen::Ref<const Eigen::RowVectorXd>& a, double beq,
1247  const VariableRefList& vars) {
1248  return AddConstraint(std::make_shared<LinearEqualityConstraint>(a, beq),
1250  }
1251 
1263  const Eigen::Ref<const Eigen::RowVectorXd>& a, double beq,
1264  const Eigen::Ref<const VectorXDecisionVariable>& vars) {
1265  return AddLinearEqualityConstraint(a, Vector1d(beq), vars);
1266  }
1267 
1276  Binding<BoundingBoxConstraint> AddConstraint(
1277  const Binding<BoundingBoxConstraint>& binding);
1278 
1298  const Eigen::Ref<const Eigen::VectorXd>& lb,
1299  const Eigen::Ref<const Eigen::VectorXd>& ub,
1300  const VariableRefList& vars) {
1301  return AddBoundingBoxConstraint(lb, ub, ConcatenateVariableRefList(vars));
1302  }
1303 
1312  Binding<BoundingBoxConstraint> AddBoundingBoxConstraint(
1313  const Eigen::Ref<const Eigen::VectorXd>& lb,
1314  const Eigen::Ref<const Eigen::VectorXd>& ub,
1315  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1316 
1324  double lb, double ub, const symbolic::Variable& var) {
1325  MatrixDecisionVariable<1, 1> var_matrix(var);
1326  return AddBoundingBoxConstraint(Vector1d(lb), Vector1d(ub), var_matrix);
1327  }
1328 
1336  double lb, double ub, const VariableRefList& vars) {
1337  return AddBoundingBoxConstraint(lb, ub, ConcatenateVariableRefList(vars));
1338  }
1339 
1348  template <typename Derived>
1349  typename std::enable_if<
1351  Derived::ColsAtCompileTime == 1,
1353  AddBoundingBoxConstraint(double lb, double ub,
1354  const Eigen::MatrixBase<Derived>& vars) {
1355  const int kSize = Derived::RowsAtCompileTime;
1356  return AddBoundingBoxConstraint(
1357  Eigen::Matrix<double, kSize, 1>::Constant(vars.size(), lb),
1358  Eigen::Matrix<double, kSize, 1>::Constant(vars.size(), ub), vars);
1359  }
1360 
1370  template <typename Derived>
1371  typename std::enable_if<
1373  Derived::ColsAtCompileTime != 1,
1375  AddBoundingBoxConstraint(double lb, double ub,
1376  const Eigen::MatrixBase<Derived>& vars) {
1377  const int kSize =
1378  Derived::RowsAtCompileTime != Eigen::Dynamic &&
1379  Derived::ColsAtCompileTime != Eigen::Dynamic
1380  ? Derived::RowsAtCompileTime * Derived::ColsAtCompileTime
1381  : Eigen::Dynamic;
1382  Eigen::Matrix<symbolic::Variable, kSize, 1> flat_vars(vars.size());
1383  for (int j = 0; j < vars.cols(); ++j) {
1384  for (int i = 0; i < vars.rows(); ++i) {
1385  flat_vars(j * vars.rows() + i) = vars(i, j);
1386  }
1387  }
1388  return AddBoundingBoxConstraint(
1389  Eigen::Matrix<double, kSize, 1>::Constant(vars.size(), lb),
1390  Eigen::Matrix<double, kSize, 1>::Constant(vars.size(), ub), flat_vars);
1391  }
1392 
1405  Binding<LorentzConeConstraint> AddConstraint(
1406  const Binding<LorentzConeConstraint>& binding);
1407 
1418  Binding<LorentzConeConstraint> AddLorentzConeConstraint(
1419  const Eigen::Ref<const VectorX<symbolic::Expression>>& v);
1420 
1447  Binding<LorentzConeConstraint> AddLorentzConeConstraint(
1448  const symbolic::Expression& v1, const symbolic::Expression& v2);
1449 
1469  const Eigen::Ref<const Eigen::MatrixXd>& A,
1470  const Eigen::Ref<const Eigen::VectorXd>& b, const VariableRefList& vars) {
1471  return AddLorentzConeConstraint(A, b, ConcatenateVariableRefList(vars));
1472  }
1473 
1492  Binding<LorentzConeConstraint> AddLorentzConeConstraint(
1493  const Eigen::Ref<const Eigen::MatrixXd>& A,
1494  const Eigen::Ref<const Eigen::VectorXd>& b,
1495  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1496 
1509  const VariableRefList& vars) {
1510  return AddLorentzConeConstraint(ConcatenateVariableRefList(vars));
1511  }
1512 
1524  template <int rows>
1526  const Eigen::MatrixBase<VectorDecisionVariable<rows>>& vars) {
1527  Eigen::Matrix<double, rows, rows> A(vars.rows(), vars.rows());
1528  A.setIdentity();
1529  Eigen::Matrix<double, rows, 1> b(vars.rows());
1530  b.setZero();
1531  return AddLorentzConeConstraint(A, b, vars);
1532  }
1533 
1547  const Binding<RotatedLorentzConeConstraint>& binding);
1548 
1562  Binding<RotatedLorentzConeConstraint> AddRotatedLorentzConeConstraint(
1563  const Eigen::Ref<const VectorX<symbolic::Expression>>& v);
1564 
1585  const Eigen::Ref<const Eigen::MatrixXd>& A,
1586  const Eigen::Ref<const Eigen::VectorXd>& b, const VariableRefList& vars) {
1587  return AddRotatedLorentzConeConstraint(A, b,
1589  }
1590 
1610  Binding<RotatedLorentzConeConstraint> AddRotatedLorentzConeConstraint(
1611  const Eigen::Ref<const Eigen::MatrixXd>& A,
1612  const Eigen::Ref<const Eigen::VectorXd>& b,
1613  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1614 
1630  const VariableRefList& vars) {
1631  return AddRotatedLorentzConeConstraint(ConcatenateVariableRefList(vars));
1632  }
1633 
1648  template <int rows>
1650  const Eigen::MatrixBase<VectorDecisionVariable<rows>>& vars) {
1651  Eigen::Matrix<double, rows, rows> A(vars.rows(), vars.rows());
1652  A.setIdentity();
1653  Eigen::Matrix<double, rows, 1> b(vars.rows());
1654  b.setZero();
1655  return AddRotatedLorentzConeConstraint(A, b, vars);
1656  }
1657 
1664 
1670  const Eigen::Ref<const Eigen::MatrixXd>& M,
1671  const Eigen::Ref<const Eigen::VectorXd>& q, const VariableRefList& vars) {
1672  return AddLinearComplementarityConstraint(M, q,
1674  }
1675 
1680  Binding<LinearComplementarityConstraint> AddLinearComplementarityConstraint(
1681  const Eigen::Ref<const Eigen::MatrixXd>& M,
1682  const Eigen::Ref<const Eigen::VectorXd>& q,
1683  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1684 
1690  const VectorXPoly& polynomials,
1691  const std::vector<Polynomiald::VarType>& poly_vars,
1692  const Eigen::VectorXd& lb, const Eigen::VectorXd& ub,
1693  const VariableRefList& vars) {
1694  return AddPolynomialConstraint(polynomials, poly_vars, lb, ub,
1696  }
1697 
1702  Binding<Constraint> AddPolynomialConstraint(
1703  const VectorXPoly& polynomials,
1704  const std::vector<Polynomiald::VarType>& poly_vars,
1705  const Eigen::VectorXd& lb, const Eigen::VectorXd& ub,
1706  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1707 
1713 
1718  std::shared_ptr<PositiveSemidefiniteConstraint> con,
1719  const Eigen::Ref<const MatrixXDecisionVariable>& symmetric_matrix_var);
1720 
1727  Binding<PositiveSemidefiniteConstraint> AddPositiveSemidefiniteConstraint(
1728  const Eigen::Ref<const MatrixXDecisionVariable>& symmetric_matrix_var);
1729 
1742  template <typename Derived>
1743  typename std::enable_if<
1746  AddPositiveSemidefiniteConstraint(const Eigen::MatrixBase<Derived>& e) {
1747  DRAKE_DEMAND(e.rows() == e.cols());
1748  DRAKE_ASSERT(e == e.transpose());
1749  const int e_rows = Derived::RowsAtCompileTime;
1751  if (Derived::RowsAtCompileTime == Eigen::Dynamic) {
1752  M = NewSymmetricContinuousVariables(e_rows);
1753  } else {
1754  M = NewSymmetricContinuousVariables<e_rows>();
1755  }
1756  // Adds the linear equality constraint that M = e.
1757  AddLinearEqualityConstraint(
1758  e - M, Eigen::Matrix<double, e_rows, e_rows>::Zero(e.rows(), e.rows()),
1759  true);
1760  const int M_flat_size =
1761  e_rows == Eigen::Dynamic ? Eigen::Dynamic : e_rows * e_rows;
1762  const Eigen::Map<Eigen::Matrix<symbolic::Variable, M_flat_size, 1>> M_flat(
1763  &M(0, 0), e.size());
1764  return AddPositiveSemidefiniteConstraint(M);
1765  }
1766 
1772 
1777  const std::vector<Eigen::Ref<const Eigen::MatrixXd>>& F,
1778  const VariableRefList& vars) {
1779  return AddLinearMatrixInequalityConstraint(
1780  F, ConcatenateVariableRefList(vars));
1781  }
1782 
1786  Binding<LinearMatrixInequalityConstraint> AddLinearMatrixInequalityConstraint(
1787  const std::vector<Eigen::Ref<const Eigen::MatrixXd>>& F,
1788  const Eigen::Ref<const VectorXDecisionVariable>& vars);
1789 
1797  std::pair<Binding<PositiveSemidefiniteConstraint>,
1799  AddSosConstraint(const symbolic::Polynomial& p);
1800 
1810  std::pair<Binding<PositiveSemidefiniteConstraint>,
1812  AddSosConstraint(const symbolic::Expression& e);
1813 
1814  // template <typename FunctionType>
1815  // void AddCost(std::function..);
1816  // void AddLinearCost(const Eigen::MatrixBase<Derived>& c, const vector<const
1817  // DecisionVariable&>& vars)
1818  // void addQuadraticCost ...
1819 
1825  template <typename DerivedA, typename DerivedB>
1826  void SetInitialGuess(const Eigen::MatrixBase<DerivedA>& decision_variable_mat,
1827  const Eigen::MatrixBase<DerivedB>& x0) {
1828  DRAKE_ASSERT(decision_variable_mat.rows() == x0.rows());
1829  DRAKE_ASSERT(decision_variable_mat.cols() == x0.cols());
1830  for (int i = 0; i < decision_variable_mat.rows(); ++i) {
1831  for (int j = 0; j < decision_variable_mat.cols(); ++j) {
1832  x_initial_guess_(
1833  FindDecisionVariableIndex(decision_variable_mat(i, j))) = x0(i, j);
1834  }
1835  }
1836  }
1837 
1844  template <typename Derived>
1845  void SetInitialGuessForAllVariables(const Eigen::MatrixBase<Derived>& x0) {
1846  DRAKE_ASSERT(x0.rows() == num_vars() && x0.cols() == 1);
1847  x_initial_guess_ = x0;
1848  }
1849 
1855  SolutionResult Solve();
1856  // TODO(naveenoid) : add argument for options
1857 
1858  // template <typename Derived>
1859  // bool solve(const Eigen::MatrixBase<Derived>& x0);
1860 
1861  // getCostValue();
1862  // getExitFlag();
1863  // getInfeasibleConstraintNames();
1864 
1865  void PrintSolution() {
1866  for (int i = 0; i < num_vars(); ++i) {
1867  std::cout << decision_variables_(i).get_name() << " = "
1868  << GetSolution(decision_variables_(i)) << std::endl;
1869  }
1870  }
1871 
1877  void SetDecisionVariableValues(
1878  const Eigen::Ref<const Eigen::VectorXd>& values);
1879 
1886  void SetDecisionVariableValues(
1887  const Eigen::Ref<const VectorXDecisionVariable>& variables,
1888  const Eigen::Ref<const Eigen::VectorXd>& values);
1889 
1895  void SetDecisionVariableValue(const symbolic::Variable& var, double value);
1896 
1919  void SetSolverOption(const SolverId& solver_id,
1920  const std::string& solver_option,
1921  double option_value) {
1922  solver_options_double_[solver_id][solver_option] = option_value;
1923  }
1924 
1925  void SetSolverOption(const SolverId& solver_id,
1926  const std::string& solver_option,
1927  int option_value) {
1928  solver_options_int_[solver_id][solver_option] = option_value;
1929  }
1930 
1931  void SetSolverOption(const SolverId& solver_id,
1932  const std::string& solver_option,
1933  const std::string& option_value) {
1934  solver_options_str_[solver_id][solver_option] = option_value;
1935  }
1936 
1937  const std::map<std::string, double>& GetSolverOptionsDouble(
1938  const SolverId& solver_id) {
1939  return solver_options_double_[solver_id];
1940  }
1941 
1942  const std::map<std::string, int>& GetSolverOptionsInt(
1943  const SolverId& solver_id) {
1944  return solver_options_int_[solver_id];
1945  }
1946 
1947  const std::map<std::string, std::string>& GetSolverOptionsStr(
1948  const SolverId& solver_id) {
1949  return solver_options_str_[solver_id];
1950  }
1951 
1955  void SetSolverId(SolverId solver_id) {
1956  solver_id_ = solver_id;
1957  }
1958 
1964  return solver_id_;
1965  }
1966 
1971  double GetOptimalCost() const { return optimal_cost_; }
1972 
1973  void SetOptimalCost(double optimal_cost) { optimal_cost_ = optimal_cost; }
1974 
1979  double GetLowerBoundCost() const { return lower_bound_cost_; }
1988  void SetLowerBoundCost(double lower_bound_cost) {
1989  lower_bound_cost_ = lower_bound_cost;
1990  }
1991 
1995  const std::vector<Binding<Cost>>& generic_costs() const {
1996  return generic_costs_;
1997  } // e.g. for snopt_user_fun
1998 
2002  const std::vector<Binding<Constraint>>& generic_constraints() const {
2003  return generic_constraints_;
2004  } // e.g. for snopt_user_fun
2005 
2009  const std::vector<Binding<LinearEqualityConstraint>>&
2011  return linear_equality_constraints_;
2012  }
2013 
2015  const std::vector<Binding<LinearCost>>& linear_costs() const {
2016  return linear_costs_;
2017  }
2018 
2020  const std::vector<Binding<QuadraticCost>>& quadratic_costs() const {
2021  return quadratic_costs_;
2022  }
2023 
2025  const std::vector<Binding<LinearConstraint>>& linear_constraints() const {
2026  return linear_constraints_;
2027  }
2028 
2030  const std::vector<Binding<LorentzConeConstraint>>& lorentz_cone_constraints()
2031  const {
2032  return lorentz_cone_constraint_;
2033  }
2034 
2036  const std::vector<Binding<RotatedLorentzConeConstraint>>&
2038  return rotated_lorentz_cone_constraint_;
2039  }
2040 
2042  const std::vector<Binding<PositiveSemidefiniteConstraint>>&
2044  return positive_semidefinite_constraint_;
2045  }
2046 
2048  const std::vector<Binding<LinearMatrixInequalityConstraint>>&
2050  return linear_matrix_inequality_constraint_;
2051  }
2052 
2058  std::vector<Binding<Cost>> GetAllCosts() const {
2059  auto costlist = generic_costs_;
2060  costlist.insert(costlist.end(), linear_costs_.begin(), linear_costs_.end());
2061  costlist.insert(costlist.end(), quadratic_costs_.begin(),
2062  quadratic_costs_.end());
2063  return costlist;
2064  }
2065 
2070  std::vector<Binding<LinearConstraint>> GetAllLinearConstraints() const {
2071  std::vector<Binding<LinearConstraint>> conlist = linear_constraints_;
2072  conlist.insert(conlist.end(), linear_equality_constraints_.begin(),
2073  linear_equality_constraints_.end());
2074  return conlist;
2075  }
2076 
2078  const std::vector<Binding<BoundingBoxConstraint>>& bounding_box_constraints()
2079  const {
2080  return bbox_constraints_;
2081  }
2082 
2084  const std::vector<Binding<LinearComplementarityConstraint>>&
2086  return linear_complementarity_constraints_;
2087  }
2088 
2089  // Base class for solver-specific data. A solver implementation may derive
2090  // a helper class from this for use with getSolverData.
2091  struct SolverData {
2093  SolverData() = default;
2094  virtual ~SolverData() = default;
2095  };
2096 
2097  // Call from solver implementations to get a persistently-stored
2098  // helper structure of type T (derived from SolverData). If no
2099  // data of type T is already stored then a new one will be created
2100  // and stored, replacing data from any other solver in this problem
2101  // instance.
2102  template <typename T>
2103  std::shared_ptr<T> GetSolverData() {
2104  auto p = std::dynamic_pointer_cast<T>(solver_data_);
2105  if (!p) {
2106  p = std::make_shared<T>();
2107  solver_data_ = p;
2108  }
2109  return p;
2110  }
2111 
2113  int num_vars() const { return decision_variables_.rows(); }
2114 
2116  const Eigen::VectorXd& initial_guess() const { return x_initial_guess_; }
2117 
2125  int FindDecisionVariableIndex(const symbolic::Variable& var) const;
2126 
2135  int num_indeterminates() const { return indeterminates_.rows(); }
2136 
2144  size_t FindIndeterminateIndex(const symbolic::Variable& var) const;
2145 
2153  template <typename Derived>
2154  typename std::enable_if<
2156  Eigen::Matrix<double, Derived::RowsAtCompileTime,
2157  Derived::ColsAtCompileTime>>::type
2158  GetSolution(const Eigen::MatrixBase<Derived>& var) const {
2159  Eigen::Matrix<double, Derived::RowsAtCompileTime,
2160  Derived::ColsAtCompileTime>
2161  value(var.rows(), var.cols());
2162  for (int i = 0; i < var.rows(); ++i) {
2163  for (int j = 0; j < var.cols(); ++j) {
2164  value(i, j) = GetSolution(var(i, j));
2165  }
2166  }
2167  return value;
2168  }
2169 
2173  double GetSolution(const symbolic::Variable& var) const;
2174 
2181  template <typename C>
2182  Eigen::VectorXd EvalBindingAtSolution(const Binding<C>& binding) const {
2183  Eigen::VectorXd val(binding.constraint()->num_outputs());
2184  Eigen::VectorXd binding_var_vals = GetSolution(binding.variables());
2185  binding.constraint()->Eval(binding_var_vals, val);
2186  return val;
2187  }
2188 
2191  return decision_variables_;
2192  }
2193 
2196  return decision_variables_(i);
2197  }
2198 
2200  const VectorXIndeterminate& indeterminates() const { return indeterminates_; }
2201 
2203  const symbolic::Variable& indeterminate(int i) const {
2204  return indeterminates_(i);
2205  }
2206 
2207  private:
2208  // maps the ID of a symbolic variable to the index of the variable stored in
2209  // the optimization program.
2210  std::unordered_map<symbolic::Variable::Id, int> decision_variable_index_{};
2211 
2212  VectorXDecisionVariable decision_variables_;
2213 
2214  std::unordered_map<symbolic::Variable::Id, int> indeterminates_index_;
2215  VectorXIndeterminate indeterminates_;
2216 
2217  std::vector<Binding<Cost>> generic_costs_;
2218  std::vector<Binding<Constraint>> generic_constraints_;
2219  std::vector<Binding<QuadraticCost>> quadratic_costs_;
2220  std::vector<Binding<LinearCost>> linear_costs_;
2221  // TODO(naveenoid) : quadratic_constraints_
2222 
2223  // note: linear_constraints_ does not include linear_equality_constraints_
2224  std::vector<Binding<LinearConstraint>> linear_constraints_;
2225  std::vector<Binding<LinearEqualityConstraint>> linear_equality_constraints_;
2226  std::vector<Binding<BoundingBoxConstraint>> bbox_constraints_;
2227  std::vector<Binding<LorentzConeConstraint>> lorentz_cone_constraint_;
2228  std::vector<Binding<RotatedLorentzConeConstraint>>
2229  rotated_lorentz_cone_constraint_;
2230  std::vector<Binding<PositiveSemidefiniteConstraint>>
2231  positive_semidefinite_constraint_;
2232  std::vector<Binding<LinearMatrixInequalityConstraint>>
2233  linear_matrix_inequality_constraint_;
2234 
2235  // Invariant: The bindings in this list must be non-overlapping, when calling
2236  // Linear Complementarity solver like Moby. If this constraint is solved
2237  // through a nonlinear optimization solver (like SNOPT) instead, then we allow
2238  // the bindings to be overlapping.
2239  // TODO(ggould-tri) can this constraint be relaxed?
2240  std::vector<Binding<LinearComplementarityConstraint>>
2241  linear_complementarity_constraints_;
2242 
2243  Eigen::VectorXd x_initial_guess_;
2244  std::vector<double> x_values_;
2245  std::shared_ptr<SolverData> solver_data_;
2246  optional<SolverId> solver_id_;
2247  double optimal_cost_{};
2248  // The lower bound of the objective found by the solver, during the
2249  // optimization process.
2250  double lower_bound_cost_{};
2251  std::map<SolverId, std::map<std::string, double>> solver_options_double_;
2252  std::map<SolverId, std::map<std::string, int>> solver_options_int_;
2253  std::map<SolverId, std::map<std::string, std::string>> solver_options_str_;
2254 
2255  AttributesSet required_capabilities_{0};
2256 
2257  std::unique_ptr<MathematicalProgramSolverInterface> ipopt_solver_;
2258  std::unique_ptr<MathematicalProgramSolverInterface> nlopt_solver_;
2259  std::unique_ptr<MathematicalProgramSolverInterface> snopt_solver_;
2260  std::unique_ptr<MathematicalProgramSolverInterface> moby_lcp_solver_;
2261  std::unique_ptr<MathematicalProgramSolverInterface> linear_system_solver_;
2262  std::unique_ptr<MathematicalProgramSolverInterface>
2263  equality_constrained_qp_solver_;
2264  std::unique_ptr<MathematicalProgramSolverInterface> gurobi_solver_;
2265  std::unique_ptr<MathematicalProgramSolverInterface> mosek_solver_;
2266 
2267  template <typename T>
2268  void NewVariables_impl(
2269  VarType type, const T& names, bool is_symmetric,
2270  Eigen::Ref<MatrixXDecisionVariable> decision_variable_matrix) {
2271  switch (type) {
2272  case VarType::CONTINUOUS:
2273  break;
2274  case VarType::BINARY:
2275  required_capabilities_ |= kBinaryVariable;
2276  break;
2277  case VarType::INTEGER:
2278  throw std::runtime_error(
2279  "MathematicalProgram does not support integer variables yet.");
2280  case VarType::BOOLEAN:
2281  throw std::runtime_error(
2282  "MathematicalProgram does not support Boolean variables.");
2283  }
2284  int rows = decision_variable_matrix.rows();
2285  int cols = decision_variable_matrix.cols();
2286  DRAKE_ASSERT(!is_symmetric || rows == cols);
2287  int num_new_vars = 0;
2288  if (!is_symmetric) {
2289  num_new_vars = rows * cols;
2290  } else {
2291  num_new_vars = rows * (rows + 1) / 2;
2292  }
2293  DRAKE_ASSERT(static_cast<int>(names.size()) == num_new_vars);
2294  decision_variables_.conservativeResize(num_vars() + num_new_vars,
2295  Eigen::NoChange);
2296  x_values_.resize(num_vars() + num_new_vars, NAN);
2297  int row_index = 0;
2298  int col_index = 0;
2299  for (int i = 0; i < num_new_vars; ++i) {
2300  decision_variables_(num_vars() - num_new_vars + i) =
2301  symbolic::Variable(names[i], type);
2302  const int new_var_index = num_vars() - num_new_vars + i;
2303  decision_variable_index_.insert(std::pair<int, int>(
2304  decision_variables_(new_var_index).get_id(), new_var_index));
2305  decision_variable_matrix(row_index, col_index) =
2306  decision_variables_(num_vars() - num_new_vars + i);
2307  // If the matrix is not symmetric, then store the variable in column
2308  // major.
2309  if (!is_symmetric) {
2310  if (row_index + 1 < rows) {
2311  ++row_index;
2312  } else {
2313  ++col_index;
2314  row_index = 0;
2315  }
2316  } else {
2317  // If the matrix is symmetric, then the decision variables are the lower
2318  // triangular part of the symmetric matrix, also stored in column major.
2319  if (row_index != col_index) {
2320  decision_variable_matrix(col_index, row_index) =
2321  decision_variable_matrix(row_index, col_index);
2322  }
2323  if (row_index + 1 < rows) {
2324  ++row_index;
2325  } else {
2326  ++col_index;
2327  row_index = col_index;
2328  }
2329  }
2330  }
2331 
2332  x_initial_guess_.conservativeResize(num_vars());
2333  x_initial_guess_.tail(num_new_vars)
2334  .fill(std::numeric_limits<double>::quiet_NaN());
2335  }
2336 
2337  MatrixXDecisionVariable NewVariables(VarType type, int rows, int cols,
2338  bool is_symmetric,
2339  const std::vector<std::string>& names);
2340 
2341  template <typename T>
2342  void NewIndeterminates_impl(
2343  const T& names, Eigen::Ref<MatrixXIndeterminate> indeterminates_matrix) {
2344  int rows = indeterminates_matrix.rows();
2345  int cols = indeterminates_matrix.cols();
2346  int num_new_vars = rows * cols;
2347 
2348  DRAKE_ASSERT(static_cast<int>(names.size()) == num_new_vars);
2349  indeterminates_.conservativeResize(indeterminates_.rows() + num_new_vars,
2350  Eigen::NoChange);
2351  int row_index = 0;
2352  int col_index = 0;
2353  for (int i = 0; i < num_new_vars; ++i) {
2354  indeterminates_(indeterminates_.rows() - num_new_vars + i) =
2355  symbolic::Variable(names[i]);
2356 
2357  const int new_var_index = indeterminates_.rows() - num_new_vars + i;
2358  indeterminates_index_.insert(std::pair<size_t, size_t>(
2359  indeterminates_(new_var_index).get_id(), new_var_index));
2360  indeterminates_matrix(row_index, col_index) =
2361  indeterminates_(indeterminates_.rows() - num_new_vars + i);
2362 
2363  // store the indeterminate in column major.
2364  if (row_index + 1 < rows) {
2365  ++row_index;
2366  } else {
2367  ++col_index;
2368  row_index = 0;
2369  }
2370  }
2371  }
2372 
2373  /*
2374  * Given a matrix of decision variables, checks if every entry in the
2375  * matrix is a decision variable in the program; throws a runtime
2376  * error if any variable is not a decision variable in the program.
2377  * @tparam Derived An Eigen::Matrix type of symbolic Variable.
2378  * @param vars A matrix of variables.
2379  */
2380  template <typename Derived>
2381  typename std::enable_if<
2382  std::is_same<typename Derived::Scalar, symbolic::Variable>::value>::type
2383  CheckIsDecisionVariable(const Eigen::MatrixBase<Derived>& vars) const {
2384  for (int i = 0; i < vars.rows(); ++i) {
2385  for (int j = 0; j < vars.cols(); ++j) {
2386  if (decision_variable_index_.find(vars(i, j).get_id()) ==
2387  decision_variable_index_.end()) {
2388  std::ostringstream oss;
2389  oss << vars(i, j)
2390  << " is not a decision variable of the mathematical program.\n";
2391  throw std::runtime_error(oss.str());
2392  }
2393  }
2394  }
2395  }
2396 
2397  /*
2398  * Ensure a binding is valid *before* adding it to the program.
2399  * @pre The binding has not yet been registered.
2400  * @pre The decision variables have been registered.
2401  * @throws std::runtime_error if the binding is invalid.
2402  */
2403  template <typename C>
2404  void CheckBinding(const Binding<C>& binding) const {
2405  // TODO(eric.cousineau): In addition to identifiers, hash bindings by
2406  // their constraints and their variables, to prevent duplicates.
2407  // TODO(eric.cousineau): Once bindings have identifiers (perhaps
2408  // retrofitting `description`), ensure that they have unique names.
2409  CheckIsDecisionVariable(binding.variables());
2410  }
2411 
2412  // Adds a linear constraint represented by a set of symbolic formulas to the
2413  // program.
2414  //
2415  // Precondition: ∀ f ∈ formulas, is_relational(f).
2416  Binding<LinearConstraint> AddLinearConstraint(
2417  const std::set<symbolic::Formula>& formulas);
2418 
2419  // Adds a linear-equality constraint represented by a set of symbolic formulas
2420  // to the program.
2421  //
2422  // Precondition: ∀ f ∈ formulas, is_equal_to(f).
2423  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
2424  const std::set<symbolic::Formula>& formulas);
2425 
2429  template <int Rows, int Cols>
2431  VarType type, const typename NewVariableNames<Rows, Cols>::type& names,
2432  int rows, int cols) {
2433  DRAKE_DEMAND(rows >= 0 && cols >= 0);
2434  MatrixDecisionVariable<Rows, Cols> decision_variable_matrix;
2435  decision_variable_matrix.resize(rows, cols);
2436  NewVariables_impl(type, names, false, decision_variable_matrix);
2437  return decision_variable_matrix;
2438  }
2439 
2446  template <int Rows>
2447  MatrixDecisionVariable<Rows, Rows> NewSymmetricVariables(
2448  VarType type,
2449  const typename NewSymmetricVariableNames<Rows>::type& names,
2450  int rows = Rows) {
2451  MatrixDecisionVariable<Rows, Rows> decision_variable_matrix(rows, rows);
2452  NewVariables_impl(type, names, true, decision_variable_matrix);
2453  return decision_variable_matrix;
2454  }
2455 };
2456 
2457 } // namespace solvers
2458 } // namespace drake
Binding< QuadraticCost > AddQuadraticErrorCost(const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &x_desired, const VariableRefList &vars)
Adds a cost term of the form (x-x_desired)&#39;Q(x-x_desired).
Definition: mathematical_program.h:838
const std::vector< Binding< QuadraticCost > > & quadratic_costs() const
Getter for quadratic costs.
Definition: mathematical_program.h:2020
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:1584
std::enable_if< detail::assert_if_is_constraint< F >::value, Binding< Cost > >::type AddCost(F &&, Vars &&)
Statically assert if a user inadvertently passes a binding-compatible Constraint. ...
Definition: mathematical_program.h:766
void PrintSolution()
Definition: mathematical_program.h:1865
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:1746
Definition: eigen_types.h:194
Definition: mathematical_program.h:184
std::vector< snopt::doublereal > F
Definition: snopt_solver.cc:59
Binding< LinearConstraint > ParseLinearConstraint(const Eigen::Ref< const VectorX< Expression >> &v, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)
Definition: create_constraint.cc:34
VectorXDecisionVariable NewBinaryVariables(int rows, const std::string &name="b")
Adds binary variables to this MathematicalProgram.
Definition: mathematical_program.h:459
shared_ptr< QuadraticCost > MakeL2NormCost(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b)
Creates a cost term of the form | Ax - b |^2.
Definition: cost.cc:46
Definition: mathematical_program.h:2091
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:2049
Binding< BoundingBoxConstraint > AddBoundingBoxConstraint(double lb, double ub, const VariableRefList &vars)
Adds the same scalar lower and upper bound to every variable in the list.
Definition: mathematical_program.h:1335
std::vector< Binding< LinearConstraint > > GetAllLinearConstraints() const
Getter returning all linear constraints (both linear equality and inequality constraints).
Definition: mathematical_program.h:2070
Represents a symbolic variable.
Definition: symbolic_variable.h:24
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Eigen::Matrix< Polynomiald, Eigen::Dynamic, 1 > VectorXPoly
A column vector of polynomials; used in several optimization classes.
Definition: polynomial.h:483
static std::shared_ptr< Cost > MakeCost(F &&f)
Convert an input of type F to a FunctionCost object.
Definition: mathematical_program.h:728
Definition: symbolic_expression.h:915
Provides a portable macro for use in generating compile-time warnings for use of code that is permitt...
The type of the names for the newly added variables.
Definition: mathematical_program.h:201
double GetOptimalCost() const
Getter for optimal cost at the solution.
Definition: mathematical_program.h:1971
Binding< RotatedLorentzConeConstraint > AddRotatedLorentzConeConstraint(const VariableRefList &vars)
Impose that a vector is in rotated Lorentz cone.
Definition: mathematical_program.h:1629
std::enable_if< is_eigen_vector_expression_double_pair< DerivedV, DerivedB >::value, Binding< LinearEqualityConstraint > >::type AddLinearEqualityConstraint(const Eigen::MatrixBase< DerivedV > &v, const Eigen::MatrixBase< DerivedB > &b)
Adds linear equality constraints , where v(i) is a symbolic linear expression.
Definition: mathematical_program.h:1148
Definition: automotive_demo.cc:88
int num_indeterminates() const
Gets the solution of an Eigen matrix of decision variables.
Definition: mathematical_program.h:2135
Binding< LinearEqualityConstraint > ParseLinearEqualityConstraint(const set< Formula > &formulas)
Definition: create_constraint.cc:307
auto AddCost(const std::shared_ptr< C > &obj, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost type to the optimization program.
Definition: mathematical_program.h:704
const std::vector< Binding< LinearComplementarityConstraint > > & linear_complementarity_constraints() const
Getter for all linear complementarity constraints.
Definition: mathematical_program.h:2085
This file contains abbreviated definitions for certain uses of AutoDiffScalar that are commonly used ...
std::shared_ptr< Cost > MakeFunctionCost(FF &&f)
Converts an input of type F to a nonlinear cost.
Definition: cost.h:239
void SetOptimalCost(double optimal_cost)
Definition: mathematical_program.h:1973
Definition: mathematical_program.h:193
Represents a symbolic form of a first-order logic formula.
Definition: symbolic_formula.h:113
Definition: mathematical_program.h:182
Binding< LorentzConeConstraint > AddLorentzConeConstraint(const VariableRefList &vars)
Imposes that a vector lies in Lorentz cone.
Definition: mathematical_program.h:1508
VectorIndeterminate< Eigen::Dynamic > VectorXIndeterminate
VectorXIndeterminate is used as an alias for Eigen::Matrix<symbolic::Variable, Eigen::Dynamic, 1>.
Definition: indeterminate.h:43
const symbolic::Variable & decision_variable(int i) const
Getter for the decision variable with index i in the program.
Definition: mathematical_program.h:2195
Definition: mathematical_program.h:175
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:185
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:1649
MatrixDecisionVariable< rows, rows > NewSymmetricContinuousVariables(const std::string &name="Symmetric")
Adds a static sized symmetric matrix as decision variables to this MathematicalProgram.
Definition: mathematical_program.h:504
const std::map< std::string, std::string > & GetSolverOptionsStr(const SolverId &solver_id)
Definition: mathematical_program.h:1947
std::enable_if< is_eigen_scalar_same< Derived, symbolic::Formula >::value, Binding< LinearConstraint > >::type AddLinearConstraint(const Eigen::ArrayBase< Derived > &formulas)
Add a linear constraint represented by an Eigen::Array<symbolic::Formula> to the program.
Definition: mathematical_program.h:1092
Definition: mathematical_program.h:187
void SetSolverOption(const SolverId &solver_id, const std::string &solver_option, int option_value)
Definition: mathematical_program.h:1925
Binding< BoundingBoxConstraint > AddBoundingBoxConstraint(double lb, double ub, const symbolic::Variable &var)
Adds bounds for a single variable.
Definition: mathematical_program.h:1323
const VectorXDecisionVariable & decision_variables() const
Getter for all decision variables in the program.
Definition: mathematical_program.h:2190
Binding< QuadraticCost > AddL2NormCost(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost term of the form | Ax - b |^2.
Definition: mathematical_program.h:866
MatrixDecisionVariable< Rows, Cols > NewContinuousVariables(const std::string &name="X")
Adds continuous variables, appending them to an internal vector of any existing vars.
Definition: mathematical_program.h:395
Definition: mathematical_program.h:186
MatrixIndeterminate< rows, cols > NewIndeterminates(const std::string &name="X")
Adds indeterminates, appending them to an internal vector of any existing indeterminates.
Definition: mathematical_program.h:615
A binding on constraint type C is a mapping of the decision variables onto the inputs of C...
Definition: binding.h:17
Definition: mathematical_program.h:179
const Eigen::VectorXd & initial_guess() const
Getter for the initial guess.
Definition: mathematical_program.h:2116
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:1468
std::vector< snopt::doublereal > A
Definition: snopt_solver.cc:65
const std::vector< Binding< Cost > > & generic_costs() const
Getter for all generic costs.
Definition: mathematical_program.h:1995
int b
Definition: rgbd_camera.cc:91
stx::optional< T > optional
Definition: drake_optional.h:15
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:2010
Binding< LinearConstraint > AddLinearConstraint(const Eigen::Ref< const Eigen::RowVectorXd > &a, double lb, double ub, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds one row of linear constraint referencing potentially a subset of the decision variables (defined...
Definition: mathematical_program.h:1005
uint32_t AttributesSet
Definition: mathematical_program.h:190
Enables us to catch and provide a meaningful assertion if a Constraint is passed in, when we should have a Cost.
Definition: create_cost.h:62
VectorIndeterminate< rows > NewIndeterminates(const std::string &name="x")
Adds indeterminates to the program.
Definition: mathematical_program.h:634
Binding< LinearEqualityConstraint > AddLinearEqualityConstraint(const Eigen::Ref< const Eigen::RowVectorXd > &a, double beq, const VariableRefList &vars)
Adds one row of linear equality constraint referencing potentially a subset of decision variables...
Definition: mathematical_program.h:1245
const std::vector< Binding< LinearCost > > & linear_costs() const
Getter for linear costs.
Definition: mathematical_program.h:2015
int v
Definition: rgbd_camera_test.cc:165
#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:39
std::enable_if< Size==Eigen::Dynamic, typename NewVariableNames< Size >::type >::type CreateNewVariableNames(int size)
Return un-initialized new variable names.
Definition: mathematical_program.h:236
Definition: mathematical_program.h:206
Binding< LinearCost > AddLinearCost(const Eigen::Ref< const Eigen::VectorXd > &a, const VarType &vars)
Adds a linear cost term of the form a&#39;*x.
Definition: mathematical_program.h:812
const std::vector< Binding< Constraint > > & generic_constraints() const
Getter for all generic constraints.
Definition: mathematical_program.h:2002
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:2037
Definition: mathematical_program.h:178
Provides Drake&#39;s assertion implementation.
Binding< LinearEqualityConstraint > AddLinearEqualityConstraint(const Eigen::Ref< const Eigen::RowVectorXd > &a, double beq, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds one row of linear equality constraint referencing potentially a subset of decision variables...
Definition: mathematical_program.h:1262
void SetInitialGuessForAllVariables(const Eigen::MatrixBase< Derived > &x0)
Set the intial guess for ALL decision variables.
Definition: mathematical_program.h:1845
auto AddConstraint(std::shared_ptr< C > con, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a generic constraint to the program.
Definition: mathematical_program.h:945
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:47
std::enable_if< is_eigen_nonvector_expression_double_pair< DerivedV, DerivedB >::value, Binding< LinearEqualityConstraint > >::type AddLinearEqualityConstraint(const Eigen::MatrixBase< DerivedV > &V, const Eigen::MatrixBase< DerivedB > &B, bool lower_triangle=false)
Adds a linear equality constraint for a matrix of linear expression V, such that V(i, j) = B(i, j).
Definition: mathematical_program.h:1176
Definition: mathematical_program.h:211
Binding< BoundingBoxConstraint > AddBoundingBoxConstraint(const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const VariableRefList &vars)
AddBoundingBoxConstraint.
Definition: mathematical_program.h:1297
std::vector< Binding< Cost > > GetAllCosts() const
Getter returning all costs (for now linear costs appended to generic costs, then quadratic costs appe...
Definition: mathematical_program.h:2058
Binding< LinearConstraint > AddLinearConstraint(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const VariableRefList &vars)
Adds linear constraints referencing potentially a subset of the decision variables (defined in the va...
Definition: mathematical_program.h:961
Binding< LinearEqualityConstraint > AddLinearEqualityConstraint(const Eigen::Ref< const Eigen::MatrixXd > &Aeq, const Eigen::Ref< const Eigen::VectorXd > &beq, const VariableRefList &vars)
AddLinearEqualityConstraint.
Definition: mathematical_program.h:1203
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
MatrixIndeterminate< Eigen::Dynamic, Eigen::Dynamic > MatrixXIndeterminate
MatrixXIndeterminate is used as an alias for Eigen::Matrix<symbolic::Variable, Eigen::Dynamic, Eigen::Dynamic>.
Definition: indeterminate.h:37
const VectorXDecisionVariable & variables() const
Definition: binding.h:49
SolutionResult
Definition: mathematical_program_solver_interface.h:13
Eigen::VectorXd EvalBindingAtSolution(const Binding< C > &binding) const
Evaluate the constraint in the Binding at the solution value.
Definition: mathematical_program.h:2182
auto AddCost(const std::shared_ptr< C > &obj, const VariableRefList &vars)
Adds a generic cost to the optimization program.
Definition: mathematical_program.h:717
Eigen::Matrix< symbolic::Variable, rows, cols > MatrixIndeterminate
MatrixIndeterminate<int, int> is used as an alias for Eigen::Matrix<symbolic::Variable, int, int>.
Definition: indeterminate.h:21
void SetSolverId(SolverId solver_id)
Sets the ID of the solver that was used to solve this program.
Definition: mathematical_program.h:1955
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:2158
std::array< std::string, Size > type
Definition: mathematical_program.h:202
const std::vector< Binding< LinearConstraint > > & linear_constraints() const
Getter for linear constraints.
Definition: mathematical_program.h:2025
Represents a symbolic form of an expression.
Definition: symbolic_expression.h:172
Do not use, to avoid & vs. && typos.
Definition: mathematical_program.h:176
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:1919
ProgramAttributes
Definition: mathematical_program.h:174
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:1988
std::vector< std::string > type
Definition: mathematical_program.h:207
const VectorXDecisionVariable * vars
Definition: nlopt_solver.cc:103
std::enable_if< detail::is_cost_functor_candidate< F >::value, Binding< Cost > >::type AddCost(F &&f, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost to the optimization program on an Eigen::Vector containing decision variables.
Definition: mathematical_program.h:753
Binding< Constraint > AddPolynomialConstraint(const VectorXPoly &polynomials, const std::vector< Polynomiald::VarType > &poly_vars, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const VariableRefList &vars)
Adds a polynomial constraint to the program referencing a subset of the decision variables (defined i...
Definition: mathematical_program.h:1689
VectorIndeterminate< rows > NewIndeterminates(const std::array< std::string, rows > &names)
Adds indeterminates, appending them to an internal vector of any existing indeterminates.
Definition: mathematical_program.h:589
void SetVariableNames(const std::string &name, int rows, int cols, Derived *names)
Set the names of the newly added variables.
Definition: mathematical_program.h:248
const std::map< std::string, double > & GetSolverOptionsDouble(const SolverId &solver_id)
Definition: mathematical_program.h:1937
const symbolic::Variable & indeterminate(int i) const
Getter for the indeterminate with index i in the program.
Definition: mathematical_program.h:2203
void SetSolverOption(const SolverId &solver_id, const std::string &solver_option, const std::string &option_value)
Definition: mathematical_program.h:1931
std::enable_if< detail::is_cost_functor_candidate< F >::value, Binding< Cost > >::type AddCost(F &&f, const VariableRefList &vars)
Adds a cost to the optimization program on a list of variables.
Definition: mathematical_program.h:740
Binding< QuadraticCost > AddQuadraticCost(const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
Adds a cost term of the form 0.5*x&#39;*Q*x + b&#39;x.
Definition: mathematical_program.h:877
std::list< Eigen::Ref< const VectorXDecisionVariable >> VariableRefList
Definition: decision_variable.h:19
std::shared_ptr< T > GetSolverData()
Definition: mathematical_program.h:2103
MatrixDecisionVariable< Rows, Cols > NewBinaryVariables(int rows, int cols, const std::string &name)
Adds binary variables, appending them to an internal vector of any existing vars. ...
Definition: mathematical_program.h:429
MatrixDecisionVariable< Rows, Cols > NewContinuousVariables(int rows, int cols, const std::string &name)
Adds continuous variables, appending them to an internal vector of any existing vars.
Definition: mathematical_program.h:359
const std::map< std::string, int > & GetSolverOptionsInt(const SolverId &solver_id)
Definition: mathematical_program.h:1942
optional< SolverId > GetSolverId() const
Returns the ID of the solver that was used to solve this program.
Definition: mathematical_program.h:1963
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:1776
MatrixDecisionVariable< Rows, Cols > NewBinaryVariables(const std::string &name="b")
Adds a matrix of binary variables into the optimization program.
Definition: mathematical_program.h:448
auto AddConstraint(std::shared_ptr< C > con, const VariableRefList &vars)
Adds a generic constraint to the program.
Definition: mathematical_program.h:934
MatrixDecisionVariable< rows, 1 > VectorDecisionVariable
Definition: decision_variable.h:14
Definition: mathematical_program.h:183
Definition: mathematical_program.h:188
double GetLowerBoundCost() const
Getter for lower bound on optimal cost.
Definition: mathematical_program.h:1979
MatrixDecisionVariable< Eigen::Dynamic, Eigen::Dynamic > MatrixXDecisionVariable
Definition: decision_variable.h:16
Definition: mathematical_program.h:215
Represents symbolic polynomials.
Definition: symbolic_polynomial.h:30
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value &&Derived::ColsAtCompileTime==1, Binding< BoundingBoxConstraint > >::type AddBoundingBoxConstraint(double lb, double ub, const Eigen::MatrixBase< Derived > &vars)
Adds the same scalar lower and upper bound to every variable in vars.
Definition: mathematical_program.h:1353
Eigen::Matrix< double, 1, 1 > Vector1d
A column vector of size 1 of doubles.
Definition: eigen_types.h:26
Represents a set of variables.
Definition: symbolic_variables.h:30
Definition: mathematical_program.h:181
Binding< QuadraticCost > AddL2NormCost(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
Adds a cost term of the form | Ax - b |^2.
Definition: mathematical_program.h:857
Template condition to only catch when Constraints are inadvertently passed as an argument.
Definition: mathematical_program.h:276
std::vector< std::string > names
Definition: translator_test.cc:19
Definition: mathematical_program.h:180
const std::vector< Binding< PositiveSemidefiniteConstraint > > & positive_semidefinite_constraints() const
Getter for positive semidefinite constraint.
Definition: mathematical_program.h:2043
Definition: mathematical_program.h:177
Definition: symbolic_expression.h:905
VectorXDecisionVariable ConcatenateVariableRefList(const VariableRefList &var_list)
Concatenates each element in var_list into a single Eigen vector of decision variables, returns this concatenated vector.
Definition: decision_variable.cc:5
Provides public header files of Drake&#39;s symbolic library.
VectorXDecisionVariable NewContinuousVariables(int rows, const std::string &name="x")
Adds continuous variables, appending them to an internal vector of any existing vars.
Definition: mathematical_program.h:320
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value &&Derived::ColsAtCompileTime!=1, Binding< BoundingBoxConstraint > >::type AddBoundingBoxConstraint(double lb, double ub, const Eigen::MatrixBase< Derived > &vars)
Adds the same scalar lower and upper bound to every variable in vars.
Definition: mathematical_program.h:1375
void SetInitialGuess(const Eigen::MatrixBase< DerivedA > &decision_variable_mat, const Eigen::MatrixBase< DerivedB > &x0)
Set the initial guess for the decision variables stored in var to be x0.
Definition: mathematical_program.h:1826
Definition: mathematical_program.h:286
const int kSize
Definition: diagram_test.cc:174
Binding< LinearCost > AddLinearCost(const Eigen::Ref< const Eigen::VectorXd > &a, double b, const VariableRefList &vars)
Adds a linear cost term of the form a&#39;*x + b.
Definition: mathematical_program.h:792
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:35
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:1669
Binding< LorentzConeConstraint > AddLorentzConeConstraint(const Eigen::MatrixBase< VectorDecisionVariable< rows >> &vars)
Imposes that a vector lies in Lorentz cone.
Definition: mathematical_program.h:1525
const std::vector< Binding< LorentzConeConstraint > > & lorentz_cone_constraints() const
Getter for Lorentz cone constraint.
Definition: mathematical_program.h:2030
Binding< LinearConstraint > AddLinearConstraint(const Eigen::Ref< const Eigen::RowVectorXd > &a, double lb, double ub, const VariableRefList &vars)
Adds one row of linear constraint referencing potentially a subset of the decision variables (defined...
Definition: mathematical_program.h:989
Identifies a MathematicalProgramSolverInterface implementation.
Definition: solver_id.h:17
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
MatrixIndeterminate< rows, cols > NewIndeterminates(const std::array< std::string, rows *cols > &names)
Adds indeterminates, appending them to an internal vector of any existing indeterminates.
Definition: mathematical_program.h:561
int num_vars() const
Getter for number of variables in the optimization program.
Definition: mathematical_program.h:2113
const VectorXIndeterminate & indeterminates() const
Getter for all indeterminates in the program.
Definition: mathematical_program.h:2200
const std::vector< Binding< BoundingBoxConstraint > > & bounding_box_constraints() const
Getter for all bounding box constraints.
Definition: mathematical_program.h:2078