Add sum-of-squares to MathematicalProgram

[hongkai-dai]

Goal

To solve sum-of-squares programming through MathematicalProgram. The user can write the code like

// Find the Lyapunov function V(x) for system
// xdot0 = -x1 + 3/2*x0^2 - 1/2*x0^3
// xdot1 = 3*x0-x1
// By solving a sum-of-squared problem.
// V(x) is sum-of-squares
// -Vdot(x)  is sum-of-squares
MathematicalProgram prog;
symbolic::Indeterminate x0, x1;
Vector2<symbolic::Expression> x;
x << symbolic::Expression(x0), symbolic::Expression(x1);
// Form the dynamics of the system.
Vector2<symbolic::Expression> dynamics;
dynamics << -x(1) + 3/2*x(0)*x(0) - 1/2*pow(x(0), 3), 3*x(0)-x(1);
// Add V(x) as a 4th order sum-of-squares polynomial.
auto V = prog.AddSumOfSquaresPolynomial({x0, x1}, 4);
// Compute Vdot.
auto Vdot = Jacobian(V, {x0, x1}) * dynamics;
// -Vdot is sum-of-squares.
prog.AddSumOfSquaresConstraint(-Vdot);

prog.Solve();

Proposed Solution

1. [x] 1. Derive a class symbolic::Indeterminate from class symbolic::Expression, to differentiate this variable from the decision variable of type symbolic::DecisionVariable.
2. [x] 2. Add function

   VectorX<symbolic::Expression> monomials(const std::set<symbolic::Indeterminate>& x, unsiged int degree);

   to return all the monomials of x up to a given degree.

3. [x] 3. Add a function, such that given a symbolic expression, decompose it a matrix product form.

   Decompose(const symbolic::Expression& expression, const std::vector<symbolic::Variable>& x, VectorX<symbolic::Expression>* coefficients, MatrixXi* power)

   Such that for an expression a*x^2 + 2*x + b would be decomposed to

   coefficients = [a; 2; b]
   power = [2; 1; 0]

   The math is

   expression = sum_k coefficients(k) * prod_j x(j)^power(k, j)

4. [x] 4. Add a function to return the Jacobian matrix of a symbolic expression w.r.t some symbolic variables.

   MatrixX<symbolic::Expression> Jacobian(const Eigen::Ref<const VectorX<symbolic::Expression>> expression, const std::vector<symbolic::Variable>& variables);

5. [x] 5. Semi-definite programming needs to be written in the symbolic form.
6. [ ] 6. Compute the Newton polytope. This requires using qhull, to determine which monomial is in the Newton polytope.

[RussTedrake]

Can we flip your first few lines into, e.g.,

auto x = prog.AddIndeterminate(2);

and presumably, we could have also done

auto dynamics = (matrix expression using x)

but I like that we can use the initializer lists, too.

The "AddSumsOfSquaresPolynomial" is consistent with our other "AddVariable" naming, but feels confusing here (seems like you are adding a constraint). And it's not actually adding anything to the mathematical program, is it? Maybe "MakeSumsOfSquaresPolynomial"? or "Create..."...?

[RussTedrake]

also, could AddSumsOfSquaresPolynomial and Jacobian have taken x? (instead of {x0,x1})?

[RussTedrake]

The "AddSumsOfSquaresPolynomial" is consistent with our other "AddVariable" naming, but feels confusing here (seems like you are adding a constraint). And it's not actually adding anything to the mathematical program, is it? Maybe "MakeSumsOfSquaresPolynomial"? or "Create..."...?

scratch that. after thinking a little more, I see that you ARE adding the SOS constraint to the program. got it.

[hongkai-dai]

@RussTedrake . I do not think we should have a function AddIndeterminate in MathematicalProgram. Indeterminate is not a decision variable, nor a constraint, so conceptually it does not have to be part of MathematicalProgram. In spotless we have the newIndeterminate function because spotless makes not distinction between indeterminates and decision variables, both are of type msspoly, so the program needs a function to register some msspoly objects as indeterminates, while the rest are decision variables. Now we have two classes symbolic::Indeterminate and DecisionVariableScalar, so I do not think we need a function AddIndeterinate to differentiate indeterminates and decision variables.

[RussTedrake]

i agree that "AddIndeterminate" is the wrong name if the variable is not added to the mathematical program. I'd still like the creation to be as consistent as possible. For instance, could we at least have a IndeterminateVectorX, etc?

[soonho-tri]

@hongkai-dai , I've marked item 1 and 3 in the "proposed solution". BTW, is the proposed solution still up-to-date? I can work on item 2 and 4 if you still need them.

[hongkai-dai]

I think probably can work on item 4? I feel we are not very ready for item 2 yet. The pre-requisite for sum-of-squares optimization is to get semi-definite programming working, I still have not finished the symbolic form for semi-definite programming yet.

For item 4, I am imagining that we need a function Jacobian, to return the Jacobian of a symbolic expression, if that expression is a polynomial. So we might want two functions

// Returns the row vector as the Jacobian for the expression \p e w.r.t the variable \p vars. 
// @retval J  J(i) = ∂e / ∂vars(i)
// @pre{e is a polynomial, vars.cols() == 1}
template <typename Derived>
typename std::enable_if<std::is_same<Derived::Scalar, Variable>::value, 
                                     Eigen::Matrix<Expression, 1, Derived::RowsAtCompileTime>::type
Expression Jacobian(const Expression& e, const Eigen::MatrixBase<Derived> vars);

// Returns the Matrix as the Jacobian for a vector of expression \p e w.r.t the variable \p vars.
// @retval J J(i,j) = ∂e(i) / ∂vars(j)
// @pre{e(i) is a polynomial, vars.cols() == 1}
template <typename DerivedE, typename DerivedX>
typename std::enable_if<std::is_same<DerivedE::Scalar, Expression>::value &&
                        std::is_same<DerivedX::Scalar, Variable>::value, 
                        Eigen::Matrix<Expression, DerivedE::RowsAtCompileTime, DerivedX::RowsAtCompileTime>::type
Expression::Jacobian(const Eigen::MatrixBase<DerivedE>& e, const Eigen::MatrixBase<DerivedX>& vars);

What do you think?

[soonho-tri]

For item 4, I am imagining that we need a function Jacobian, to return the Jacobian of a symbolic expression, if that expression is a polynomial. So we might want two functions What do you think?

Sounds good to me. To implement this, we need symbolic (partial) derivation, right? I'll work on that next week.

[soonho-tri]

@RussTedrake , FYI, @hongkai-dai and I just had a meeting and updated our TODO list.

[moritzkuhne]

Hey all,

the (intermediate) goal is to write a function which matches coefficients of monomials, such that we can add polynomial constraints to the system. This is also an intermediate step in SOS decomposition.

While I am busy with working into the code, some parts made me wonder. I hope you can help me out with the interpretation.

1. symbolic::Indeterminate does not exist, but it's function is exactly what symbolic::Varaible does, correct?

2. I can not ask to get a whole bunch of symbolic::Varaible at once, but need to declare them all one by one and group them in symbolic::Varaibles, correct? Wouldn't it be more convenient to have an interface, which allows to generate X symbolic::Varaibles and stores them automatically in symbolic::Varaibles? The interface would be similar to VectorDecisionVariable<X>.

3. I don't have the feeling that polynomials are represented in a consistent way. Sometimes they are represented as MonomialToCoefficientMap, sometimes as symbolic::Expression, and sometimes as symbolic::Polynomial. Which one is preferred?

4. There exist multiple ways to convert one representation into the other, but this conversion can often not be reversed, e.g. an expression can be converted with symbolic::Expression::ToPolynomial() but from there it can't be reversed back. Why? Furthermore, once you converted an symbolic::Expression to symbolic::Polynomial, you can basically do not do anything with it as functions like symbolic::DecomposePolynomialIntoExpression(const symbolic::Expression& e, const Variables& vars) do not want symbolic::Polynomial.

5. The class symbolic::Polynomial has members of symbolic::Polynomial::Monomial, why not using symbolic::Monomials instead (which have already a lot of utility).

Last but not least:

6. I couldn't find a way to create a monomial with decision variables, as the decision variables are created with ID= 0 (dummy variable), which causes troubles. Is there a way to do so?

Lots of question, sorry for that. I will watch out they do not pile up the next time.

[hongkai-dai]

1. symbolic::Indeterminate does not exist, but it's function is exactly what symbolic::Varaible does, correct?

   That is right. We plan to use symbolic::Variable to represent indeterminates as well. The plan is to add a function

   template<int rows, int cols>
   Eigen::Matrix<symbolic::Variable, rows, cols> MathematicalProgram::NewIndeterminates<rows, cols>(const std::string& name);

   to add indeterminate to the program. The indeterminate is represented as symbolic::Variable. Inside the MathematicalProgram class, we will store the indeterminate as a private data

   class MathematicalProgram {
   private:
    VectorXDecisionVariable decision_variables_; // store the decision variables added by NewXXXVariable
    Eigen::Matrix<symbolic::Variable, Eigen::Dynamic, 1> indeterminates_; // store the indeterminates added by NewIndeterminates
   }

2. I can not ask to get a whole bunch of symbolic::Varaible at once, but need to declare them all one by one and group them in symbolic::Varaibles, correct? Wouldn't it be more convenient to have an interface, which allows to generate X symbolic::Varaibles and stores them automatically in symbolic::Varaibles? The interface would be similar to VectorDecisionVariable.

   You can define something like

   template<int rows, int cols>
   using MatrixIndeterminate = Eigen::Matrix<symbolic::Variable, rows, cols>

   as we did for decision variables in decision_variable.h.

3. I don't have the feeling that polynomials are represented in a consistent way. Sometimes they are represented as MonomialToCoefficientMap, sometimes as symbolic::Expression, and sometimes as symbolic::Polynomial. Which one is preferred?

   I do not know we have a symbolic::Polynomial class. Currently we use two approaches to represent a polynomial, as either a symbolic::Expression, or a MonomialToCoefficientMap. You can convert one to the other, and vice versa. Personally I prefer to use symbolic::Expression for the API exposed to the external users, and inside the function I use symbolic::MonomialToCoefficientMap. The reason why we did not have a symbolic::Polynomial, is that there exists a Polynomial class in drake/common, before we wrote the symbolic expression.

4. There exist multiple ways to convert one representation into the other, but this conversion can often not be reversed, e.g. an expression can be converted with symbolic::Expression::ToPolynomial() but from there it can't be reversed back. Why? Furthermore, once you converted an symbolic::Expression to symbolic::Polynomial, you can basically do not do anything with it as functions like symbolic::DecomposePolynomialIntoExpression(const symbolic::Expression& e, const Variables& vars) do not want symbolic::Polynomial.

   symbolic::Expression::ToPolynomial() does not return a symbolic::Polynomial. It returns a Polynomial<double> used for evaluating the polynomial at some double valued variable. I do not think we should use Polynomial<double> class in SOS, this class is mostly used for nonlinear optimization, where we want to evaluate a polynomial at certain value.

[moritzkuhne]

I do not know we have a symbolic::Polynomial class. Currently we use two approaches to represent a polynomial, as either a symbolic::Expression, or a MonomialToCoefficientMap. You can convert one to the other, and vice versa. Personally I prefer to use symbolic::Expression for the API exposed to the external users, and inside the function I use symbolic::MonomialToCoefficientMap. The reason why we did not have a symbolic::Polynomial, is that there exists a Polynomial class in drake/common, before we wrote the symbolic expression.

Oh true, there is no symbolic::Polynomial. Polynomial is not in the symbolic namespace.

[soonho-tri]

Closed by 4b10b69d42fb23264b2408712951a7081a3bf396.

[RussTedrake]

woohoo! ;-)

[soonho-tri]

@hongkai-dai , in #6675 , I added the following example in sos_constraint_test.cc which is what you wrote in the first comment of this issue.

  // Find the Lyapunov function V(x) for system:
  //
  //     ẋ₀ = -x₁ + 1.5x₀² - 0.5x₀³
  //     ẋ₁ = 3x₀ - x₁
  //
  // by solving sum-of-squared problems:
  //
  //     V(x) is sum-of-squares
  //    -V̇(x) is sum-of-squares
  VectorX<Variable> x = prog_.NewIndeterminates<2>();
  const auto& x0 = x(0);
  const auto& x1 = x(1);
 
  // Form the dynamics of the system.
  Vector2<symbolic::Polynomial> dynamics;
  // clang-format off
  dynamics << symbolic::Polynomial{-x1 + 1.5 * x0 * x0 - 0.5 * pow(x0, 3)},
              symbolic::Polynomial{3 * x0 - x1};
  // clang-format on
 
  // Adds V(x) as a 4th order sum-of-squares polynomial.
  const symbolic::Polynomial V{prog_.NewSosPolynomial({x0, x1}, 4).first};
 
  // Computes Vdot.
  const symbolic::Polynomial Vdot = V.Jacobian(x).transpose().dot(dynamics);
 
  // -Vdot is sum-of-squares.
  prog_.AddSosConstraint(-Vdot);
  const auto result = prog_.Solve();
  EXPECT_EQ(result, SolutionResult::kSolutionFound);

[jediofgever]

Hi, I came here from Underactuated course notes. It seems from the example code above that I can obtain a valid Vdot, but the ultimate goal for me is to use the acquired Vdot for deriving a controller, I will appreciate it if you can point me to any example/docu related to this. Thank you!

[hongkai-dai]

@jediofgever You can refer to the paper "Control Design along Trajectories with Sums of Squares Programming" on designing a controller and a Lyapunov function simultaneously.