# echo
# echo "OCTAVE_PATH=$OCTAVE_PATH"
# status = os.system(
# "octave --eval \"convert_dae2gnsf({})\"".format("\'"+model.name+"_acados_dae.json\'")
# )
# if status == 0:
# print("\nsuccessfully detected GNSF structure in Octave\n")
# else:
# Exception("Failed to detect GNSF structure in Octave")
# load gnsf from json
with open(model.name + '_gnsf_functions.json', 'r') as f:
import json
gnsf_dict = json.load(f)
ocp.gnsf_model = gnsf_dict
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI
ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
simX = np.ndarray((N + 1, nx))
simU = np.ndarray((N, nu))
status = ocp_solver.solve()
if status != 0:
raise Exception('acados returned status {}. Exiting.'.format(status))
def main(cost_type='NONLINEAR_LS', hessian_approximation='EXACT', ext_cost_use_num_hess=0,
integrator_type='ERK'):
print(f"using: cost_type {cost_type}, integrator_type {integrator_type}")
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_pendulum_ode_model()
ocp.model = model
Tf = 1.0
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
N = 20
ocp.dims.N = N
# set cost
Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2])
R = 2*np.diag([1e-2])
x = ocp.model.x
u = ocp.model.u
cost_W = scipy.linalg.block_diag(Q, R)
if cost_type == 'LS':
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx,:nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[4,0] = 1.0
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
elif cost_type == 'NONLINEAR_LS':
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
ocp.model.cost_y_expr = vertcat(x, u)
ocp.model.cost_y_expr_e = x
elif cost_type == 'EXTERNAL':
ocp.cost.cost_type = 'EXTERNAL'
ocp.cost.cost_type_e = 'EXTERNAL'
ocp.model.cost_expr_ext_cost = vertcat(x, u).T @ cost_W @ vertcat(x, u)
ocp.model.cost_expr_ext_cost_e = x.T @ Q @ x
else:
raise Exception('Unknown cost_type. Possible values are \'LS\' and \'NONLINEAR_LS\'.')
if cost_type in ['LS', 'NONLINEAR_LS']:
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
ocp.cost.W_e = Q
ocp.cost.W = cost_W
# set constraints
Fmax = 80
ocp.constraints.constr_type = 'BGH'
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.constraints.x0 = x0
ocp.constraints.idxbu = np.array([0])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = hessian_approximation
ocp.solver_options.regularize_method = 'CONVEXIFY'
ocp.solver_options.integrator_type = integrator_type
if ocp.solver_options.integrator_type == 'GNSF':
import json
with open('../getting_started/common/' + model.name + '_gnsf_functions.json', 'r') as f:
gnsf_dict = json.load(f)
ocp.gnsf_model = gnsf_dict
ocp.solver_options.qp_solver_cond_N = 5
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI
ocp.solver_options.ext_cost_num_hess = ext_cost_use_num_hess
# create solver
AcadosOcpSolver.generate(ocp, json_file='acados_ocp.json')
AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp.json')
# time create
Ncreate = 1
create_time = []
for i in range(Ncreate):
t0 = time.time()
# ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json', build=False, generate=False)
from c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
ocp_solver = AcadosOcpSolverCython(ocp.model.name, ocp.solver_options.nlp_solver_type, ocp.dims.N)
create_time.append( time.time() - t0)
print(f"create_time: min {np.min(create_time)*1e3:.4f} ms mean {np.mean(create_time)*1e3:.4f} ms max {np.max(create_time)*1e3:.4f} ms over {Ncreate} executions")
# create_time: min 0.2189 ms mean 0.2586 ms max 0.6881 ms over 10000 executions
# RESET
Nreset = 3
reset_time = []
for i in range(Nreset):
# set NaNs as input to test reset() -> NOT RECOMMENDED!!!
# ocp_solver.options_set('print_level', 2)
for i in range(N):
ocp_solver.set(i, 'x', np.NaN * np.ones((nx,)))
ocp_solver.set(i, 'u', np.NaN * np.ones((nu,)))
status = ocp_solver.solve()
ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics")
if status == 0:
raise Exception(f'acados returned status {status}, although NaNs were given.')
else:
print(f'acados returned status {status}, which is expected, since NaNs were given.')
t0 = time.time()
ocp_solver.reset()
reset_time.append( time.time() - t0)
print(f"reset_time: min {np.min(reset_time)*1e3:.4f} ms mean {np.mean(reset_time)*1e3:.4f} ms max {np.max(reset_time)*1e3:.4f} ms over {Nreset} executions")
# OLD: without HPIPM mem reset:
# reset_time: min 0.0019 ms mean 0.0022 ms max 0.0250 ms over 10000 executions
# NEW: with HPIPM mem reset:
# reset_time: min 0.0036 ms mean 0.0047 ms max 0.0906 ms over 10000 executions
for i in range(N):
ocp_solver.set(i, 'x', x0)
if cost_type == 'EXTERNAL':
# NOTE: hessian is wrt [u,x]
if ext_cost_use_num_hess:
for i in range(N):
ocp_solver.cost_set(i, "ext_cost_num_hess", np.diag([0.04, 4000, 4000, 0.04, 0.04, ]))
ocp_solver.cost_set(N, "ext_cost_num_hess", np.diag([4000, 4000, 0.04, 0.04, ]))
simX = np.ndarray((N+1, nx))
simU = np.ndarray((N, nu))
status = ocp_solver.solve()
ocp_solver.print_statistics()
if status != 0:
raise Exception(f'acados returned status {status} for cost_type {cost_type}\n'
f'integrator_type = {integrator_type}.')
# get solution
for i in range(N):
simX[i,:] = ocp_solver.get(i, "x")
simU[i,:] = ocp_solver.get(i, "u")
simX[N,:] = ocp_solver.get(N, "x")
def main(discretization='shooting_nodes'):
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_pendulum_ode_model()
ocp.model = model
integrator_type = 'LIFTED_IRK' # ERK, IRK, GNSF, LIFTED_IRK
if integrator_type == 'GNSF':
acados_dae_model_json_dump(model)
# structure detection in Matlab/Octave -> produces 'pendulum_ode_gnsf_functions.json'
status = os.system('octave detect_gnsf_from_json.m')
# load gnsf from json
with open(model.name + '_gnsf_functions.json', 'r') as f:
gnsf_dict = json.load(f)
ocp.gnsf_model = gnsf_dict
Tf = 1.0
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
N = 15
# discretization
ocp.dims.N = N
# shooting_nodes = np.linspace(0, Tf, N+1)
time_steps = np.linspace(0, 1, N)
time_steps = Tf * time_steps / sum(time_steps)
shooting_nodes = np.zeros((N + 1, ))
for i in range(len(time_steps)):
shooting_nodes[i + 1] = shooting_nodes[i] + time_steps[i]
# nonuniform discretizations can be defined either by shooting_nodes or time_steps:
if discretization == 'shooting_nodes':
ocp.solver_options.shooting_nodes = shooting_nodes
elif discretization == 'time_steps':
ocp.solver_options.time_steps = time_steps
else:
raise NotImplementedError(
f"discretization type {discretization} not supported.")
# set num_steps
ocp.solver_options.sim_method_num_steps = 2 * np.ones((N, ))
ocp.solver_options.sim_method_num_steps[0] = 3
# set num_stages
ocp.solver_options.sim_method_num_stages = 2 * np.ones((N, ))
ocp.solver_options.sim_method_num_stages[0] = 4
# set cost
Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2])
R = 2 * np.diag([1e-2])
ocp.cost.W_e = Q
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[4, 0] = 1.0
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
# set constraints
Fmax = 80
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.constraints.x0 = x0
ocp.constraints.idxbu = np.array([0])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = integrator_type
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.initialize_t_slacks = 1
# Set additional options for Simulink interface:
acados_path = get_acados_path()
json_path = os.path.join(acados_path,
'interfaces/acados_template/acados_template')
with open(json_path + '/simulink_default_opts.json', 'r') as f:
simulink_opts = json.load(f)
ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp.json',
simulink_opts=simulink_opts)
# ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json')
simX = np.ndarray((N + 1, nx))
simU = np.ndarray((N, nu))
# change options after creating ocp_solver
ocp_solver.options_set("step_length", 0.99999)
ocp_solver.options_set("globalization",
"fixed_step") # fixed_step, merit_backtracking
ocp_solver.options_set("tol_eq", TOL)
ocp_solver.options_set("tol_stat", TOL)
ocp_solver.options_set("tol_ineq", TOL)
ocp_solver.options_set("tol_comp", TOL)
# initialize solver
for i in range(N):
ocp_solver.set(i, "x", x0)
status = ocp_solver.solve()
if status not in [0, 2]:
raise Exception('acados returned status {}. Exiting.'.format(status))
# get primal solution
for i in range(N):
simX[i, :] = ocp_solver.get(i, "x")
simU[i, :] = ocp_solver.get(i, "u")
simX[N, :] = ocp_solver.get(N, "x")
print("inequality multipliers at stage 1")
print(ocp_solver.get(1, "lam")) # inequality multipliers at stage 1
print("slack values at stage 1")
print(ocp_solver.get(1, "t")) # slack values at stage 1
print("multipliers of dynamic conditions between stage 1 and 2")
print(ocp_solver.get(
1, "pi")) # multipliers of dynamic conditions between stage 1 and 2
# initialize ineq multipliers and slacks at stage 1
ocp_solver.set(1, "lam", np.zeros(2, ))
ocp_solver.set(1, "t", np.zeros(2, ))
ocp_solver.print_statistics(
) # encapsulates: stat = ocp_solver.get_stats("statistics")
# timings
time_tot = ocp_solver.get_stats("time_tot")
time_lin = ocp_solver.get_stats("time_lin")
time_sim = ocp_solver.get_stats("time_sim")
time_qp = ocp_solver.get_stats("time_qp")
print(
f"timings OCP solver: total: {1e3*time_tot}ms, lin: {1e3*time_lin}ms, sim: {1e3*time_sim}ms, qp: {1e3*time_qp}ms"
)
# print("simU", simU)
# print("simX", simX)
iterate_filename = f'final_iterate_{discretization}.json'
ocp_solver.store_iterate(filename=iterate_filename, overwrite=True)
plot_pendulum(shooting_nodes, Fmax, simU, simX, latexify=False)
del ocp_solver
def generate(self, dae=None, quad=None, name='tunempc', opts={}):
""" Create embeddable NLP solver
"""
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver
# extract dimensions
nx = self.__nx
nu = self.__nu + self.__ns # treat slacks as pseudo-controls
# extract reference
ref = self.__ref
xref = np.squeeze(self.__ref[0][:nx], axis=1)
uref = np.squeeze(self.__ref[0][nx:nx + nu], axis=1)
# sampling time
self.__ts = opts['tf'] / self.__N
# create acados model
model = AcadosModel()
model.x = ca.MX.sym('x', nx)
model.u = ca.MX.sym('u', nu)
model.p = []
model.name = name
# detect input type
if dae is None:
model.f_expl_expr = self.__F(x0=model.x,
p=model.u)['xf'] / self.__ts
opts['integrator_type'] = 'ERK'
opts['sim_method_num_stages'] = 1
opts['sim_method_num_steps'] = 1
else:
n_in = dae.n_in()
if n_in == 2:
# xdot = f(x, u)
if 'integrator_type' in opts:
if opts['integrator_type'] in ['IRK', 'GNSF']:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_impl_expr = xdot - dae(model.x,
model.u[:self.__nu])
model.f_expl_expr = xdot
elif opts['integrator_type'] == 'ERK':
model.f_expl_expr = dae(model.x, model.u[:self.__nu])
else:
raise ValueError('Provide numerical integrator type!')
else:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_expl_expr = xdot
if n_in == 3:
# f(xdot, x, u) = 0
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu])
elif n_in == 4:
# f(xdot, x, u, z) = 0
nz = dae.size1_in(3)
z = ca.MX.sym('z', nz)
model.z = z
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu],
z)
else:
raise ValueError(
'Invalid number of inputs for system dynamics function.'
)
if self.__gnl is not None:
model.con_h_expr = self.__gnl(model.x, model.u[:self.__nu],
model.u[self.__nu:])
if self.__type == 'economic':
if quad is None:
model.cost_expr_ext_cost = self.__cost(
model.x, model.u[:self.__nu]) / self.__ts
else:
model.cost_expr_ext_cost = self.__cost(model.x,
model.u[:self.__nu])
# create acados ocp
ocp = AcadosOcp()
ocp.model = model
ny = nx + nu
ny_e = nx
if 'integrator_type' in opts and opts['integrator_type'] == 'GNSF':
from acados_template import acados_dae_model_json_dump
import os
acados_dae_model_json_dump(model)
# Set up Octave to be able to run the following:
## if using a virtual python env, the following lines can be added to the env/bin/activate script:
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/external/casadi-octave
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/
# echo
# echo "OCTAVE_PATH=$OCTAVE_PATH"
status = os.system(
"octave --eval \"convert({})\"".format("\'" + model.name +
"_acados_dae.json\'"))
# load gnsf from json
with open(model.name + '_gnsf_functions.json', 'r') as f:
import json
gnsf_dict = json.load(f)
ocp.gnsf_model = gnsf_dict
# set horizon length
ocp.dims.N = self.__N
# set cost module
if self.__type == 'economic':
# set cost function type to external (provided in model)
ocp.cost.cost_type = 'EXTERNAL'
if quad is not None:
ocp.solver_options.cost_discretization = 'INTEGRATOR'
elif self.__type == 'tracking':
# set weighting matrices
ocp.cost.W = self.__Href[0][0]
# set-up linear least squares cost
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.W_e = np.zeros((nx, nx))
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[nx:, :] = np.eye(nu)
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(np.linalg.inv(ocp.cost.W),self.__qref[0][0].T).full(), # gradient term
axis = 1
)
ocp.cost.yref_e = np.zeros((ny_e, ))
if n_in == 4: # DAE flag
ocp.cost.Vz = np.zeros((ny, nz))
# if 'custom_hessian' in opts:
# self.__custom_hessian = opts['custom_hessian']
# initial condition
ocp.constraints.x0 = xref
# set inequality constraints
ocp.constraints.constr_type = 'BGH'
if self.__S['C'] is not None:
C = self.__S['C'][0][:, :nx]
D = self.__S['C'][0][:, nx:]
lg = -self.__S['e'][0] + ct.mtimes(C, xref).full() + ct.mtimes(
D, uref).full()
ug = 1e8 - self.__S['e'][0] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
ocp.constraints.lg = np.squeeze(lg, axis=1)
ocp.constraints.ug = np.squeeze(ug, axis=1)
ocp.constraints.C = C
ocp.constraints.D = D
if 'usc' in self.__vars:
if 'us' in self.__vars:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['us'],
self.__vars['usc']
]
else:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['usc']
]
Jsg = ca.Function(
'Jsg', [self.__vars['usc']],
[ca.jacobian(self.__h(*arg), self.__vars['usc'])])(0.0)
self.__Jsg = Jsg.full()[:-self.__nsc, :]
ocp.constraints.Jsg = self.__Jsg
ocp.cost.Zl = np.zeros((self.__nsc, ))
ocp.cost.Zu = np.zeros((self.__nsc, ))
ocp.cost.zl = np.squeeze(self.__scost.full(),
axis=1) / self.__ts
ocp.cost.zu = np.squeeze(self.__scost.full(),
axis=1) / self.__ts
# set nonlinear equality constraints
if self.__gnl is not None:
ocp.constraints.lh = np.zeros(self.__ns, )
ocp.constraints.uh = np.zeros(self.__ns, )
# terminal constraint:
x_term = self.__p_operator(model.x)
Jbx = ca.Function('Jbx', [model.x],
[ca.jacobian(x_term, model.x)])(0.0)
ocp.constraints.Jbx_e = Jbx.full()
ocp.constraints.lbx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
ocp.constraints.ubx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
for option in list(opts.keys()):
if hasattr(ocp.solver_options, option):
setattr(ocp.solver_options, option, opts[option])
self.__acados_ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
self.__acados_integrator = AcadosSimSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
# set initial guess
self.__set_acados_initial_guess()
return self.__acados_ocp_solver, self.__acados_integrator
