I have a trajectory optimization problem posed as a Non linear program. The states x and controls u for each time step are concatenated into a single vector. By using indexing to extract the appropriate x’s and u’s, I want to add constraints for dynamics and also create an objective function which is the sum of x’Qx +u’Ru for each time step.I also have an initial condition and goal condition to be set as constraints

At the moment, the following is my code where I have created an array Z but to enforce the initial and goal conditions, I had to go element by element. Even though it works,is there a better way to do this?

                nx = 13
                nu = 4
                N = int(self.tf/self.h)+1
         
                Q = np.diag(np.ones(nx))
                R = 0.1*np.diag(np.ones(nu))
                Qf = 10*np.diag(np.ones(nx))    

                #Initialize Model
                m = GEKKO(remote=False)
                #Set global options
                m.options.IMODE = 3 #steady state optimization   
                m.options.SOLVER = 3             
                Z = m.Array(m.Var,self.nz,value=0,lb=-1e5,ub=1e5)

                #Set upper and lower bounds: 1) U_lb = 0 
                for i in np.arange(0,N-1):
                    for j in self.u_index[:,i]:
                        Z[j].lower = 0

                for count, ele in enumerate(self.x_index[:,0]):
                        print(Z[ele],xic[count])
                        m.Equation(Z[ele]==xic[count])

                for count, ele in enumerate(self.x_index[:,N-1]):
                        print(Z[ele],xgoal[count])
                        m.Equation(Z[ele]==xgoal[count])

                #Objective
                J = []
                for i in np.arange(0,N-1):
                        x = Z[self.x_index[:,i]]
                        u = Z[self.u_index[:,i]]
                        J.append(0.5*(x-xgoal).T@Q@(x-xgoal) + 0.5*u.T@R@u)
                        
                #Solve simulation          
                m.Minimize(J)     
                m.solve()

My cost function is currently the main issue as it causes the following error: Exception: @error: Max Equation Length” error. Any suggestions on how to tackle this? I am aware there is another method of solving optimal control problems using Gekko but I would like to try to solve it as an NLP