def formulate_center_problem(f, v, Hv, hv, p=None):
nf = f.shape[0]
v_lwb = np.asarray([min_func(v[i], v, Hv, hv) for i in range(v.shape[0])])
v_uwb = -np.asarray([min_func(-v[i], v, Hv, hv) for i in range(v.shape[0])])
v_span = np.vstack(v_uwb - v_lwb)
norm_Hh = (Hv @ v - hv) / (Hv @ v_span)
objective = norm_Hh.T @ norm_Hh
g = []
lbg = []
ubg = []
g += [f]
lbg += [0] * nf
ubg += [0] * nf
g += [Hv @ v]
lbg += [-inf] * g[-1].shape[0]
ubg += [hv]
problem = {'f': objective, 'x': v, 'g': vertcat(*g)}
if p is None:
solver = nlpsol("solver", "ipopt", problem, NLP_OPTS)
else:
problem["p"] = p
solver = nlpsol("solver", "ipopt", problem, NLP_OPTS)
return solver, lbg, ubg
def create_nlp(var, par, obj, con, options, name=''):
codegen = options['codegen']
if options['verbose'] >= 1:
print('Building nlp ... ', end=' ')
t0 = time.time()
nlp = {'x': var, 'p': par, 'f': obj, 'g': con}
slv_opt = options['solver_options'][options['solver']]
opt = {}
for key, value in slv_opt.items():
opt[key] = value
opt.update({'expand': True})
print('opt updated')
solver = nlpsol('solver', options['solver'], nlp, opt)
print('solver created')
name = 'nlp' if name == '' else 'nlp_' + name
if codegen['build'] == 'jit':
if options['verbose'] >= 1:
print(('[jit compilation with flags %s]' % (codegen['flags'])), end=' ')
solver.generate_dependencies(name+'.c')
compiler = Compiler(
name+'.c', 'clang', {'flags': codegen['flags']})
problem = nlpsol('solver', options['solver'], compiler, slv_opt)
os.remove(name+'.c')
elif codegen['build'] == 'shared':
if os.name == 'nt':
raise ValueError('Build option is not supported for Windows!')
directory = os.path.join(os.getcwd(), 'build')
if not os.path.isdir(directory):
os.makedirs(directory)
path = os.path.join(directory, name)
if options['verbose'] >= 1:
print(('[compile to .so with flags %s]' % (codegen['flags'])), end=' ')
if os.path.isfile(path+'.so'):
os.remove(path+'.so')
solver.generate_dependencies(name+'.c')
shutil.move(name+'.c', path+'.c')
os.system('gcc -fPIC -shared %s %s.c -o %s.so' %
(codegen['flags'], path, path))
problem = nlpsol('solver', options['solver'], path+'.so', slv_opt)
os.remove(path+'.c')
elif codegen['build'] == 'existing':
if os.name == 'nt':
raise ValueError('Build option is not supported for Windows!')
directory = os.path.join(os.getcwd(), 'build')
path = os.path.join(directory, name)
if not os.path.isfile(path+'.so'):
raise ValueError('%s.so does not exist!', path)
if options['verbose'] >= 1:
print(('[using shared object %s.so]' % path), end=' ')
problem = nlpsol('solver', options['solver'], path+'.so', slv_opt)
elif codegen['build'] is None:
problem = solver
else:
raise ValueError('Invalid build option.')
print('nlpsol done.')
t1 = time.time()
if options['verbose'] >= 1:
print('in %5f s' % (t1-t0))
return problem, (t1-t0)
def solve_numerical_nlp(chosen_fun,
chosen_data,
opt_problem_details,
optim_options={}):
"""solve nlp for determining parameter values numerically"""
# formulate nlp
nlp = chosen_fun(chosen_data)
# NLP solver options
if optim_options:
solver = optim_options["solver"]
opts = optim_options["opts"]
else:
solver = "ipopt"
opts = {"ipopt.tol": 1e-16}
# solver = "sqpmethod"
# opts = {"qpsol": "qpoases"}
# Allocate an NLP solver and buffer
solver = casadi.nlpsol("solver", solver, nlp, opts)
# Solve the problem
res = solver(**opt_problem_details)
return res
def solve_nlp(prop_name, thrust):
rho0 = 1.225
i0_motor0 = 0.04
V_inf0 = 1
R_motor0 = 0.05
T_desired = thrust #### Thrust constraint
prop_data = prop_db[prop_name]
key0 = list(prop_data['dynamic'].keys())[0]
my_prop = prop_data['dynamic'][key0]['data']
CT_lut = ca.interpolant('CT', 'bspline', [my_prop.index], my_prop.CT)
CP_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.CP)
eta_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.eta)
eta_prop_lut = ca.interpolant('eta', 'bspline', [my_prop.index],
my_prop.eta)
Q_prop_lut = ca.substitute(Q_prop, CP, CP_lut(J))
T_prop_lut = ca.substitute(T_prop, CT, CT_lut(J))
states = ca.vertcat(rpm_prop, v_batt, Kv_motor)
params = ca.vertcat(rho, r_prop_in, i0_motor, V_inf, R_motor)
p0 = [rho0, prop_data['D'] / 2, i0_motor0, V_inf0, R_motor0]
f_Q_prop = ca.Function('Q_prop', [states, params], [Q_prop_lut])
f_Q_motor = ca.Function('Q_motor', [states, params], [Q_motor])
f_eta_motor = ca.Function('eta_motor', [states, params], [eta_motor])
f_eta_prop = ca.Function('eta_prop', [states, params], [eta_prop_lut(J)])
f_eta = ca.Function('eta', [states, params], [eta_prop_lut(J) * eta_motor])
f_T_prop = ca.Function('T_prop', [states, params], [T_prop_lut])
#%%
nlp = {
'f':
-f_eta(states, p0),
'x':
states,
'g':
ca.substitute(ca.vertcat(Q_prop_lut - Q_motor, T_prop_lut - T_desired),
params, p0)
}
S = ca.nlpsol('eff_max', 'ipopt', nlp, {
'print_time': 0,
'ipopt': {
'sb': 'yes',
'print_level': 0,
}
})
res = S(x0=[1000, 5, 1000], lbg=[0, 0], ubg=[0, 0])
stats = S.stats()
if stats['success']:
print('\n')
print('propeller -> ' + prop_name, res['x'], res['g'], -res['f'])
print('\n')
v0 = float(res['x'][1])
kv0 = float(res['x'][2])
if -res['f'] > 0.3:
rpm_sweep(prop_name, v0, kv0, i0_motor0, V_inf0, R_motor0)
return res, stats
def fitModel(self, u_in, y_out):
y_coeff = ca.SX.sym('y_c', self.n_output_lag)
u_coeff = ca.SX.sym('u_c',
self.n_input_lag + 1) # include current input
params = ca.vertcat(u_coeff, y_coeff)
obj = self.getObjective(u_in, y_out, u_coeff, y_coeff)
# set up solver
nlp = {'x': params, 'f': obj}
solver_opts = {
'ipopt': {
'print_level': 0,
'linear_solver': 'mumps'
},
'print_time': 0
}
solver = ca.nlpsol('solver', 'ipopt', nlp, solver_opts)
# solver.generate_dependencies('nlp.c')
# os.system("gcc -fPIC -shared nlp.c -o nlp.so")
# abspath = os.path.abspath('nlp.so')
# solver = ca.nlpsol('solver', 'ipopt', abspath, solver_opts)
x0 = np.zeros(params.shape[0])
res = np.array(solver(x0=x0)['x'])
self.u_coeff = res[:u_coeff.shape[0]]
self.y_coeff = res[u_coeff.shape[0]:]
print(res) #
# def eval_fct(u_prev, y_prev):
# return self.predict(u_prev, y_prev, res[:u_coeff.shape[0]], res[u_coeff.shape[0]:])
def create_solver(self):
"""
Create a new nonlinear solver.
"""
problem = {
'f': self.cost_function,
'x': vertcat(*self.var_symbols),
'g': vertcat(*self.cons_exprs)}
options = {}
if self.solver_name in ['scpgen', 'sqpmethod']:
options.update({
'qpsol': "qpoases",
'qpsol_options': {"printLevel": "none"},
})
elif self.solver_name == 'ipopt':
options.update({
'expand': self.casadi_expand,
'ipopt.fast_step_computation': self.ipopt_fast_step_computation,
'ipopt.fixed_variable_treatment':
self.ipopt_fixed_variable_treatment,
'ipopt.linear_solver': self.ipopt_linear_solver,
'ipopt.max_cpu_time': self.ipopt_max_cpu_time,
'ipopt.max_iter': self.ipopt_max_iter,
'ipopt.mu_strategy': self.ipopt_mu_strategy,
'ipopt.nlp_lower_bound_inf': self.ipopt_nlp_lower_bound_inf,
'ipopt.nlp_upper_bound_inf': self.ipopt_nlp_upper_bound_inf,
'ipopt.print_level': self.ipopt_print_level,
'ipopt.warm_start_init_point': self.ipopt_warm_start_init_point,
'verbose': False
})
self.solver = nlpsol('solver', self.solver_name, problem, options)
def _initialize_solver(self, **kwargs):
nlpsol_args = {
"expand", "iteration_callback", "iteration_callback_step",
"iteration_callback_ignore_errors", "ignore_check_vec",
"warn_initial_bounds", "eval_errors_fatal", "print_time",
"verbose_init"
}
# Initialize NLP object
opts = {
'ipopt.max_iter': 10000,
# 'linear_solver' : 'ma27'
}
if kwargs is not None:
for key, val in kwargs.items():
if key in nlpsol_args:
opts.update({key: val})
else:
opts.update({'ipopt.' + key: val})
self._solver_opts = opts
constraints = cs.vertcat(*self._constraints_sx)
self._solver = cs.nlpsol(
"solver", "ipopt", {
'x': self.var.vars_sx,
'p': self.pvar.vars_sx,
'f': self.objective_sx,
'g': constraints
}, self._solver_opts)
self.col_vars['lbg'] = np.concatenate(self._constraints_lb)
self.col_vars['ubg'] = np.concatenate(self._constraints_ub)
def compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef):
model = export_chain_mass_model(n_mass, m, D, L)
nx = model.x.shape[0]
M = int((nx/3 -1)/2)
# initial guess for state
pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass)
x0 = np.zeros((nx, 1))
x0[:3*(M+1):3] = pos0_x[1:].reshape((M+1,1))
# decision variables
w = [model.x, model.xdot, model.u]
# initial guess
w0 = ca.vertcat(*[x0, np.zeros(model.xdot.shape), np.zeros(model.u.shape)])
# constraints
g = []
g += [model.f_impl_expr] # steady state
g += [model.x[3*M:3*(M+1)] - xEndRef] # fix position of last mass
g += [model.u] # don't actuate controlled mass
# misuse IPOPT as nonlinear equation solver
nlp = {'x': ca.vertcat(*w), 'f': 0, 'g': ca.vertcat(*g)}
solver = ca.nlpsol('solver', 'ipopt', nlp)
sol = solver(x0=w0,lbg=0,ubg=0)
wrest = sol['x'].full()
xrest = wrest[:nx]
return xrest
def setup_solver(self):
# Setup relevant functions and expressions
self.setup_problem_functions()
self.solver = cs.nlpsol("solver",
self.options["solver_name"],
self.mpc_problem["nlp"],
self.options["solver_opts"])
def nlp_solve(prob, obj, p_init, x_init, y_init):
"""
NLP solver for initial conditions to path-following algorithm
"""
nx, np, neq, niq, name = prob()
if niq > 0:
x, p, f, f_fun, con, conf, ubx, lbx, ubg, lbg = obj(
x_init, y_init, p_init, neq, niq, nx, np)
#Formulating NLP to solve
#All constraints must be formatted as inequality constraints for this solver
nlp = {'x': x, 'p': p, 'f': f, 'g': con}
solver = nlpsol('solver', 'ipopt', nlp)
sol = solver(x0=x_init, p=p_init, lbg=lbg, ubg=ubg, ubx=ubx, lbx=lbx)
x_opt = sol['x'] #Solving for x
lagmul = sol['lam_g']
#Determining active constraints
#(necessary to determine which multipliers are a lambda and which are a mu)
con_vals = conf(x_opt, p_init)
tol = 1e-6
for k in range(0, len(con_vals)):
if con_vals[k] >= 0 + tol or con_vals[
k] >= 0 - tol: #active constraint
lam_opt = lagmul[k]
else: #inactive constraint
mu_opt = lagmul[k]
#print('x_opt:',x_opt,'lambda:',lam_opt,'mu:',mu_opt)
return x_opt, lam_opt, mu_opt, con
def export_casadi_problems(self, destination, father, problem):
cwd = os.getcwd()
filenames = []
# substitutes
for child, subst in father.substitutes.items():
for name, fun in subst.items():
filenames.append('subst_'+name+'.c')
fun.generate(filenames[-1])
shutil.move(cwd+'/'+filenames[-1], destination+'src/'+filenames[-1])
# updx
obj = father.problem_description['obj']
con = father.problem_description['con']
var = father.problem_description['var']
par = father.problem_description['par']
opt = father.problem_description['opt']
nlp = {'x': var, 'p': par, 'f': obj, 'g': con}
options = {}
for key, value in opt['solver_options']['ipopt'].items():
options[key] = value
options.update({'expand': True})
solver = nlpsol('solver', 'ipopt', nlp, options)
solver.generate_dependencies('updx.c')
# updz, updl, upd_res
problem.problem_upd_z.generate('updz.c')
problem.problem_upd_l.generate('updl.c')
problem.problem_upd_res.generate('updres.c')
filenames.extend(['updx.c', 'updz.c', 'updl.c', 'updres.c'])
# move files
shutil.move(cwd+'/updx.c', destination+'src/updx.c')
shutil.move(cwd+'/updz.c', destination+'src/updz.c')
shutil.move(cwd+'/updl.c', destination+'src/updl.c')
shutil.move(cwd+'/updres.c', destination+'src/updres.c')
return filenames
def trim(vt, gamma, m_fuel, z, rhs, p, s0=None):
if s0 is None:
s0 = [0.1, -np.deg2rad(45), 0, 0, 0, 0]
s = ca.SX.sym('s', 6)
nlp = {
'x': s,
'f': objective(s, vt=vt, gamma=gamma, m_fuel=m_fuel, rhs=rhs, p=p,
z=z),
'g': constraint(s,
vt=vt,
gamma=gamma,
m_fuel=m_fuel,
rhs=rhs,
p=p,
z=z)
}
S = ca.nlpsol('S', 'ipopt', nlp, {
'print_time': 0,
'ipopt': {
'sb': 'yes',
'print_level': 0,
}
})
# s = [m_dot, alpha, beta, elev, ail, rdr]
m_dot = s[0]
alpha = s[1]
beta = s[2]
ail = s[3]
elv = s[4]
rdr = s[5]
res = S(x0=s0,
lbg=[0, 0, 0],
ubg=[0, 0, 0],
lbx=[
0, -np.deg2rad(20), -np.deg2rad(20), -np.deg2rad(20),
-np.deg2rad(20), -np.deg2rad(20)
],
ubx=[
10,
np.deg2rad(20),
np.deg2rad(20),
np.deg2rad(20),
np.deg2rad(20),
np.deg2rad(20)
])
stats = S.stats()
if not stats['success']:
raise ValueError('Trim failed to converge', stats['return_status'],
res)
s_opt = res['x']
x0, u0 = constrain(s_opt, vt, gamma, m_fuel, z)
return {
'x0': np.array(ca.DM(x0)).reshape(-1),
'u0': np.array(ca.DM(u0)).reshape(-1),
's': np.array(ca.DM(s_opt)).reshape(-1),
'f': float(res['f']),
'g': np.array(ca.DM(res['g'])).reshape(-1),
'status': stats['return_status']
}
def run_ipopt_ssmevaluator(ssm, n_s, n_u, linearize_mean):
casadi_ssm = CasadiSSMEvaluator(ssm, linearize_mean)
x = cas.MX.sym("x", (n_s, 1))
y = cas.MX.sym("y", (n_u, 1))
if linearize_mean:
mu, sigma, mu_jac = casadi_ssm(x, y)
f = cas.sum1(cas.sum2(mu)) + cas.sum1(cas.sum2(sigma)) + cas.sum1(
cas.sum2(mu_jac))
else:
mu, sigma = casadi_ssm(x, y)
f = cas.sum1(cas.sum2(mu)) + cas.sum1(cas.sum2(sigma))
x = cas.vertcat(x, y)
options = {
"ipopt": {
"hessian_approximation": "limited-memory",
"max_iter": 2,
"derivative_test": "first-order"
}
}
solver = cas.nlpsol("solver", "ipopt", {"x": x, "f": f}, options)
with capture_stdout() as out:
res = solver(x0=np.random.randn(5, 1))
return str(type(ssm)), linearize_mean, out[0]
def smooth_Lagrange_poly(x, y):
t = ca.SX.sym('t')
d = len(x) # amount of parameters
tau = ca.SX.sym('tau', d) # parameter as minimisation variable
poly = 0
for j in range(d): # for all data points ...
L = tau[j]
for r in range(d):
if r != j:
L *= (t - x[r]) / (x[j] - x[r])
poly += L
L_fun = ca.Function('L_fun', [t, tau], [poly])
ddL, _ = ca.hessian(poly, t)
ddL_fun = ca.Function('ddL_fun', [t, tau], [ddL])
# ddL_fun = L_fun.hessian(0) # second order derivative to
# [ddL,_,_] = ddL_fun([t,tau])
# minimise tau = fct output, incl penalize curvature
res = 0.1 * sum([(L_fun(x[k], tau) - y[k])**2 for k in range(d)])[0]
res += sum([ddL_fun(x[k], tau)[0]**2 * 1e4 for k in range(d)])[0]
Cost = ca.Function('cost', [tau], [res])
nlp = {'x': tau, 'f': res}
solver = ca.nlpsol("solver", "ipopt", nlp)
sol = solver(**{})
tau_opt = sol['x'] # optimal parameter for polynomial
return L_fun, tau_opt
def create_problem(self, nb_of_veh):
# Objective is 1 norm of the slacks
obj = sum(
definite_integral(self.sx_list[i], 0, 1) +
definite_integral(self.sy_list[i], 0, 1) for i in range(nb_of_veh))
var = cas.vertcat()
par = cas.vertcat()
# Setting up the optimization variables (symbolic) for each vehicle.
for i in range(nb_of_veh):
var = cas.vertcat(var, self.x_cfs_list[i], self.y_cfs_list[i],
self.sx_cfs_list[i], self.sy_cfs_list[i])
for i in range(nb_of_veh):
var = cas.vertcat(var, self.lmbd_cfs_list[i])
# Setting up the parameter set (symbolic) for each vehicle. These values will be filled in later on, in each solver iteration.
for i in range(nb_of_veh):
par = cas.vertcat(par, self.corr_start_list[i],
self.corr_end_list[i], self.corr_x_min_list[i],
self.corr_y_min_list[i], self.corr_x_max_list[i],
self.corr_y_max_list[i], self.corr_hp_list[i],
self.treshold_list[i])
par = cas.vertcat(par, self.corr_v0_list[i], self.maxVel_list[i],
self.maxAcc_list[i], self.radius_list[i],
self.overlap_list[i])
con = cas.vertcat(*self.con)
nlp = {'x': var, 'f': obj, 'p': par, 'g': con}
solver = cas.nlpsol('solver', 'ipopt', nlp, {
'ipopt.tol': 1e-5,
'ipopt.linear_solver': 'ma57'
})
self.solver = solver
self.nlp = nlp
def solve(self):
"""
Gives to CasADi states, controls, constraints, sum of all objective functions and theirs bounds.
Gives others parameters to control how solver works.
"""
# NLP
nlp = {"x": self.V, "f": self.J, "g": self.g}
opts = {
"ipopt.tol": 1e-6,
"ipopt.max_iter": 1000,
"ipopt.hessian_approximation":
"exact", # "exact", "limited-memory"
"ipopt.limited_memory_max_history": 50,
"ipopt.linear_solver": "mumps", # "ma57", "ma86", "mumps"
"iteration_callback": self.show_online_optim_callback,
}
solver = casadi.nlpsol("nlpsol", "ipopt", nlp, opts)
# Bounds and initial guess
arg = {
"lbx": self.V_bounds.min,
"ubx": self.V_bounds.max,
"lbg": self.g_bounds.min,
"ubg": self.g_bounds.max,
"x0": self.V_init.init,
}
# Solve the problem
return solver.call(arg)
def J_qi(T, Q, R, P_f, N_pred, states, controls, rhs, func_type=1):
# kriegen die Anzahl der Zeiler("shape" ist ein Tuple)
n_states = states.shape[0] # p 22.
# Eingangsanzahl
n_controls = controls.shape[0]
# nonlinear mapping function f(x,u)
f = Function('f', [states, controls], [rhs])
# Decision variables (controls) u_pwm
U = SX.sym('U', n_controls, N_pred)
# parameters (which include the !!!initial and the !!!reference state of
# the robot!!!reference input)
P = SX.sym('P', n_states + n_states + n_controls)
# number of x is always N+1 -> 0..N
X = SX.sym('X', n_states, (N_pred + 1))
X[:, 0] = P[0:n_states]
obj = 0
# compute predicted states and cost function
for k in range(N_pred):
st = X[:, k]
con = U[:, k]
f_value = f(st, con)
# print(f_value)
st_next = st + (T * f_value)
X[:, k + 1] = st_next
obj = obj + (st - P[n_states:2 * n_states]).T @ Q @ (
st - P[n_states:2 * n_states]) + (
con - P[2 * n_states:2 * n_states + n_controls]).T @ R @ (
con - P[2 * n_states:2 * n_states + n_controls])
# print(obj)
st = X[:, N_pred]
obj = obj + (st - P[n_states:2 * n_states]).T @ P_f @ (
st - P[n_states:2 * n_states])
# create a dense matrix of state constraints. Size of g: n_states * (N_pred+1) +1
g = SX.zeros(n_states * (N_pred + 1) + 1, 1)
# Constrainsvariable spezifizieren
for i in range(N_pred + 1):
for j in range(n_states):
g[n_states * i + j] = X[j, i]
g[n_states *
(N_pred + 1)] = (X[:, N_pred] - P[n_states:2 * n_states]).T @ P_f @ (
X[:, N_pred] - P[n_states:2 * n_states])
OPT_variables = reshape(U, N_pred, 1)
nlp_prob = {'f': obj, 'x': OPT_variables, 'g': g, 'p': P}
opts = {}
opts['print_time'] = False
opts['ipopt'] = {
'max_iter': 100,
'print_level': 0,
'acceptable_tol': 1e-8,
'acceptable_obj_change_tol': 1e-6
}
# print(opts)
return f, nlpsol("solver", "ipopt", nlp_prob, opts)
def export_casadi_problems(self, destination, father, problem):
cwd = os.getcwd()
filenames = []
# substitutes
for child, subst in father.substitutes.items():
for name, fun in subst.items():
filenames.append('subst_'+name+'.c')
fun.generate(filenames[-1])
shutil.move(cwd+'/'+filenames[-1], destination+'src/'+filenames[-1])
# updx
obj = father.problem_description['obj']
con = father.problem_description['con']
var = father.problem_description['var']
par = father.problem_description['par']
opt = father.problem_description['opt']
nlp = {'x': var, 'p': par, 'f': obj, 'g': con}
options = {}
for key, value in opt['solver_options']['ipopt'].items():
options[key] = value
options.update({'expand': True})
solver = nlpsol('solver', 'ipopt', nlp, options)
solver.generate_dependencies('updx.c')
# updz, updl, upd_res
problem.problem_upd_z.generate('updz.c')
problem.problem_upd_l.generate('updl.c')
problem.problem_upd_res.generate('updres.c')
filenames.extend(['updx.c', 'updz.c', 'updl.c', 'updres.c'])
# move files
shutil.move(cwd+'/updx.c', destination+'src/updx.c')
shutil.move(cwd+'/updz.c', destination+'src/updz.c')
shutil.move(cwd+'/updl.c', destination+'src/updl.c')
shutil.move(cwd+'/updres.c', destination+'src/updres.c')
return filenames
def solve(self, problem):
prob, bounds, w, x_plot, u_plot = problem
# Create an NLP solver
solver = ca.nlpsol('solver', 'ipopt', prob, {'verbose': False})
# Function to get x and u trajectories from w
trajectories = ca.Function('trajectories', [w], [x_plot, u_plot],
['w'], ['x', 'u'])
# Solve the NLP
#sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
sol = solver(**bounds)
x_opt, u_opt = trajectories(sol['x'])
self.x_opt = x_opt.full() # to numpy array
self.u_opt = u_opt.full() # to numpy array
# Feed the previous state back to the model
self.model.set_state_estimate(self.x_opt)
self.model.set_control_estimate(self.u_opt)
# This ensures the control does not change drastically from the previous
# (already-executed) control
print("u_opt", [self.u_opt[0, 0], self.u_opt[1, 0]])
self.Uk_prev = [self.u_opt[0, 0],
self.u_opt[1, 0]] # we don't need this? we do.
return self.x_opt, self.u_opt, solver.stats()
def solve(self):
all_J = self.__dispatch_obj_func()
all_g, all_g_bounds = self.__dispatch_bounds()
self.ipopt_nlp = {"x": self.ocp.V, "f": sum1(all_J), "g": all_g}
self.ipopt_limits = {
"lbx": self.ocp.V_bounds.min,
"ubx": self.ocp.V_bounds.max,
"lbg": all_g_bounds.min,
"ubg": all_g_bounds.max,
"x0": self.ocp.V_init.init,
}
if self.lam_g is not None:
self.ipopt_limits["lam_g0"] = self.lam_g
if self.lam_x is not None:
self.ipopt_limits["lam_x0"] = self.lam_x
solver = nlpsol("nlpsol", "ipopt", self.ipopt_nlp, self.opts)
# Solve the problem
self.out = {"sol": solver.call(self.ipopt_limits)}
self.out["sol"]["time_tot"] = solver.stats()["t_wall_total"]
# To match acados convention (0 = success, 1 = error)
self.out["sol"]["status"] = int(not solver.stats()["success"])
return self.out
def fit(self, input, output):
nsamples = input.shape[0]
fobj = 0
for n in range(nsamples):
yhat = self.sym['fcn'].call({
'u': input[n, :],
'w': self.sym['w']
})['yhat']
y = indicator_array(yhat.shape, output[n])
fobj -= cas.sum1(xlogy(y, yhat) + xlogy(
(1 - y), 1 - yhat)) # cross entropy loss
fobj = fobj / nsamples # scaling
if self.alpha:
fobj += self.alpha * cas.dot(self.sym['w'],
self.sym['w']) # L2 regularization
nlp = {'x': self.sym['w'], 'f': fobj, 'g': []}
opts = {
'print_time': False,
'ipopt': {
'tol': 1e-3,
'max_iter': 1000,
'hessian_approximation': 'limited-memory',
'print_level': 0
}
}
solver = cas.nlpsol('nlpsol', 'ipopt', nlp, opts)
res = solver(x0=self.weights)
self.weights = np.asarray(res['x'])
def solve(self, solver="ipopt", show_online_optim=False, options_ipopt={}):
"""
Gives to CasADi states, controls, constraints, sum of all objective functions and theirs bounds.
Gives others parameters to control how solver works.
"""
all_J = MX()
for j_nodes in self.J:
for j in j_nodes:
all_J = vertcat(all_J, j)
for nlp in self.nlp:
for obj_nodes in nlp["J"]:
for obj in obj_nodes:
all_J = vertcat(all_J, obj)
all_g = MX()
all_g_bounds = Bounds(interpolation_type=InterpolationType.CONSTANT)
for i in range(len(self.g)):
for j in range(len(self.g[i])):
all_g = vertcat(all_g, self.g[i][j])
all_g_bounds.concatenate(self.g_bounds[i][j])
for nlp in self.nlp:
for i in range(len(nlp["g"])):
for j in range(len(nlp["g"][i])):
all_g = vertcat(all_g, nlp["g"][i][j])
all_g_bounds.concatenate(nlp["g_bounds"][i][j])
nlp = {"x": self.V, "f": sum1(all_J), "g": all_g}
options_common = {}
if show_online_optim:
options_common["iteration_callback"] = OnlineCallback(self)
if solver == "ipopt":
options_default = {
"ipopt.tol": 1e-6,
"ipopt.max_iter": 1000,
"ipopt.hessian_approximation":
"exact", # "exact", "limited-memory"
"ipopt.limited_memory_max_history": 50,
"ipopt.linear_solver": "mumps", # "ma57", "ma86", "mumps"
}
for key in options_ipopt:
if key[:6] != "ipopt.":
options_ipopt[f"ipopt.{key}"] = options_ipopt[key]
del options_ipopt[key]
opts = {**options_default, **options_common, **options_ipopt}
else:
raise RuntimeError("Available solvers are: 'ipopt'")
solver = casadi.nlpsol("nlpsol", solver, nlp, opts)
# Bounds and initial guess
arg = {
"lbx": self.V_bounds.min,
"ubx": self.V_bounds.max,
"lbg": all_g_bounds.min,
"ubg": all_g_bounds.max,
"x0": self.V_init.init,
}
# Solve the problem
return solver.call(arg)
def __init__(self, x, f_exp, g_exp, p):
nx = x.size(1)
ng = g_exp.size(1)
self.x_sol = np.zeros((nx, 1))
# create reference solver
# x0 = np.zeros(nx)
# lbx = []
# ubx = []
J = f_exp
g = g_exp
self.lbg = -1e12 * np.ones((ng, 1))
self.ubg = np.zeros((ng, 1))
travis_run = os.getenv('TRAVIS_RUN')
if (travis_run != 'true'):
print_level = 3
else:
print_level = 0
prob = {'f': f_exp, 'x': x, 'g': g_exp, 'p': p}
opts = {
'ipopt': {
'print_level': print_level,
'dual_inf_tol': 1e-10,
'constr_viol_tol': 1e-10,
'compl_inf_tol': 1e-10
}
}
self.ipopt_solver = ca.nlpsol('solver', 'ipopt', prob, opts)
def trim(s0, x: f16.State, p: f16.Parameters, phi_dot: float, theta_dot: float,
psi_dot: float, gam: float):
def constrain(x, s):
u = f16.Control(thtl=s[0], elv_deg=s[1], ail_deg=s[2], rdr_deg=s[3])
alpha = s[4]
beta = s[5]
x.alpha = alpha
x.beta = beta
cos = ca.cos
sin = ca.sin
tan = ca.tan
atan = ca.arctan
sqrt = ca.sqrt
VT = x.VT
g = p.g
G = psi_dot * VT / g
a = 1 - G * tan(alpha) * sin(beta)
b = sin(gam) / cos(beta)
c = 1 + G**2 * cos(beta)**2
# coordinated turn constraint pg. 188
phi = atan(G * cos(beta) / cos(alpha) *
((a - b**2) +
b * tan(alpha) * sqrt(c *
(1 - b**2) + G**2 * sin(beta)**2)) /
(a**2 - b**2 * (1 + c * tan(alpha)**2)))
x.phi = phi
# rate of climb constraint pg. 187
a = cos(alpha) * cos(beta)
b = sin(phi) * sin(beta) + cos(phi) * sin(alpha) * cos(beta)
theta = (a*b + sin(gam)*sqrt(a**2 - sin(gam)**2 + b**2)) \
/ (a**2 - sin(gam)**2)
x.theta = theta
# kinematics pg. 20
x.P = phi_dot - sin(theta) * psi_dot
x.Q = cos(phi) * phi_dot + sin(phi) * cos(theta) * psi_dot
x.R = -sin(phi) * theta_dot + cos(phi) * cos(theta) * psi_dot
# engine power constraint
x.power = f16.tables['tgear'](u.thtl)
return x, u
s = ca.MX.sym('s', 6)
x, u = constrain(x, s)
f = f16.trim_cost(f16.dynamics(x, u, p))
nlp = {'x': s, 'f': f}
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {
'print_level': 0,
}})
r = S(x0=s0, lbg=0, ubg=0)
s_opt = r['x']
x, u = constrain(x, s_opt)
return x, u
def quadratic_optimizer_2(eco,
payoff_matrix=None,
prior_sol=None,
current_layered_attack=None):
Mx = eco.spectral
tot_points = eco.layers
res_conc = eco.parameters.efficiency * eco.water.res_counts * eco.parameters.layered_foraging[:,
0]
beta = current_layered_attack[:, 1, 0]
lam = ca.MX.sym('lam', 2)
sigma = ca.MX.sym('sigma', Mx.x.shape[0])
sigma_p = ca.MX.sym('sigma_p', Mx.x.shape[0])
# sigma = eco.heat_kernels[0].T @ sigma_eff
# sigma_p = eco.heat_kernels[0].T @ sigma_p_eff
inte = np.ones(eco.layers).reshape(1, eco.layers)
df1 = eco.parameters.clearance_rate[
0] * res_conc - eco.parameters.clearance_rate[
1] * sigma_p * beta - lam[0] * np.ones(tot_points)
df2 = eco.parameters.clearance_rate[
1] * eco.parameters.efficiency * sigma * beta - lam[1] * np.ones(
tot_points)
# g0 = ca.vertcat(cons_dyn, pred_dyn)
g1 = inte @ Mx.M @ (df1 * sigma) + inte @ Mx.M @ (df2 * sigma_p) #
g2 = inte @ Mx.M @ sigma_p - 1
g3 = inte @ Mx.M @ sigma - 1
g4 = ca.vertcat(-df1, -df2)
g = ca.vertcat(g1, g2, g3, g4)
# print(g0.size())
f = 0
sigmas = ca.vertcat(sigma, sigma_p) # sigma_bar
x = ca.vertcat(*[sigmas, lam])
lbg = np.zeros(3 + 2 * tot_points)
ubg = ca.vertcat(*[np.zeros(3), [ca.inf] * 2 * tot_points])
s_opts = {
'ipopt': {
'print_level': 1,
'linear_solver': 'ma57',
'hessian_approximation': 'limited-memory',
'acceptable_iter': 15
}
} # , 'tol':10**-3, 'acceptable_tol': 10**(-2)}}
prob = {'x': x, 'f': f, 'g': g}
lbx = ca.vertcat(*[np.zeros(x.size()[0] - 2), -ca.inf, -ca.inf])
solver = ca.nlpsol('solver', 'ipopt', prob, s_opts)
sol = solver(lbx=lbx, lbg=lbg, ubg=ubg, x0=prior_sol)
x_out = np.array(sol['x']).flatten()
return x_out
def solveTheProblem(self, gallerys):
print('pose:', self.s_x_, self.s_y_, self.s_theta_, self.s_curvative_,
self.g_x_, self.g_y_, self.g_theta_, self.g_curvative_)
print_time = rospy.Time().now().to_sec()
curve_num = 2 * len(self.gallerys_)
X = ca.SX.sym('X', 2 * curve_num) #定义优化求解变量
E = ca.SX.sym('E', 4) #定义误差向量
init_X = ca.SX.sym('init_X', 4) #定义初始状态变量
init_X[0] = self.s_x_
init_X[1] = self.s_y_
init_X[2] = self.s_theta_
init_X[3] = self.s_curvative_
g = [] #声明约束方程组
for i in range(curve_num):
init_X[0] += self.FC(X[2 * i], init_X[3], init_X[2], X[2 * i + 1])
init_X[1] += self.FS(X[2 * i], init_X[3], init_X[2], X[2 * i + 1])
init_X[2] += 0.5 * X[2 * i] * X[2 * i + 1] * X[
2 * i + 1] + init_X[3] * X[2 * i + 1]
init_X[3] += X[2 * i] * X[2 * i + 1]
g.append(init_X[0]) #添加约束方程
g.append(init_X[1]) #添加约束方程
g.append(init_X[3]) #添加约束方程
E[0] = init_X[0] - self.g_x_
E[1] = init_X[1] - self.g_y_
E[2] = init_X[2] - self.g_theta_
E[3] = init_X[3] - self.g_curvative_
obj = ca.mtimes([E.T, self.Q_, E]) #定义目标函数
nlp_prob = {'f': obj, 'x': X, 'g': ca.vertcat(*g)} #加载目标函数、求解变量、约束方程组
#设置求解器
opts_setting = {
'ipopt.max_iter': 100,
'ipopt.print_level': 0,
'print_time': 0,
'ipopt.acceptable_tol': 1e-8,
'ipopt.acceptable_obj_change_tol': 1e-6
}
solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)
lbx = []
ubx = []
lbg = []
ubg = []
for i in range(curve_num):
lbx.append(-10), ubx.append(10)
lbx.append(0), ubx.append(5)
for i in range(len(gallerys)):
lbg.append(gallerys[i].x_min_), ubg.append(gallerys[i].x_max_)
lbg.append(gallerys[i].y_min_), ubg.append(gallerys[i].y_max_)
lbg.append(-2), ubg.append(2)
lbg.append(gallerys[-1].x_min_), ubg.append(gallerys[-1].x_max_)
lbg.append(gallerys[-1].y_min_), ubg.append(gallerys[-1].y_max_)
lbg.append(-2), ubg.append(2)
res = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) #执行求解计算
print(res)
x = []
for i in range(2 * curve_num):
x.append(res['x'][i])
self.displayClothoid(x)
def test_directCollocationSimple(self):
"""Test direct collocation on a very simple model
"""
# Double integrator model
x = ce.struct_symMX([ce.entry('q'), ce.entry('v')])
u = cs.MX.sym('u')
ode = ce.struct_MX(x)
ode['q'] = x['v']
ode['v'] = u
quad = x['v']**2
NT = 2 # number of control intervals
N = 3 # number of collocation points per interval
ts = 1 # time step
# DAE model
dae = dae_model.SemiExplicitDae(x=x.cat, ode=ode.cat, u=u, quad=quad)
# Create direct collocation scheme
scheme = cl.CollocationScheme(dae=dae,
t=np.arange(NT + 1) * ts,
order=N)
# Optimization variable
w = scheme.combine(['x', 'K', 'Z', 'u'])
# Objective
f = scheme.q[:, -1]
# Constraints
g = ce.struct_MX([
ce.entry('eq', expr=scheme.eq),
ce.entry('initial', expr=scheme.x[:, 0]), # q0 = 0, v0 = 0
ce.entry('final',
expr=scheme.x[:, -1] - np.array([1, 0])) # qf = 1, vf = 0
])
# Make NLP
nlp = {'x': w, 'g': g, 'f': f}
# Init NLP solver
opts = {'ipopt.linear_solver': 'ma86'}
solver = cs.nlpsol('solver', 'ipopt', nlp, opts)
# Run NLP solver
sol = solver(lbg=0, ubg=0)
if np.isnan(float(sol['f'])):
raise RuntimeError('Nlp Solver failed')
sol_w = w(sol['x'])
# Check agains the known solution
nptest.assert_allclose(sol_w['u'], [[1, -1]])
nptest.assert_allclose(sol['f'], 2. / 3.)
def CorrThermoConsis(self, dE_start, fix_Ea=[], fix_BE=[], print_screen=False):
if self._thermo_constraint_expression is None:
self.build_thermo_constraint(thermoTem=298.15)
Pnlp = self._Pnlp
ini_p = np.hstack([dE_start])
dev = Pnlp - ini_p
if py == 2:
object_fxn = cas.mul(dev.T, dev)
elif py == 3:
object_fxn = cas.dot(dev, dev)
nlp = dict(f=object_fxn, x=Pnlp, g=self._thermo_constraint_expression)
nlpopts = dict()
if py == 2:
nlpopts['max_iter'] = 500
nlpopts['tol'] = 1e-8
nlpopts['expect_infeasible_problem'] = 'yes'
nlpopts['hessian_approximation'] = 'exact'
nlpopts['jac_d_constant'] = 'yes'
elif py == 3:
nlpopts['ipopt.max_iter'] = 500
nlpopts['ipopt.tol'] = 1e-8
nlpopts['ipopt.expect_infeasible_problem'] = 'yes'
nlpopts['ipopt.hessian_approximation'] = 'exact'
nlpopts['ipopt.jac_d_constant'] = 'yes'
if py == 2:
solver = cas.NlpSolver('solver', 'ipopt', nlp, nlpopts)
elif py == 3:
solver = cas.nlpsol('solver', 'ipopt', nlp, nlpopts)
# Bounds and initial guess
lbP = -np.inf * np.ones(self._Np)
ubP = np.inf * np.ones(self._Np)
for i in range(len(fix_Ea)):
if check_index(fix_Ea[i], self.dEa_index):
lbP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)]
ubP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)]
for i in range(len(fix_BE)):
if check_index(fix_BE[i], self.dBE_index):
lbP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)]
ubP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)]
lbG = 0 * np.ones(2 * self.nrxn)
ubG = np.inf * np.ones(2 * self.nrxn)
if not print_screen:
old_stdout = sys.stdout
sys.stdout = tempfile.TemporaryFile()
solution = solver(x0=ini_p, lbg=lbG, ubg=ubG, lbx=lbP, ubx=ubP)
dE_corr = solution['x'].full().T[0].tolist()
if not print_screen:
sys.stdout = old_stdout
return(dE_corr)
def _setup_nlpsolver(self):
__dirname__ = os.path.dirname(os.path.abspath(__file__))
path_to_nlp_object = os.path.join(__dirname__, self._PATH_TO_NLP_OBJECT, \
self._NLP_OBJECT_FILENAME)
self._nlpsolver = ca.nlpsol(self._solver_name, "ipopt",
path_to_nlp_object,
self._nlpsolver_options)
def _export_nlp_to_c_code(self):
__dirname__ = os.path.dirname(os.path.abspath(__file__))
nlpsolver = ca.nlpsol("nlpsol", "ipopt", self.nlp)
nlpsolver.generate_dependencies(self._NLP_SOURCE_FILENAME)
os.rename(self._NLP_SOURCE_FILENAME, os.path.join(__dirname__,
self._PATH_TO_NLP_SOURCE, self._NLP_SOURCE_FILENAME))
def solve(self):
self.__dispatch_bounds()
solver = nlpsol("nlpsol", "ipopt", self.ipopt_nlp, self.opts)
# Solve the problem
self.out = {"sol": solver.call(self.ipopt_limits)}
self.out["sol"]["time_tot"] = solver.stats()["t_wall_total"]
return self.out
def export_casadi_problems(self, destination, father, problem):
filenames = Export.export_casadi_problems(self, destination, father, problem)
obj = father.problem_description['obj']
con = father.problem_description['con']
var = father.problem_description['var']
par = father.problem_description['par']
opt = father.problem_description['opt']
nlp = {'x': var, 'p': par, 'f': obj, 'g': con}
options = {}
for key, value in opt['solver_options']['ipopt'].items():
options[key] = value
options.update({'expand': True})
solver = nlpsol('solver', 'ipopt', nlp, options)
solver.generate_dependencies('nlp.c')
cwd = os.getcwd()
shutil.move(cwd+'/nlp.c', destination+'src/nlp.c')
filenames.append('nlp.c')
return filenames
def _initialize_solver(self, **kwargs):
nlpsol_args = {"expand", "iteration_callback",
"iteration_callback_step",
"iteration_callback_ignore_errors", "ignore_check_vec",
"warn_initial_bounds", "eval_errors_fatal",
"print_time", "verbose_init"}
# Initialize NLP object
opts = {
'ipopt.max_iter' : 10000,
# 'linear_solver' : 'ma27'
}
if kwargs is not None:
for key, val in kwargs.items():
if key in nlpsol_args:
opts.update({key: val })
else:
opts.update({'ipopt.' + key: val })
self._solver_opts = opts
constraints = cs.vertcat(*self._constraints_sx)
self._solver = cs.nlpsol(
"solver", "ipopt",
{'x': self.var.vars_sx,
'p': self.pvar.vars_sx,
'f': self.objective_sx,
'g': constraints},
self._solver_opts)
self.col_vars['lbg'] = np.concatenate(self._constraints_lb)
self.col_vars['ubg'] = np.concatenate(self._constraints_ub)
def __init__(self,objective,*args):
"""
optisolve(objective)
optisolve(objective,constraints)
"""
if len(args)>=1:
constraints = args[0]
else:
constraints = []
options = dict()
if len(args)>=2:
options = args[1]
if not isinstance(constraints,list):
raise Exception("Constraints must be given as list: [x>=0,y<=0]")
[ gl_pure, gl_equality] = sort_constraints( constraints )
symbols = OptimizationObject.get_primitives([objective]+gl_pure)
# helper functions for 'x'
X = C.veccat(*symbols["x"])
helper = C.Function('helper',[X],symbols["x"])
helper_inv = C.Function('helper_inv',symbols["x"],[X])
# helper functions for 'p' if applicable
if 'p' in symbols:
P = C.veccat(*symbols["p"])
self.Phelper_inv = C.Function('Phelper_inv',symbols["p"],[P])
else:
P = C.MX.sym('p',0,1)
if len(gl_pure)>0:
g_helpers = [];
for p in gl_pure:
g_helpers.append(C.MX.sym('g',p.sparsity()))
G_helpers = C.veccat(*g_helpers)
self.Ghelper = C.Function('Ghelper',[G_helpers],g_helpers)
self.Ghelper_inv = C.Function('Ghelper_inv',g_helpers,[G_helpers])
codegen = False;
if 'codegen' in options:
codegen = options["codegen"]
del options["codegen"]
opt = {}
if codegen:
options["jit"] = True
opt["jit"] = True
gl_pure_v = C.MX()
if len(gl_pure)>0:
gl_pure_v = C.veccat(*gl_pure)
if objective.is_vector() and objective.numel()>1:
F = C.vec(objective)
objective = 0.5*C.dot(F,F)
FF = C.Function('nlp',[X,P], [F])
JF = FF.jacobian()
J_out = JF.call([X,P])
J = J_out[0].T;
H = C.mtimes(J,J.T)
sigma = C.MX.sym('sigma')
Hf = C.Function('H',dict(x=X,p=P,lam_f=sigma,hess_gamma_x_x=sigma*C.triu(H)),['x', 'p', 'lam_f', 'lam_g'],['hess_gamma_x_x'],opt)
if "expand" in options and options["expand"]:
Hf = Hf.expand()
options["hess_lag"] = Hf
nlp = {"x":X,"p":P,"f":objective,"g":gl_pure_v}
self.solver = C.nlpsol('solver','ipopt', nlp, options)
# Save to class properties
self.symbols = symbols
self.helper = helper
self.helper_inv = helper_inv
self.gl_equality = gl_equality
self.resolve()
ubg.append([0, 0])
# Concatenate vectors
w = ca.vertcat(*w)
g = ca.vertcat(*g)
x_plot = ca.horzcat(*x_plot)
u_plot = ca.horzcat(*u_plot)
w0 = np.concatenate(w0)
lbw = np.concatenate(lbw)
ubw = np.concatenate(ubw)
lbg = np.concatenate(lbg)
ubg = np.concatenate(ubg)
# Create an NLP solver
prob = {'f': J, 'x': w, 'g': g}
solver = ca.nlpsol('solver', 'ipopt', prob);
# Function to get x and u trajectories from w
trajectories = ca.Function('trajectories', [w], [x_plot, u_plot], ['w'], ['x', 'u'])
# Solve the NLP
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
x_opt, u_opt = trajectories(sol['x'])
x_opt = x_opt.full() # to numpy array
u_opt = u_opt.full() # to numpy array
# Plot the result
tgrid = np.linspace(0, T, N+1)
plt.figure(1)
plt.clf()
plt.plot(tgrid, x_opt[0], '--')
