Блог

Этот код предназначен для оптимизации перевернутого маятника с использованием CasADi. Однако я получаю ошибку, что мой эрмит Симпсон выходит за рамки дозволенного. Кто-нибудь знает, как этого избежать?

 for N in range(6,10):

    opti = casadi.Opti()

      # number of collocation points
    dt = (tf-t0) / (N-1) #step size

    #states
    x = opti.variable(4,N) #col vector of 4 vars
    y1 = x[0,:] #cart pos
    y2 = x[1,:] #pendulum position
    ydot1 = x[2,:]  
    ydot2 = x[3,:]
    u = opti.variable(1,N-1)

    def f(x,u):
        [y1,y2,ydot1,ydot2] =vertsplit(x)
        dy1 = ydot1
        dy2 = ydot2
        dydot1 = ((l*m2*sin(y2)*ydot2**2)   u   (m2*g*cos(y2)*sin(y2))) / (m1   m2*(1-cos(y2)**2))
        dydot2 = -1*((l*m2*cos(y2)*sin(y2)*ydot2**2)   u*cos(y2)   ((m1 m2)*g*sin(y2))) / (l*m1   l*m2*(1-cos(y2)**2))
        return vertcat(dy1,dy2,dydot1,dydot2)  

    
    for k in range(N):
        f1 = f(x[:,2*k],u[:,2*k])
        f2 = f(x[:,2*k 2],u[:,2*k])
        f3 = f(x[:,2*k 1],u[:,2*k])

        opti.subject_to(0.5*(x[:,2*k] x[:,2*k 2])   (dt/8)*(f1-f2) == x[:,2*k 1])

        opti.subject_to((dt/6)*(f1 4*f3 f2) == x[:,2*k 2]-x[:,2*k])