col_obj = np.linspace(0, num_nodes - 1, num_nodes, dtype=np.int32) + 2 * num_nodes # linear and nonlinear jacobin structures Arow, Acol, rowG, colG = problem.jacobianstructureAG() # Full nonlinear jacobin structure Grow = np.hstack((row_obj, rowG + 1)) Gcol = np.hstack((col_obj, colG)) Gind = ((Grow), (Gcol)) # define a huge number to infinite inf = 1.0e10 # set optimization options options = SNOPT_options() options.setOption('Verbose', True) options.setOption('Solution print', True) options.setOption('Print filename', 'sntoya_' + str(dint) + 'IpoptInit.out') options.setOption('Summary frequency', 1) # set initial guess x0 = np.zeros(3 * num_nodes) x0[i_x] = 5.0 x0[i_v] = 0.0 x0[peak_dis_ind] = peak_dis x0[bot_dis_ind] = -0.02 x0[i_u] = 10.3 # set lower bounds of optimization parameters
sumx = x[0] + x[1] + x[2] # Nonlinear objective term only fObj = 0.0 if mode == 0 or mode == 2: fObj = sumx**2 if mode == 1 or mode == 2: pass #gObj[0] = 2.0*sum #gObj[1] = 2.0*sum #gObj[2] = 2.0*sum return mode, fObj#, gObj options = SNOPT_options() inf = 1.0e+20 options.setOption('Infinite bound',inf) options.setOption('Verify level',3) options.setOption('Print filename','sntoyb.out') m = 4 n = 4 nnCon = 2 nnJac = 2 nnObj = 3 x0 = np.zeros(n+m,float)
def cs_optimize(self, muInit, covInit, wtsInit, tFinal, goal): self.delta = int(np.ceil(tFinal / self.nSegments)) self.goal = goal * 1. # print "\n******************************************************" # print "Inputs:\n", "mu = ", muInit, "\ncovInit = ", covInit, "\nwtsInit = ", wtsInit, "\ngoal = ", goal # print "\n******************************************************" ##### Setting up to use SNOPT ### inf = 1.0e20 options = SNOPT_options() options.setOption('Verbose', False) options.setOption('Solution print', False) options.setOption('Print filename', 'ds_goal_snopt.out') options.setOption('Print level', 0) options.setOption('Optimality tolerance', 1e-2) options.setOption('Summary frequency', 1) options.setOption('Major print level', 0) options.setOption('Minor print level', 0) X0 = np.random.rand( (self.nState + self.len_s + self.nInput) * self.nSegments) * 100. X0[:self.nState] = muInit * 1. # X0[self.nState*self.nSegments:self.nState*self.nSegments + self.len_s] = covInit[self.id1]*1. ''' Do cholesky Factorization on the input matrix ''' if not isPD(covInit): covInit = deepcopy(nearestPD(covInit)) L = la.cholesky(covInit).T X0[self.nState * self.nSegments:self.nState * self.nSegments + self.len_s] = np.reshape(L[self.id1], (self.len_s, ), 'F') self.X0 = deepcopy(X0) Xlow = np.array( [-150.0] * len(X0[:self.nState * self.nSegments]) + [0.] * len(X0[self.nState * self.nSegments: (self.nState + self.len_s) * self.nSegments]) + [-100.0] * len(X0[(self.nState + self.len_s) * self.nSegments: (self.nState + self.len_s + self.nInput) * self.nSegments])) Xupp = np.array( [150.0] * len(X0[:self.nState * self.nSegments]) + [1500.] * len(X0[self.nState * self.nSegments: (self.nState + self.len_s) * self.nSegments]) + [100.0] * len(X0[(self.nState + self.len_s) * self.nSegments: (self.nState + self.len_s + self.nInput) * self.nSegments])) n = len(X0) nF = int(1 + len(X0)) F_init = [0.] * nF Fstate_init = [0] * nF constraintRelax = 0.0 ## Setting the initial values of mu, cov and wts as boundary constraints Flow = np.array( [0.] + muInit.tolist() + [-constraintRelax] * len(X0[self.nState:self.nState * self.nSegments]) + covInit[self.id1].tolist() + [-constraintRelax] * len(X0[self.nState * self.nSegments + self.len_s: (self.nState + self.len_s + self.nInput) * self.nSegments])) Fupp = np.array( [0.] + muInit.tolist() + [constraintRelax] * len(X0[self.nState:self.nState * self.nSegments]) + covInit[self.id1].tolist() + [constraintRelax] * len(X0[self.nState * self.nSegments + self.len_s: (self.nState + self.len_s + self.nInput) * self.nSegments])) ObjRow = 1 Start = 0 # Cold Start cw = [None] * 5000 iw = [None] * 5000 rw = [None] * 5000 res = snopta(self.snopt_objFun, n, nF, x0=X0, xlow=Xlow, xupp=Xupp, Flow=Flow, Fupp=Fupp, ObjRow=ObjRow, F=F_init, Fstate=Fstate_init, name='ds_goal', start=Start, options=options) if res == None: raise ValueError("SNOPT FAILED TO OPTIMIZE!") print "SNOPT Result =", np.round(res.x, 4) xfinal = res.x mu_new = np.reshape(xfinal[:self.nState * self.nSegments], (self.nState, self.nSegments), 'F') s_new = np.reshape( xfinal[self.nState * self.nSegments:(self.nState + self.len_s) * self.nSegments], (self.len_s, self.nSegments), 'F') u_new = np.reshape( xfinal[(self.nState + self.len_s) * self.nSegments:(self.nState + self.len_s + self.nInput) * self.nSegments], (self.nInput, self.nSegments), 'F') final_wts = np.ndarray(self.nModel) covFinal = deepcopy(covInit) covFinal[self.id1] = s_new[:, -1] self.sysDynamics.fastWtsMapped(mu_new[:, -1], covFinal, final_wts) # if self.do_verbose: print '*****************\nSet Goal: ', goal print 'Plan Time Horizon: ', tFinal print 'Planning for segments: ', self.nSegments print 'Each Segment Length: ', self.delta print "Generated Plan: \n", np.round(mu_new.T, 3) #[-1,:] print "s_new: ", np.round(s_new.T, 3) print "u_new: ", np.round(u_new.T, 3) print "final_wts: ", final_wts print "Final Cost = ", res.F[0] print "********************\n" return res.F[0], mu_new, s_new, u_new, final_wts
def constantk_search_snopt_min(simplex, inter_par, K, y_safe, L_safe): ''' The function F is composed as: 1st - objective 2nd to nth - simplex bounds n+1 th .. - safe constraints :param x0 : The mass-center of delaunay simplex :param inter_par : :param xc : :param R2: :param y0: :param K0: :param A_simplex: :param b_simplex: :param lb_simplex: :param ub_simplex: :param y_safe: :param L_safe: :return: ''' xE = inter_par.xi n = xE.shape[0] # Determine if the boundary corner exists in simplex, if boundary corner detected: # e(x) = || x - x' ||^2_2 # else, e(x) is the regular uncertainty function. exist = unevaluated_vertices_identification(xE, simplex) # ------- ADD THE FOLLOWING LINE WHEN DEBUGGING -------- # simplex = xi[:, tri[ind, :]] # ------- ADD THE FOLLOWING LINE WHEN DEBUGGING -------- # Find the minimizer of the search fucntion in a simplex using SNOPT package. R2, xc = Utils.circhyp(simplex, n) # x is the center of this simplex x = np.dot(simplex, np.ones([n + 1, 1]) / (n + 1)) # First find minimizer xr on reduced model, then find the 2D point corresponding to xr. Constrained optm. A_simplex, b_simplex = Utils.search_simplex_bounds(simplex) lb_simplex = np.min(simplex, axis=1) ub_simplex = np.max(simplex, axis=1) inf = 1.0e+20 m = n + 1 # The number of constraints which is determined by the number of simplex boundaries. assert m == A_simplex.shape[0], 'The No. of simplex constraints is wrong' # TODO: multiple safe constraints in future. # nF: The number of problem functions in F(x), including the objective function, linear and nonlinear constraints. # ObjRow indicates the number of objective row in F(x). ObjRow = 1 # solve for constrained minimization of safe learning within each open ball of the vertices of simplex. # Then choose the one with the minimum continuous function value. x_solver = np.empty(shape=[n, 0]) y_solver = [] for i in range(n + 1): vertex = simplex[:, i].reshape(-1, 1) # First find the y_safe[vertex]: val, idx, x_nn = Utils.mindis(vertex, xE) if val > 1e-10: # This vertex is a boundary corner point. No safe-guarantee, we do not optimize around support points. continue else: # TODO: multiple safe constraints in future. safe_bounds = y_safe[idx] if n > 1: # The first function in F(x) is the objective function, the rest are m simplex constraints. # The last part of functions in F(x) is the safe constraints. # In high dimension, A_simplex make sure that linear_derivative_A won't be all zero. nF = 1 + m + 1 # the last 1 is the safe constraint. Flow = np.hstack((-inf, b_simplex.T[0], -safe_bounds)) Fupp = inf * np.ones(nF) # The lower and upper bounds of variables x. xlow = np.copy(lb_simplex) xupp = np.copy(ub_simplex) # For the nonlinear components, enter any nonzero value in G to indicate the location # of the nonlinear derivatives (in this case, 2). # A must be properly defined with the correct derivative values. linear_derivative_A = np.vstack((np.zeros( (1, n)), A_simplex, np.zeros((1, n)))) nonlinear_derivative_G = np.vstack((2 * np.ones( (1, n)), np.zeros((m, n)), 2 * np.ones((1, n)))) else: # For 1D problem, the simplex constraint is defined in x bounds. # TODO multiple safe cons. # 2 = 1 obj + 1 safe con. Plus one redundant constraint to make matrix A suitable. nF = 2 + 1 Flow = np.array([-inf, -safe_bounds, -inf]) Fupp = np.array([inf, inf, inf]) xlow = np.min(simplex) * np.ones(n) xupp = np.max(simplex) * np.ones(n) linear_derivative_A = np.vstack((np.zeros( (1, n)), np.zeros((1, n)), np.ones((1, n)))) nonlinear_derivative_G = np.vstack((2 * np.ones( (2, n)), np.zeros((1, n)))) x0 = x.T[0] # ------- ADD THE FOLLOWING LINE WHEN DEBUGGING -------- # cd dogs # ------- ADD THE FOLLOWING LINE WHEN DEBUGGING -------- save_opt_for_snopt_ck(n, nF, inter_par, xc, R2, K, A_simplex, L_safe, vertex, exist) # Since adaptiveK using ( p(x) - f0 ) / e(x), the objective function is nonlinear. # The constraints are generated by simplex bounds, all linear. options = SNOPT_options() options.setOption('Infinite bound', inf) options.setOption('Verify level', 3) options.setOption('Verbose', False) options.setOption('Print level', -1) options.setOption('Scale option', 2) options.setOption('Print frequency', -1) options.setOption('Summary', 'No') result = snopta(dogsobj, n, nF, x0=x0, name='DeltaDOGS_snopt', xlow=xlow, xupp=xupp, Flow=Flow, Fupp=Fupp, ObjRow=ObjRow, A=linear_derivative_A, G=nonlinear_derivative_G, options=options) x_solver = np.hstack((x_solver, result.x.reshape(-1, 1))) y_solver.append(result.objective) y_solver = np.array(y_solver) y = np.min(y_solver) x = x_solver[:, np.argmin(y_solver)].reshape(-1, 1) return x, y
def adaptiveK_p_snopt_min(x0, inter_par, Acons, bcons): inf = 1.0e+20 n = x0.shape[0] # the number of dimension of the problem. m = Acons.shape[0] # the number of linear constraints. nF = m + 1 ObjRow = 1 Flow = np.hstack((-inf * np.ones(1), bcons[0])) Fupp = inf * np.ones(nF) xlow = np.zeros(n) xupp = np.ones(n) x0 = x0.T[0] save_opt_for_snopt_p(n, nF, inter_par, Acons) linear_derivative_A = np.vstack((np.zeros((1, n)), Acons)) nonlinear_derivative_G = np.vstack((2 * np.ones((1, n)), np.zeros((m, n)))) options = SNOPT_options() options.setOption('Infinite bound', inf) options.setOption('Verify level', 3) options.setOption('Verbose', False) options.setOption('Print level', -1) options.setOption('Print frequency', -1) options.setOption('Summary', 'No') # options.setOption('Print filename', 'DDOGS.out') result = snopta(pobj, n, nF, x0=x0, name='DeltaDOGS_snopt', xlow=xlow, xupp=xupp, Flow=Flow, Fupp=Fupp, ObjRow=ObjRow, A=linear_derivative_A, G=nonlinear_derivative_G, options=options) x = result.x y = result.objective if abs(y) > 1e-6: y = -1 / y else: y = 1e+10 return x.reshape(-1, 1), y
return status, F def sntoya_objFG(status,x,needF,F,needG,G): F[0] = x[1] # objective row F[1] = x[0]**2 + 4.0*x[1]**2 F[2] = (x[0] - 2.0)**2 + x[1]**2 G[0] = 2*x[0] G[1] = 8*x[1] G[2] = 2*(x[0]-2) G[3] = 2*x[1] return status, F, G inf = 1.0e20 options = SNOPT_options() options.setOption('Verbose',True) options.setOption('Solution print',True) options.setOption('Print filename','sntoya.out') options.setOption('Summary frequency',1) # Name arrays have to be dtype='|S1' and also have to be the # correct length, else they are ignored by SNOPT: xnames = np.empty(2,dtype='|S8') xnames[0] = " x0" xnames[1] = " x1" Fnames = np.empty(3,dtype='|S8')
def adaptiveK_search_snopt_min(x0, inter_par, xc, R2, y0, A_simplex, b_simplex, lb_simplex, ub_simplex, y_safe, L_safe): ''' The function F is composed as: 1st - objective 2nd to nth - simplex bounds n+1 th .. - safe constraints :param x0 : The mass-center of delaunay simplex :param inter_par : :param xc : :param R2: :param y0: :param K0: :param A_simplex: :param b_simplex: :param lb_simplex: :param ub_simplex: :param y_safe: :param L_safe: :return: ''' # Find the minimizer of the search fucntion in a simplex using SNOPT package. inf = 1.0e+20 n = x0.shape[0] # the number of dimension of the problem. m = n + 1 # The number of constraints which is determined by the number of simplex boundaries. # TODO: multiple safe constraints in future. # nF: The number of problem functions in F(x), # including the objective function, linear and nonlinear constraints. # ObjRow indicates the numer of objective row in F(x). ObjRow = 1 # Upper and lower bounds of functions F(x). if n > 1: # The first function in F(x) is the objective function, the rest are m constraints. nF = m + 1 + 1 # the last 1 is the safe constraint. Flow = np.hstack((-inf * np.ones(1), b_simplex.T[0], np.zeros(1))) Fupp = inf * np.ones(nF) # The lower and upper bounds of variables x. xlow = np.copy(lb_simplex) xupp = np.copy(ub_simplex) # For the nonlinear components, enter any nonzero value in G to indicate the location # of the nonlinear derivatives (in this case, 2). # A must be properly defined with the correct derivative values. linear_derivative_A = np.vstack((np.zeros( (1, n)), A_simplex, np.zeros((1, n)))) nonlinear_derivative_G = np.vstack((2 * np.ones( (1, n)), np.zeros((m, n)), 2 * np.ones((1, n)))) else: # For 1D problem, only have 1 objective function, the only one constraint is defined in x bounds. # nF = 2, cuz 1 obj + safe cons. #TODO multiple safe cons. # plus one redundant constraint: nF = 2 Flow = np.hstack((-inf * np.ones(1), np.zeros(1))) Fupp = inf * np.ones(nF) xlow = np.min(b_simplex) * np.ones(n) xupp = np.max(b_simplex) * np.ones(n) linear_derivative_A = np.vstack((np.zeros((1, n)), np.zeros((1, n)))) linear_derivative_A[0, 0] = 1e-10 nonlinear_derivative_G = 2 * np.ones((2, n)) x0 = x0.T[0] save_opt_for_snopt(n, nF, inter_par, xc, R2, y0, A_simplex, y_safe, L_safe) # Since adaptiveK using ( p(x) - f0 ) / e(x), the objective function is nonlinear. # The constraints are generated by simplex bounds, all linear. options = SNOPT_options() options.setOption('Infinite bound', inf) options.setOption('Verify level', 3) options.setOption('Verbose', False) options.setOption('Print level', -1) options.setOption('Scale option', 2) # options.setOption('Print frequency', -1) # options.setOption('Summary', 'Yes') result = snopta(dogsobj, n, nF, x0=x0, name='DeltaDOGS_snopt', xlow=xlow, xupp=xupp, Flow=Flow, Fupp=Fupp, ObjRow=ObjRow, A=linear_derivative_A, G=nonlinear_derivative_G, options=options) x = result.x y = result.objective y = (-1 / y if abs(y) > 1e-10 else 1e+10) return x.reshape(-1, 1), y
fObj = 0.0 if mode == 0 or mode == 2: fObj = 100.0 * (x[1] - x[0]**2)**2 + (1.0 - x[0])**2 if mode == 0: return mode, fObj if mode == 1 or mode == 2: gObj[0] = -400.0 * (x[1] - x[0]**2) * x[0] - 2.0 * (1.0 - x[0]) gObj[1] = 200.0 * (x[1] - x[0]**2) return mode, fObj, gObj options = SNOPT_options() inf = 1.0e+20 options.setOption('Infinite bound', inf) options.setOption('Verify level', 3) options.setOption('Verbose', True) options.setOption('Print filename', 't2banana.out') m = 1 n = 2 nnCon = 0 nnJac = 0 nnObj = n x0 = np.zeros(n + m, float)
al = np.array([ -7.0, -7.0, -7.0, -7.0, -7.0, -7.0, -7.0, -7.0, -7.0, -7.0, -7.0, -7.0, 60.0, 50.0, 70.0, 85.0, 100.0 ]) au = np.array([ 6.0, 6.0, 6.0, 6.0, 7.0, 7.0, 7.0, 7.0, 6.0, 6.0, 6.0, 6.0, inf, inf, inf, inf, inf ]) c = np.array([ 2.3, 1.7, 2.2, 2.3, 1.7, 2.2, 2.3, 1.7, 2.2, 2.3, 1.7, 2.2, 2.3, 1.7, 2.2 ]) # Solve the problem options = SNOPT_options() options.setOption('Print frequency', 1) options.setOption('Summary frequency', 1) result = sqopt(userHx, x0, xl=xl, xu=xu, A=A, al=al, au=au, c=c, name='HS118 ', options=options)
gObj[0] = x[6] gObj[1] = - x[5] gObj[2] = - x[6] + x[7] gObj[3] = x[8] gObj[4] = - x[7] gObj[5] = - x[1] gObj[6] = - x[2] + x[0] gObj[7] = - x[4] + x[2] gObj[8] = x[3] return mode, fObj, gObj inf = 1.0e+20 options = SNOPT_options() options.setOption('Infinite bound',inf) options.setOption('Specs filename','snmainb.spc') options.setOption('Print filename','snmainb.out') m = 18 n = 9 nnCon = 14 ne = 52 nnJac = n nnObj = n bl = -inf*np.ones(n+m) bu = inf*np.ones(n+m) # Nonlinear constraints
""" import numpy as np import scipy.sparse as sp from optimize import snopta, SNOPT_options def dieta_fun(status, x, F, G, needF, needG): # LP has no nonlinear terms in the objective F = [] G = [] return status, F, G inf = 1.0e20 options = SNOPT_options() options.setOption('Print filename', 'dieta.out') options.setOption('Minor print level', 1) options.setOption('Summary frequency', 1) options.setOption('Print frequency', 1) n = 6 nF = 4 ObjRow = 4 # We provide the linear components of the Jacobian # matrix as a dense matrix. A = np.array([[110, 205, 160, 160, 420, 260], [4, 32, 13, 8, 4, 14], [2, 12, 54, 285, 22, 80], [3, 24, 13, 9, 20, 19]], float)