CODEWITHSUNDEEP
drake
Noah Wechtler
Noah Wechtler

Reputation: 81

Troubleshooting optimization for finding a robot pose in equilibrium

I am trying to write a program which given some cost and constraints finds a combination of states and torques which will result in a fixed point. The script can be found in this Colab.

For a bit more background, I am trying to balance a humanoid lower body with contact (1 point contact at each foot). So I use Drake's position constraint to get a pose where the feet are on the ground. The contact should be within the friction cone. And the robot should be in equilibrium. My cost function consists of the torques squared and position of the center of mass.

There are a few things going wrong with my optimization:

  1. The SNOPT output tells me that one or multiple (I can only see the constraint with the largest discrepancy) constraints seem to have incorrect gradients. SNOPT also provides the constraint number. Is there a way to see which constraint in drake corresponds to the constraint number given by SNOPT?

  2. Possibly related to 1, SNOPT mostly exits with 2 different error codes: SNOPT 42: Singular basis and SNOPT 43: Cannot satisfy the general constraints. Error code 43 is quite clear, but I don't know how to approach finding a solution to 42.

Lastly, I was wondering what kind of delta is usually acceptable for such an optimization. Currently, of the constraints that fail most have a delta <1E-2 which is still much too large. But maybe my constraints are a bit too strict with the default solver settings?

Upvotes: 2

Views: 100

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

Is there a way to see which constraint in drake corresponds to the constraint number given by SNOPT?

Unfortunately currently we don't provide the API to see which constraint in drake corresponds to the constraint number given by SNOPT. On the other hand, you could compute the gradient of your Drake constraint numerically, and compare that with the gradient in Eval function. Drake has ComputeNumericalGradient function.

Possibly related to 1, SNOPT mostly exits with 2 different error codes: SNOPT 42: Singular basis and SNOPT 43:

Most likely this is caused by the gradient error.

Upvotes: 1

Related Questions