RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.18k stars 1.24k forks source link

Strange handling of constraints with IPOPT. #21485

Closed jorgenin closed 1 month ago

jorgenin commented 1 month ago

What happened?

I was playing around with the mathematical program in both Casadi and Drake and I found some weird issues how linear constraints are evaluated.

In particular it seems like putting a matrix into a linear constraint many non-zeros to appear in the Jacobian. This greatly slows down the ipopt solver.

As a motivating example I've added a quick test file that compares two approaches in drake and then also an implementation in casadi. drake_ipopt_issue.py.zip

The first version added the program constraints as such:

    prog.AddLinearConstraint(-max_val - controls ,np.ones_like(controls)*-np.inf, -DATA)
    prog.AddLinearConstraint(eq(integral_val[1:],( integral_val[:-1] + controls*.25)))

Looking at the solver output this generated:

...
This is Ipopt version 3.14.16, running with linear solver MUMPS 5.4.1.

Number of nonzeros in equality constraint Jacobian...:    80203
Number of nonzeros in inequality constraint Jacobian.:    40200
...

When we change those lines to use a loop instead as such:

  for i in range(N):
        prog.AddLinearConstraint(integral_val[i+1]==integral_val[i] + controls[i]*.25)
        prog.AddLinearConstraint(-max_val- controls[i] ,[-np.inf], [-DATA[i]])

We get instead:

Number of nonzeros in equality constraint Jacobian...:      603
Number of nonzeros in inequality constraint Jacobian.:      400

Casadi has the same number:

Number of nonzeros in equality constraint Jacobian...:      603
Number of nonzeros in inequality constraint Jacobian.:      400

Looking at solve times we see that the large number of nonzero greatly affect the solve time (though casadi is faster than either approach still by large margin)

Solve time First Program: 0.7652149200439453 Solve time Fixed Version: 0.11430883407592773 Casadi Solve Time: 0.012894868850708008

I saw a similar (though slightly different) issue mentioned in #18292. I took the liberty in using their values for IPOPT settings.

Version

No response

What operating system are you using?

macOS 14 (Sonoma)

What installation option are you using?

pip install drake

Relevant log output

No response

hongkai-dai commented 1 month ago

Thanks @jorgenin for reporting the issue.

This is a defect of Drake's IpoptSolver. For a matrix of linear constraints, we treated all entries in the coefficient matrix as non-zero values. I will work on a fix PR to handle the sparsity in the coefficient matrix.

RussTedrake commented 1 month ago

Related to #18292.