Strandbeest example with SAP constraints is broken

[vincekurtz]

What happened?

I went to try @joemasterjohn's Strandbeest example with SAP constraints, bazel run //examples/multibody/strandbeest:run_with_motor, but found this:

The continuous time/bushing version works fine. A bit of digging indicates that the problem is with the inverse kinematics solve that sets the initial condition. The IK solve fails for the discrete-time plant, leading to an initial condition with some of the links under the ground slab:

Setting slightly more verbose options for the IK solve (solver_options.SetOption(IpoptSolver::id(), "print_level", 5);) reveals the following error message:

This is Ipopt version drake_vendor, running with linear solver spral.

Too few degrees of freedom (n_x = 72, n_c = 145).
  Trying fixed_variable_treatment = RELAX_BOUNDS

Number of nonzeros in equality constraint Jacobian...:    11524
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:        0

Exception of type: TOO_FEW_DOF in file "bazel-out/k8-opt/bin/external/+internal_repositories+ipopt_internal/drake_src/Interfaces/IpIpoptApplication.cpp" at line 662:
 Exception message: status != TOO_FEW_DEGREES_OF_FREEDOM evaluated false: Too few degrees of freedom (rethrown)!

EXIT: Problem has too few degrees of freedom

Again, this error is only thrown with the SAP constrained version. The IK solve is successful with --mbt_dt=0 --with_constraints=false.

Version

No response

What operating system are you using?

No response

What installation option are you using?

No response

Relevant log output

[joemasterjohn]

Thanks @vincekurtz! This seems to be a regression due to some change in InverseKinematics. The example using discrete-time plant and SAP constraints runs fine as of #21970 (the last time the strandbeest directory was touched).

At that commit, the Ipopt output is:

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

Number of nonzeros in equality constraint Jacobian...:     2592
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:        0

Total number of variables............................:       72
                     variables with only lower bounds:        0
                variables with lower and upper bounds:       60
                     variables with only upper bounds:        0
Total number of equality constraints.................:       37
Total number of inequality constraints...............:        0
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:        0

At first I thought it might be caused by the default linear_solver for the IpoptSolver being changed (#22385). However, modifying the example to make it use mumps still fails. From the output, it looks like the number of constraints jumped from 37 to 145. That might be a clue, but further bisection to find the regression is needed.

[joemasterjohn]

OK found it, the regression is caused by: a466e14

I'll have to dig deeper into what changed when AddMultibodyConstraints() was hooked up in order to figure out the fix.

[joemasterjohn]

The following patch is a workaround to get the example to work, and probably not what we want long term.

diff --git examples/multibody/strandbeest/run_with_motor.cc examples/multibody/strandbeest/run_with_motor.cc
index 3eff10f77e..931cd251e2 100644
--- examples/multibody/strandbeest/run_with_motor.cc
+++ examples/multibody/strandbeest/run_with_motor.cc
@@ -205,6 +205,10 @@ int do_main() {
   // without it parsing the default position limits as constraints.
   InverseKinematics ik(strandbeest, &strandbeest_context, false);
 
+  for (auto c : ik.prog().GetAllConstraints()) {
+    ik.get_mutable_prog()->RemoveConstraint(c);
+  }
+
   // Add our custom position constraints that fix the floating body (crossbar).
   ik.get_mutable_prog()->AddBoundingBoxConstraint(lower, upper, ik.q());
 

AddMultibodyConstraints() adds position constraints for the MbP BallConstraints, 3 constraint equations for each BallConstraint. But since the mechanisms are planar by constuction, one of those equations is redundant. This leads to an overconstrained problem and Ipopt fails silently. I used (abused?) point/point distance constraints with 0 distance and added them manually to the IK program in the example originally.

I wonder, maybe AddMultibodyConstraints() should be checking if the translational velocity Jacobian it computes is rank deficient and reformulating as necessary? Or maybe just a solver option needs to be changed to handle the overconstrained problem. I don't know enough about the solvers to tell. What do you think @RussTedrake?

[jwnimmer-tri]

This leads to an overconstrained problem and Ipopt fails silently.

Whatever else is going on, "failing silently" doesn't seem like a good thing.

Later... ah, it's probably not slient. The call to solvers::Solve in the example program returns a MathematicalProgramResult which is being ignored. Hopefully that result would tell if the program failed, at which point we should probably halt, or at least warn the user.

[joemasterjohn]

You're absolutely right, I misspoke. It's a defect of the example to ignore the solver result and do nothing. I tested and the result reports failure (although IpoptSolverDetails::ConvertStatusToString() returns "Unassigned"). I should update the example regardless.