Search code examples
drake

Evaluate a symbolic expression inside a MathematicalProgram constraint


I want to use a symbolic expression as a MathematicalProgram constraint but am unsure how to achieve this. My best go so far is the following (simplified example):

x = Variable("x")
expression = x**2

prog = MathematicalProgram()
v = prog.NewContinuousVariables(1)
prog.AddConstraint(
    lambda a: Evaluate(np.array([expression]), {x: a[0].value()}),
    lb=np.array([0.0]),
    ub=np.array([0.0]),
    vars=v,
)
result = Solve(prog)

I'm getting the error PyFunctionConstraint: Output must be of scalar type AutoDiffXd. Got float instead.. Using lambda a: Evaluate(np.array([expression]), {x: a[0]}) does not work due to incompatible function arguments.

I'd highly appreciate any help with this.


Solution

  • @Hongkai Dai's answer with the ExpressionConstraint in C++ led me in the right direction. There is such a constraint in pydrake (see here). However, it currently does not support array inputs. The second required insight was that it is possible to use prog.NewContinuousVariables in symbolic expression operations (e.g. Jacobian).

    Using these insights, I solved my problem with something similar to the following:

    prog = MathematicalProgram()
    x = prog.NewContinuousVariables(2)
    expression = x[0]**2
    J = expression.Jacobian([x[0]])
    for i in range(2):
      prog.AddConstraint(J[i], 0.0, 0.0)
    result = Solve(prog)