drake
drake copied to clipboard
Jupyter Kernel Dying when using AddSosConstraint
Consider the following:
from pydrake.all import MathematicalProgram, Polynomial, Solve
import pydrake.symbolic as sym
import numpy as np
prog = MathematicalProgram()
x, u = prog.NewIndeterminates(1, "x"), prog.NewIndeterminates(1, "u")
V_sym = sym.Polynomial(u[0]**4 - 2*x[0]*u[0])
prog.AddSosConstraint(V_sym)
result = Solve(prog)
print(result.is_success())
If this cell is run in a Jupyter notebook, the kernel simply dies and gives no error message.
When run as a standalone python script, the terminal prints out "Constraint 2 is empty"
There appears to be some sort of parsing error in the AddSosConstraint
I can reproduce the problem. The CSDP library calls the C exit()
function.
#0 __GI_exit (status=206) at exit.c:138
#1 0x00007ffff0c14c3e in sort_entries (k=2, C=..., constraints=0x12683e0) at external/csdp/lib/sortentries.c:92
#2 0x00007ffff0c02866 in easy_sdp (params_pathname=0x7fffffff9d88 "", n=1, k=2, C=..., a=0x12683c0, constraints=0x12683e0, constant_offset=-0, pX=0x7fffffff9aa8, py=0x7fffffff9a90, pZ=0x7fffffff9a98, ppobj=0x7fffffff9a78, pdobj=0x7fffffff9a70) at external/csdp/lib/easysdp.c:452
#3 0x00007ffff0c51e3b in drake::solvers::(anonymous namespace)::SolveProgramWithNoFreeVariables (...) at solvers/csdp_solver.cc:137
https://github.com/coin-or/Csdp/blob/0dcf187a159c365b6d4e4e0ed5849f7b706da408/lib/sortentries.c#L92
Obviously, we'll need to patch CSDP to report errors a different way, instead of quitting the entire process.
I haven't yet looked into whether the program / constraints are malformed or not.
Can confirm that using Mosek to solve the program runs without crashing and returns that the program is infeasible.
Mathematically here is what happens:
- since V_sym = x[0]**4 - 2 * x[0] * u[0], by calling AddSosConstraint(V_sym), we first compute the monomial basis for V_sym, which is x[0]**2. Hence we end up with a constraint
x[0]**4 - 2 * x[0]*u[0] = a * x[0]**4
a >= 0
- By comparing the coefficients of
x[0]**4 - 2 * x[0] * u[0]
anda * x[0]**4
, we end up with the constraint1 == a
-2 == 0
Note that the constraint -2 == 0
is a weird infeasible constraint. In CSDP it just says this constraint is empty and throws an error. In Mosek it reports infeasibility.
One solution could be that when we impose the linear equality constraint, we check if we have this kind of weird constraint -2 == 0
and throws an error directly.
Obviously, we'll need to patch CSDP to report errors a different way, instead of quitting the entire process.
==> #16736 for remove the exit()
calls.
We should still change MathematicalProgram
or CsdpSolver
to reject this program with a good error message, instead of feeding into CSDP.
@hongkai-dai it seems like the two problems seen in this particular report have been resolved: exit vs exception in #16736, and always-false constraint in #16735.
Is there anything remaining to do here, or shall we close it?
From a recent StackOverflow post, it seems like we still might need better SOS error checking, but in any case I don't think there's anything actionable in this report anymore? WDYT?
I agree the problem is resolved here.
Still curious why the stackoverflow post mentions about empty constraint, but we should resolve it in a different issue.
As suggested by Prof. Tedrake, I am posting an issue with CSDP solver on a deepnote notebook encountered when implementing a SOS matrix search problem. CSDP solver gives error message "the CSDP library exited via a fatal exception" when certain entries in the matrix is implemented as a free polynomial. The issue does not appear when all entries of the matrix are defined as SOS polynomials.
A minimal example that illustrates the issue can be found in the following shared deepnote notebook. Any help or insight why this issue is happening would be much appreciated: https://deepnote.com/workspace/tangsun-3dfd38e4-052c-4f2f-8bcb-175b9ae1136c/project/Matrix-SOS-b2f18769-ce51-402c-8a75-43278829a1cc/%2Fnotebook.ipynb
Here is the slightly simplified version of the example:
from pydrake.all import (MathematicalProgram, Variables,
Solve)
prog0 = MathematicalProgram()
x = prog0.NewIndeterminates(1, 'x')
y = prog0.NewIndeterminates(1, 'y')
l_deg = 2
l_11 = prog0.NewFreePolynomial(Variables(x), l_deg).ToExpression()
M = l_11
V = y.dot(M).dot(y)
prog0.AddSosConstraint(V)
result = Solve(prog0)
Follow-up: Using Mosek in a separate notebook can resolve this issue.
Thanks for reporting this with the reproducible code. I believe the problem is in drake's code in sdpa_free_format.cc file. With the default argument of drake::solvers::CsdpSolver it generates the wrong SDPA file. I will look more closely tomorrow.
In the mean time, it is better to use Mosek for this problem.
I just looked into this example code
from pydrake.all import (MathematicalProgram, Variables,
Solve)
prog0 = MathematicalProgram()
x = prog0.NewIndeterminates(1, 'x')
y = prog0.NewIndeterminates(1, 'y')
l_deg = 2
l_11 = prog0.NewFreePolynomial(Variables(x), l_deg).ToExpression()
M = l_11
V = y.dot(M).dot(y)
prog0.AddSosConstraint(V)
result = Solve(prog0)
And CSDP throws an error
Problem must have at least one constraint.
Mathematically the solver is not lying, the optimization problem we hand to CSDP doesn't have a constraint. Drake simplifies the sum-of-squares problem above to an optimization problem with only a psd cone constraint without any linear equality/inequality constraints.
In our CsdpSolver class, in its class constructor https://github.com/RobotLocomotion/drake/blob/489e3b2a6f997a5848b0a44d79e4f7d9370d9e37/solvers/csdp_solver.h#L53-L54, we have an optional input argument RemoveFreeVariableMethod method
. The default method is nullspace approach, which would simplify this particular optimization problem such that the resultant problem doesn't have inequality/equality constraints, causing CSDP to fail. The non-default method, such as kTwoSlackVariable
and kLorentzConeSlack
won't simplify the problem, and CSDP can work.
So here is my proposal
- Remove the input argument
method
from CsdpSolver constructor. So CsdpSolver constructor should take no input argument, same as other solvers in Drake. - The user can set
RemoveFreeVariableMethod
through SolverOptions, for example
and the default option value issolver_options.SetOption(CsdpSolver::id(), "RemoveFreeVariableMethod", "TwoSlackVariables");
kLorentzConeSlack
instead of the current value (kNullspace which causes CSDP to fail).
@jwnimmer-tri what do you think? This will break our current API, but I think it is more consistent with other solvers.
I fully support moving CsdpSolver
constructor argument to a SolverOption. The constructor arguments are typically inaccessable to users.
This will break our current API ...
I think we can do it with deprecation. The constructor argument stays for now (but is deprecated), and forms the implied default setting for the SolverOption when the option is not given by the user. If the user gives the option, it always overrides the constructor argument. Later, we can drop the constructor argument once deprecation expires.
For the option name, I want to be careful to distinguish drake-specific options from the solver's native options. Maybe we could name it drake:RemoveFreeVariableMethod
or similar.
Or course, best would be for the solver to give the right answer without any option -- if it could understand the program and decide which approach to take, autonomously. That might be too difficult to implement, though. In the meantime, SolverOption is strictly better than constructor argument.
fwiw, I just hit this again (I think), with the following reproduction:
import numpy as np
from pydrake.all import MathematicalProgram, Variables, Solve
prog = MathematicalProgram()
# SOS indeterminates
x = prog.NewIndeterminates(2, 'x')
# Lyapunov function
P = np.array([[ 1.5, -0.5], [-0.5, 1.]])
f = lambda x: [- x[1], x[0] + (x[0]**2 - 1) * x[1]]
V = x.dot(P).dot(x)
V_dot = 2*x.dot(P).dot(f(x))
# SOS Lagrange multipliers
l_deg = 4
l = prog.NewSosPolynomial(Variables(x), l_deg)[0].ToExpression()
# level set as optimization variable
rho = prog.NewContinuousVariables(1, 'rho')[0]
result = Solve(prog)
Running this at the command line gives
WARNING:drake:The problem has free variables, and CSDP removes the free variables by computing the null space of linear constraint in the dual space. This step can be time consuming. Consider providing a lower and/or upper bound for each decision variable.
zsh: segmentation fault
~~@RussTedrake Sorry I have problem to reproduce the error. The code runs fine on my machine, and result.is_success() = True
. I am on the master revision 7ece1fbb62cabaad854da8bc0c8971756d68686b~~
My bad, I turned on Mosek when I compiled pydrake by default. With Mosek off I can reproduce the error.
This issue should be resolved by #17728.
This issue should be resolved by https://github.com/RobotLocomotion/drake/pull/17728.
That pull request was closed, remaking "Close this PR, will fix it through a different approach."
Is this issue fixed now, somewhere else?