Search code examples
drake

Exit condition 41 on SNOPT

I'm trying to run an optimization with SNOPT.

Right now as I run it I consistently get exit condition 41.

I've added the following parameters to the solver:

prog.SetSolverOption(solver.solver_id(),"Function Precision","1e-6")
prog.SetSolverOption(solver.solver_id(),"Major Optimality Tolerance","1e-3")
prog.SetSolverOption(solver.solver_id(),"Superbasics limit","600")
#print("Trying to Solve")
result = solver.Solve(prog)
print(result.is_success())

But I still contently get the same exit condition.

The error seems to be from the interpolation function I'm using. (When I remove it I no longer get the error).

t, c, k = interpolate.splrep(sref, kapparef, s=0, k=3)

kappafnc = interpolate.BSpline(t, c, k, extrapolate=False)

Here is the function I think I determined was causing the issue:

  def car_continous_dynamics(self, state, force, steering):
        beta = steering
        s = state[0]
        #print(s)
        n = state[1]
        alpha = state[2]
        v = state[3]
        #State = s, n, alpha , v
        #s= distance along the track, n = perpendicular distance along the track
        #alpha = velocity angle relative to the track
        #v= magnitude of the velocity of the car

        s_val = 0
        if s.value() >0:
          s_val = s.value()
          
        
        Cm1 = 0.28
        Cm2 = 0.05
        Cr0 = 0.011
        Cr2 = 0.006
        m = 0.043
        phi_dot = v*beta*15.5
        Fxd = (Cm1 - Cm2 * v) * force - Cr2 * v * v - Cr0 *tanh(5 * v)
        s_dot = v*cos(alpha+.5*beta)/(1)##-n*self.kappafunc(s_val))
        n_dot= v*sin(alpha+.5*beta)
        alpha_dot = phi_dot #-s_dot*self.kappafunc(s_val)
        v_dot=Fxd/m*cos(beta*.5)

        # concatenate velocity and acceleration
        #print(s_dot)
        #print(n_dot)
        #print(alpha_dot)
        #print(v_dot)
        state_dot = np.array((s_dot,n_dot,alpha_dot,v_dot))
        #print("State dot")

        #print(state_dot.dtype.name)
        #print(state_dot)
        #print(state_dot)
        return state_dot
``
Solution

  • Debugging INFO 41 in SNOPT is generally challenging, it is often caused by some bad gradient (but could also due to the problem being really hard to optimize).

    You should check if your gradient is well-behaved. I see you have

    s_val = 0
if s.value() >0:
    s_val = s.value()

    There are two potential problems

    1. Your s carries the gradient (you could check s.derivatives(), it has non-zero gradient), but when you compute s_val, this is a float object with no gradient. Hence you lose the gradient information.
    2. s_val is non-differentiable at 0.

    I would use s instead of s_val in your function (without the branch if s.value() > 0). And also add a constraint s>=0 by

    prog.AddBoundingBoxConstraint(0, np.inf, s)

    With this bounding box constraint, SNOPT guarantees that in each iteration, the value of is is always non-negative.