I've been working on some NLP problems in CasADi, but I was looking at maybe coming back and working with Drake again.
One thing I was wondering is if there was a way to visualize the jacobian of the constraints wrt to the optimization variables in Drake.
In CasADi I can do something like this:
jac = ca.jacobian(constraints,opt_variables)
plt.figure(figsize=(10,10),dpi=100)
plt.spy(jac.sparsity(),markersize=0.5)
And get an output like this: Image of the jacobian sparity matrix
I'm curious if there is a way to do something similar in Drake?
Another related question is how to check the constraint/variable orders, or if there are any tips with that.
Here I attached the jacobian of the same problem but just with a different ordering of constraints and variables: Jacobian with rearranged variables
The order/gradient matrix shape seems to make a huge difference for ipopt, the first diagonalized example has a solve time of around 0.5 seconds while the second one has almost a 50-100 seconds solve time.
You can get the Jacobian of a constraint through this
x_ad = pydrake.math.InitializeAutoDiff(x)
y_ad = constraint.Eval(x_ad)
jac = pydrake.math.ExtractGradient(y_ad)
plt.figure(figsize=(10,10),dpi=100)
plt.spy(jac.sparsity(),markersize=0.5)
As a follow on, is there any easy way to change the order of constraints in pydrake, or do you just have to define them in the order you want them to show up in the jacobian
When you call MathematicalProgram::AddConstraint() function, you should add them in the order you want them to show up in the jacobian.
Depending on which solver you use (IPOPT/SNOPT), inside each solver, we group the constraints based on their types. Namely we first add all the generic nonlinear constraints, and then the quadratic constraints, and lorentz cone constraints, etc. So if you add the constraint in this order: generic_constraint0, quadratic constraint0, lorentz_cone_constraint0, generic_constraint1, lorentz_cone_constraint1, then in the IpoptSolver, the constraints are ordered as
generic_constraint0
generic_constraint1
quadratic_constraint0
lorentz_cone_constraint0
lorentz_cone_constraint1