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)