Search code examples
drake

Quadratic cost using np linalg norm doesn't work

Why does this line:

    prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)

works.

But this:

  prog.AddCost(np.linalg.norm(q_variables - q_nominal)**2)

RuntimeError: Expression pow(sqrt((pow(q(0), 2) + pow(q(2), 2) + pow(q(4), 2) + pow(q(6), 2) + pow(q(7), 2) + pow(q(8), 2) + pow((-1 + q(5)), 2) + pow((-0.59999999999999998 + q(1)), 2) + pow((1.75 + q(3)), 2))), 2) is not a polynomial. ParseCost does not support non-polynomial expression.

does not?

Are the expressions not mathematically identical?

Solution

  • They are mathematically identical, but our symbolic engine is not yet powerful enough to recognize that sqrt(x)**2 should be simplified as x.

    You can also write the expression using the symbolic form

    prog.AddQuadraticCost((q-q0).dot(q-q0))

    if you prefer readable code.