Evaluate MathematicalProgram based on initial guess

[richardrl]

Is your feature request related to a problem? Please describe. Suppose I have written my MathematicalProgram and solved it. But the solution is wrong - indicating a bug. Now, I want to plug the solution back in, step through my constraints / expressions line by line, see what each expression in the program is is based on the plugged-in solution. This is an extremely laborious task to do this right now, and it could really speed up debugging if there was an easier way to do this.

What I mean is it is really hard to evaluate all the numerical values of all expressions in a MathematicalProgram given an initial guess. Note, I mean any expression, not just the constraints or costs.

Ideally, it should be as easy as sticking import pdb, pdb.set_trace at any line when constructing the program, and running Evaluate - but this does not work, as I will explain below.

Scenario 1: If all your variables are setup at once at the beginning of your mathematical program, perhaps you can do this:

1. Solve your program, serialize the solution.
2. Re-solve your program, but this time set your previous solution as an initial guess.
3. Use pdb to set breakpoints as desired at any line in your mathematical program.
4. Once you are in pdb, you can do this: env_dict = {k:v for k,v in zip(prog.decision_variables(), prog.GetInitialGuess(prog.decision_variables()))}

and Evaluate(target_expression, env_dict)

Scenario 2: However, if you do not setup all your decision variables at the beginning, this doesn't work. If you incrementally add decision variables, the size of the decision variable vector at the beginning of the program is different from result.GetSolution from your previous run. For example, suppose you have a helper function that reformulates absolute value constraints, adding the necessary slack variables. Having to track and initialize each of these slack variables during debugging is way harder than just setting all the decision variables at once with prog.GetInitialGuess(prog.decision_variables()).

There is no easy way I can see to quickly evaluate any arbitrary expression in your mathematical program given an initialguess.

I would like debugging MathematicalProgram given an initialguess to be as easy as setting as breakpoint and running pdb, but right now it seems you have to setup an elaborate data structure to track all your decision variables and call Evaluate. It becomes really complicated and tedious to do this if we have some more complex logic for incrementally setting up decision variables.

Finally, another way to handle this is: we can setup a data structure to track all the expressions we are interested in, then after solving the program the first time, we can Evaluate these constraints. But setting up this data structure each time is tedious and it would be better if we could use the debugger.

Describe the solution you'd like This issue would be quickly resolved we could serialize and load everything, with built-in Drake functions, that goes into env_dict (the decision variables and values of decision variables). But we cannot serialize prog.decision_variables() right now.

Perhaps we need an alternative way of running Evaluate that does not use the variables as the keys, but strings or hashes. Then we need a built-in Drake function to dump and load this dictionary of strings to variable values.

That way we can serialize everything in env_dict for a given MathematicalProgram, then load it in a second run and quickly debug any line in MathematicalProgram using steps 3-4 stated above.

[hongkai-dai]

Why you need to serialize the solution? Why not to add a break point where you call Solve(prog) function (maybe a conditional breakpoint with not result.is_success(). Then when pdb is invoked, you have all your decision variables, and the value of your decision variables, it is easy to construct the symbolic environment.

[jwnimmer-tri]

Alternatively, if you have env_dict but it has too many variables in it (i.e., you need to adjust it for an earlier program that hasn't had everything added yet), it doesn't seem that hard to either adjust it yourself (removing things) or serialize it yourself (to strings and doubles).

It sounds like in your case every Variable has a unique name. In that case you can convert env_dict to be keyed on str instead of Variable (by calling get_name() on the variables). That Dict[str, float] would be easy to serialize (e.g., using pickle). Then you could write a function that did Evaluate(expr) but instead of taking an Environment it would take that dict and use the unique string names to build an Environment for the given expr.

[jwnimmer-tri]

Would it help if we amended these two functions:

• https://drake.mit.edu/pydrake/pydrake.symbolic.html#pydrake.symbolic.Evaluate
• https://drake.mit.edu/pydrake/pydrake.symbolic.html#pydrake.symbolic.Expression.Evaluate

to accept the var->value mapping in two ways:

• env: Dict[pydrake.symbolic.Variable, float]
• str_env: Dict[str, float]

For example, the user could call this way:

x = Variable("x")
y = Variable("y")
expr = x + y
expr.Evaluate(env={x: 2}, env_str={"y": 3})

... results in 5.

[RussTedrake]

There is no easy way I can see to quickly evaluate any arbitrary expression in your mathematical program given an initialguess.

Sorry I'm late to the party. Is there a reason that MathematicalProgram::EvalBindingAtInitialGuess isn't sufficient? (btw -- the missing python for this method is being added in #19632)

[jwnimmer-tri]

Because of this part of the request:

Note, I mean any expression, not just the constraints or costs.

[RussTedrake]

I feel that a Binding is the primary conceptual atom that links variables to evaluators. I think it's reasonable to say that if you have an expression that needs to be evaluated, then create a binding for it, and call EvalBindingAtInitialGuess?

[richardrl]

Why you need to serialize the solution? Why not to add a break point where you call Solve(prog) function (maybe a conditional breakpoint with not result.is_success(). Then when pdb is invoked, you have all your decision variables, and the value of your decision variables, it is easy to construct the symbolic environment.

But how do you have a get a reference to the expression you are interested in? Here's an example: https://gist.github.com/richardrl/02bd5907a3dbb6eba1b5755a97de664e#file-debugging_example-py-L44

Suppose I want to know what net_torque_to_i_from_j is when i = 3, j = 1. In order to get a reference to this, I need to create a dictionary that stores every expression I am interested in, name it appropriately, so I can get a reference to this expression after setting up the complete MathematicalProgram. This is the paradigm I was describing here:

Finally, another way to handle this is: we can setup a data structure to track all the expressions we are interested in, then after solving the program the first time, we can Evaluate these constraints. But setting up this data structure each time is tedious and it would be better if we could use the debugger.

Setting up that data structure throughout your code every time you are interested in an expression is tedious

[richardrl]

Alternatively, if you have env_dict but it has too many variables in it (i.e., you need to adjust it for an earlier program that hasn't had everything added yet), it doesn't seem that hard to either adjust it yourself (removing things) or serialize it yourself (to strings and doubles).

It gets really complicated when you have nested for loops or similar complex logic: https://gist.github.com/richardrl/02bd5907a3dbb6eba1b5755a97de664e#file-debugging_example-py-L44

Now anytime you want to debug things you have to write a helper function to filter out the dictionary - and this helper function will only be specific to that exact line in your program. It would be much more efficient if we could step through the program debugger style.

[richardrl]

Would it help if we amended these two functions:

• https://drake.mit.edu/pydrake/pydrake.symbolic.html#pydrake.symbolic.Evaluate
• https://drake.mit.edu/pydrake/pydrake.symbolic.html#pydrake.symbolic.Expression.Evaluate

to accept the var->value mapping in two ways:

• env: Dict[pydrake.symbolic.Variable, float]
• str_env: Dict[str, float]

For example, the user could call this way:

x = Variable("x")
y = Variable("y")
expr = x + y
expr.Evaluate(env={x: 2}, env_str={"y": 3})

... results in 5.

I think this would work, and your suggestion above this of using strings to access variables. If there is a way to: 1) save a dictionary of variable names to variable values after setting up the MP, and 2) Evaluate an expression given this dictionary on a second run, while the MP is only partially-built - that would work. I will try it.

This strategy will still require passing your dictionary of strings to variable values into any function you want to debug though... Perhaps an even more user-friendly solution would be if you could set "additional" variables in your initial guess at the start of your MP, even beyond the size of any initialized variables, then you can call EvalBindingAtInitialGuess which will use the relevant subset of those initial guess variables at any point in the middle of constructing your MP. But I don't know enough about what's going on internally to really say how to best implement such a feature (being able to eval expressions in the middle of MP construction inside the debugger).

Also, just a note that this discussion reminds me of Eager Mode vs Graph Mode in Pytorch vs old Tensorflow. Eager Mode is very much desired for many people who use these libraries for quick debugging as you can step into any line in your Pytorch code and see the output, whereas in graph mode you need to track which variables to plugin to some graph expression.

[richardrl]

I feel that a Binding is the primary conceptual atom that links variables to evaluators. I think it's reasonable to say that if you have an expression that needs to be evaluated, then create a binding for it, and call EvalBindingAtInitialGuess?

Ah I'm actually not sure how to create a binding out of an arbitrary expression... but also, even if you could do this, you would create bindings for lots of expressions, figure out how to name them. I wonder if that would end up adding too much boilerplate throughout the code, while still perhaps being less user-friendly than PDB.

I guess it still seems tough for the example I gave above, or any example where you have lots of expressions you are interested in: https://gist.github.com/richardrl/02bd5907a3dbb6eba1b5755a97de664e#file-debugging_example-py-L44

[richardrl]

Update: @jwnimmer-tri 's suggestion seems to be working quite well! I think this is a pretty good stop-gap solution - besides the need to ensure variable names are unique, which might be hard for a large program, it seems pretty good. Much much faster to debug using this methodology and pdb. Thanks everyone for your input.

def evaluate_expr_from_string_dict(expr, prog, string_dict):
    # evaluates the expression given a dict mapping string (variable names) to values
    env_dict = dict()
    for v in prog.decision_variables():
        env_dict[v] = string_dict[v.get_name()]

    return Evaluate(expr, env_dict)

[richardrl]

Just an update here: having to name every single variable with a unique name starts to be quite tedious still... for example if you are doing extensive McCormick envelopes throughout your code, or using Hongkai's SO(3) generator, the method of using "get_name" starts to not work (if you can't set unique names e.g. in the SO(3) code) or is highly tedious. It would really nice if we could easily serialize the variables and load them anew without having to specify unique names.

[richardrl]

Just an update on this thread: I have been playing around with both strategies for some time. Here's example of ExpressionCost method, as described by @RussTedrake:

 for i in range(config.num_internal_bodies):
        for j in range(i):
            # witness_i_Bo_W_var[idx] = rotation_multiply_vec_mccormick(rot_mat_var[i], witness_i_Bo_B_var[idx], object_xyz_ranges[i], prog, status="debug", string_dict=loaded['string_dict'])
            # witness_i_Bo_W_var[idx] = rotation_multiply_vec_mccormick(rot_mat_var[i], witness_i_Bo_B_var[idx], object_xyz_ranges[i], prog, status="debug", string_dict=loaded['string_dict'])
            witness_i_Bo_W_var[idx] = rotation_multiply_vec_mccormick(rot_mat_var[i], witness_i_Bo_B_var[idx], object_xyz_over_dims_ranges[i], prog)

            witness_j_Bo_W_var[idx] = rotation_multiply_vec_mccormick(rot_mat_var[j], witness_j_Bo_B_var[idx], object_xyz_over_dims_ranges[j], prog)

            binding_dict[f"ln240 i: {i} j: {j} witness_i_Bo_W_var: {idx}"] = create_binding(witness_i_Bo_W_var[idx])

            binding_dict[f"ln241 i: {i} j: {j} witness_j_Bo_W_var: {idx}"] = create_binding(witness_j_Bo_W_var[idx])

            idx += 1

def create_binding(vec_expr):
    out = []
    for scalar_expr in vec_expr:
        out.append(Binding[ExpressionCost](ExpressionCost(scalar_expr), np.array(list(scalar_expr.GetVariables()))))
    return np.array(out)

So I create a binding_dict, populate it with expression costs, then print out the binding dict after setting up the program. In my view this is very difficult compared to the second method based on using the string dict (detailed next), for the same reason debugging by print statements versus debugger is difficult; the latter is much more mentally taxing, requires writing and deleting statements into your code, and then scanning through a huge binding_dict print-out instead of being able to step through things with a debugger.

Method 2: the string dict method as proposed by @jwnimmer-tri is much nicer. I can save a dictionary mapping variable string names to variable values, then step through the MathematicalProgram easily using my debugger PDB.

def evaluate_expr_from_string_dict(expr, prog, string_dict):
    # evaluates the expression given a dict mapping string (variable names) to values
    env_dict = dict()
    for v in prog.decision_variables():
        env_dict[v] = string_dict[v.get_name()]
    return Evaluate(expr, env_dict)

The downside of this second method is keeping the names unique is hard. It is hard for code you control, but impossible (at least from Python) for stuff like this and this i.e. built-in Drake functions that create some variables, without giving you control of the naming.

In my view it would be best if there was some way to do this string_dict method that automatically handles mapping the variables to values, without needing to ensure unique variable names. Would it be difficult to enable that somehow?

[RussTedrake]

Sorry... shouldn't the dictionary go from Variable, not the variable string name, to value? Variables have an Id element that is guaranteed to be unique. And that should be what the Evaluate method is looking for?

[richardrl]

@RussTedrake The reason I didn't use Variable is because Variable is not serializable. I am solving the program, serializing the variable results with the string->value dictionary and loading it again.

I think if I serialized the ID->value instead, I'm not sure I could load the program a second time and map from the ID to the Variable?

[hongkai-dai]

The reason I didn't use Variable is because Variable is not serializable

The variable ID is an integer, which is serializable. You can store the mapping from the ID to the double value. If you reconstruct the MathematicalProgram again with the same code, the same variable will have the same ID as the previous call, so you can load the variable value.

[hongkai-dai]

The variable ID is assigned based on the order of creating each variable. At the beginning of the process the first variable ID is set to 0. So if your variables are created using the same code (which I think you did because you are debugging the same problem), and your code doesn't contain multi-thread (which can introduce randomness in the order of creating variables), then you can assume that the second call to your process construct symbolic::Variable with the same ID as the previous call.

[jwnimmer-tri]

To help focus the discussion -- the main sticking point here is the manual (non-Drake) "serialization" of decision variables, where the representation chosen was not up to the task.

In my view it would be best if there was some way to do this string_dict method that automatically handles mapping the variables to values, without needing to ensure unique variable names. Would it be difficult to enable that somehow?

We need to uniquely identify variables somehow when de/re-serializing. The question is what information to use.

I don't support Hongkai's idea of "So if your variables are created using the same code ... the same ID as the previous call." It's quite easy to for that kind of assumption to fail, when interactively debugging.

Instead, we should ask -- in your situation, how would you disambiguate two variables with the same name, when you think about a program? Would you use the relative order they were created in ("the first q, the second q")? Would you use the name of the constraint they participate in ("the q from the initial_positionconstraint; the q from the final_orientation_constraint")? Would you use the index in the program ("the i'th decision variable")?

Or would you reject the premise entirely -- deciding instead that your variables should never have the same same? In that case, you'd need to add an optional "name_hint" or similar to the helper functions you linked, so that the user can control the names.