Unable to check infeasible constraints when using autodiff in PyDrake

January 14, 2022

I am solving a problem in PyDrake with SNOPT and I get solutions that look reasonable, but when I do result.is_success() it comes back with False, so I am hoping to investigate why it thinks the problem wasn’t solved. I assume I have a bad constraint somewhere, so I’m doing this with the following code:

result = mp.Solve(prog)
if not result.is_success():
    print("INFEASIBLE:")
    infeasible = result.GetInfeasibleConstraints(prog)
    for c in infeasible:
        print(c)

However, it exits on the call to result.GetInfeasibleConstraints(prog) with the following error:

Traceback (most recent call last):
  File "test_drake_distribution.py", line 250, in <module>
    infeasible = result.GetInfeasibleConstraints(prog)
  File "/home/adam/.miniconda3/envs/drake/lib/python3.6/site-packages/pydrake/solvers/_mathematicalprogram_extra.py", line 34, in _check_array_type
    f"{var_name} must be of scalar type {expected_name}, but unable "
RuntimeError: PyFunctionConstraint: Output must be of scalar type float, but unable to infer scalar type.

What it says is true, because my constraint functions are utilizing the autodiff functionality in Drake, so they are returning arrays with dtype=AutoDiffXd. If this is the case, does that mean I cannot use the constraint infeasibility checker? Any advice on being able to check infeasible constraints when I’m using autodiff?

>Solution :

I suppose you write your constraint using a python function. I would suggest to write this python function to handle both float and autodiffxd, so something like this

def my_evaluator(x: np.ndarray):
    if x.dtype == np.object:
        # This is the autodiff case
    elif x.dtype == np.float:
        # This is the float case