# [00, 00, @2, 00, 00],
# [00, 00, 00, @1, 00],
# [00, 00, 00, 00, @1]])
# NOTE: hessian is wrt [u,x]
if EXTERNAL_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('acados returned status {}. Exiting.'.format(status))
# 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")
plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False)
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)
plot_pendulum(shooting_nodes, Fmax, simU, simX, latexify=False)
# 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))
# 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")
plot_pendulum(np.linspace(0, Tf, N + 1), Fmax, simU, simX)
# set simulation time
sim.solver_options.T = Tf
# set options
sim.solver_options.num_stages = 4
sim.solver_options.num_steps = 3
sim.solver_options.newton_iter = 3 # for implicit integrator
# create
acados_integrator = AcadosSimSolver(sim)
simX = np.ndarray((N + 1, nx))
x0 = np.array([0.0, np.pi + 1, 0.0, 0.0])
u0 = np.array([0.0])
acados_integrator.set("u", u0)
simX[0, :] = x0
for i in range(N):
# set initial state
acados_integrator.set("x", simX[i, :])
# solve
status = acados_integrator.solve()
# get solution
simX[i + 1, :] = acados_integrator.get("x")
if status != 0:
raise Exception('acados returned status {}. Exiting.'.format(status))
# plot results
plot_pendulum(Tf / N, 10, np.zeros((N, nu)), simX)
def run_closed_loop_experiment(FORMULATION):
# 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
# set dimensions
ocp.dims.nx = nx
ocp.dims.ny = ny
ocp.dims.ny_e = ny_e
ocp.dims.nu = model.u.size()[0]
ocp.dims.ns = nu
ocp.dims.N = N
ocp.dims.nbu = 1
if FORMULATION == 0:
ocp.dims.nh = 0
ocp.dims.nsh = 0
ocp.dims.nbx = 1
ocp.dims.nsbx = 1
elif FORMULATION == 1:
ocp.dims.nh = 1
ocp.dims.nsh = 1
ocp.dims.nbx = 0
ocp.dims.nsbx = 0
# set cost module
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2])
R = 2 * np.diag([1e-2])
ocp.cost.W = scipy.linalg.block_diag(Q, R)
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.W_e = Q
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
ocp.cost.zl = 2000 * np.ones((1, ))
ocp.cost.Zl = 1 * np.ones((1, ))
ocp.cost.zu = 2000 * np.ones((1, ))
ocp.cost.Zu = 1 * np.ones((1, ))
# set constraints
Fmax = 80
vmax = 5
x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.constraints.x0 = x0
# bound on u
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.idxbu = np.array([0])
if FORMULATION == 0:
# soft bound on x
ocp.constraints.lbx = np.array([-vmax])
ocp.constraints.ubx = np.array([+vmax])
ocp.constraints.idxbx = np.array([2]) # v is x[2]
# indices of slacked constraints within bx
ocp.constraints.idxsbx = np.array([0])
# bounds on slack variables
ocp.constraints.lsbx = np.zeros((ocp.dims.nsbx, ))
ocp.constraints.usbx = np.zeros((ocp.dims.nsbx, ))
elif FORMULATION == 1:
# soft bound on x, using constraint h
v1 = ocp.model.x[2]
ocp.model.con_h_expr = v1
ocp.constraints.lh = np.array([-vmax])
ocp.constraints.uh = np.array([+vmax])
# indices of slacked constraints within h
ocp.constraints.idxsh = np.array([0])
# bounds on slack variables
ocp.constraints.lsh = np.zeros((ocp.dims.nh, ))
ocp.constraints.ush = np.zeros((ocp.dims.nh, ))
# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.tf = Tf
ocp.solver_options.nlp_solver_type = 'SQP'
json_filename = 'pendulum_soft_constraints.json'
acados_ocp_solver = AcadosOcpSolver(ocp, json_file=json_filename)
acados_integrator = AcadosSimSolver(ocp, json_file=json_filename)
# closed loop
Nsim = 20
simX = np.ndarray((Nsim + 1, nx))
simU = np.ndarray((Nsim, nu))
xcurrent = x0
for i in range(Nsim):
simX[i, :] = xcurrent
# solve ocp
acados_ocp_solver.set(0, "lbx", xcurrent)
acados_ocp_solver.set(0, "ubx", xcurrent)
status = acados_ocp_solver.solve()
if status != 0:
raise Exception(
'acados acados_ocp_solver returned status {}. Exiting.'.format(
status))
simU[i, :] = acados_ocp_solver.get(0, "u")
# simulate system
acados_integrator.set("x", xcurrent)
acados_integrator.set("u", simU[i, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(
'acados integrator returned status {}. Exiting.'.format(
status))
# update state
xcurrent = acados_integrator.get("x")
simX[Nsim, :] = xcurrent
# plot results
plot_pendulum(Tf / N, Fmax, simU, simX, latexify=False)
# store results
np.savetxt('test_results/simX_soft_formulation_' + str(FORMULATION), simX)
np.savetxt('test_results/simU_soft_formulation_' + str(FORMULATION), simU)
print("soft constraint example: ran formulation", FORMULATION,
"successfully.")
if status != 0:
raise Exception('acados returned status {}. Exiting.'.format(status))
# get solution
for i in range(N):
x_augmented = acados_solver_mhe.get(i, "x")
simXest[i, :] = x_augmented[0:nx]
simMest[i, :] = x_augmented[nx]
simWest[i, :] = acados_solver_mhe.get(i, "u")
x_augmented = acados_solver_mhe.get(N, "x")
simXest[N, :] = x_augmented[0:nx]
simMest[N, :] = x_augmented[nx]
print('difference |x0_est - x0_bar|',
np.linalg.norm(x0_bar[0:nx] - simXest[0, :]))
print('difference |x_est - x_true|', np.linalg.norm(simXest - simX))
print('difference |M_est - M_true|', np.abs(simMest[0] - M_true))
ts = np.linspace(0, Tf, N + 1)
plot_pendulum(ts, Fmax, simU, simX, simXest, simY, latexify=False)
# import matplotlib.pyplot as plt
# plt.figure()
# plt.plot(ts, M_true*np.ones((N+1, 1)), '-')
# plt.plot(ts, simMest, '.-')
# plt.grid()
# plt.ylabel('M')
# plt.legend(['true M', 'estimated M'])
# plt.show()
raise Exception('acados returned status {}. Exiting.'.format(status))
# get solution
for i in range(N0):
simX0[i, :] = ocp_solver.get(i, "x")
simU0[i, :] = ocp_solver.get(i, "u")
simX0[N0, :] = ocp_solver.get(N0, "x")
ocp_solver.print_statistics(
) # encapsulates: stat = ocp_solver.get_stats("statistics")
# plot but don't halt
plot_pendulum(np.linspace(0, Tf_01, N0 + 1),
Fmax,
simU0,
simX0,
latexify=False,
plt_show=False,
X_true_label=f'original: N={N0}, Tf={Tf_01}')
# --------------------------------------------------------------------------------
# 1) now reuse the code but set a new time-steps vector, with a new number of elements
dt1 = Tf_01 / N12
new_time_steps1 = np.tile(dt1, (N12, )) # Matlab's equivalent to repmat
time1 = np.hstack([0, np.cumsum(new_time_steps1)])
simX1 = np.ndarray((N12 + 1, nx))
simU1 = np.ndarray((N12, nu))
ocp_solver.set_new_time_steps(new_time_steps1)
ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0])
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 = 'ERK'
ocp.solver_options.print_level = 0
# 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))
# 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")
plot_pendulum(Tf/N, Fmax, simU, simX)
status = acados_solver_ocp.solve()
# acados_solver_ocp.print_statistics()
if status != 0:
raise Exception('controller returned status {} in step {}. Exiting.'.format(status, i))
simU[i:, ] = acados_solver_ocp.get(0, "u")
### simulation ###
# measurement
simY[i,:] = simX[i, :] + (V @ np.random.standard_normal((nx,1))).T
w = W @ np.random.standard_normal((nx,))
plant.set("u", w)
plant.set("p", simU[i,:])
plant.set("x", simX[i,:])
status = plant.solve()
if status != 0:
raise Exception('integrator returned status {} in step {}. Exiting.'.format(status, i))
simX[i+1,:] = plant.get("x")
# plot
print('estimation error p', np.linalg.norm(simX[:, 0] - simXest[:, 0]))
print('estimation error theta', np.linalg.norm(simX[:, 1] - simXest[:, 1]))
print('estimation error v', np.linalg.norm(simX[:, 2] - simXest[:, 2]))
print('estimation error dtheta', np.linalg.norm(simX[:, 3] - simXest[:, 3]))
plot_pendulum(np.linspace(0, Ts*Nsim, Nsim+1), u_max, simU, simX, simXest[N_mhe:, :], simY)
simX[0,:] = x0
acados_integrator.set("S_adj", np.ones((nx+nu, 1)))
for i in range(N):
# set initial state
acados_integrator.set("x", simX[i,:])
# solve
status = acados_integrator.solve()
# get solution
simX[i+1,:] = acados_integrator.get("x")
if status != 0:
raise Exception(f'acados returned status {status}.')
S_forw = acados_integrator.get("S_forw")
Sx = acados_integrator.get("Sx")
Su = acados_integrator.get("Su")
S_hess = acados_integrator.get("S_hess")
S_adj = acados_integrator.get("S_adj")
print("S_forw, sensitivities of simulaition result wrt x,u:\n", S_forw)
print("Sx, sensitivities of simulaition result wrt x:\n", Sx)
print("Su, sensitivities of simulaition result wrt u:\n", Su)
print("S_adj, adjoint sensitivities:\n", S_adj)
print("S_hess, second order sensitivities:\n", S_hess)
# plot results
plot_pendulum(np.linspace(0, Tf, N+1), 10, np.zeros((N, nu)), simX, latexify=False)
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 = 'ERK'
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 = 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))
# 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")
plot_pendulum(Tf / N, Fmax, simU, simX, latexify=False)
def main(interface_type='ctypes'):
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_pendulum_ode_model()
ocp.model = model
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
# define the different options for the use-case demonstration
N0 = 20 # original number of shooting nodes
N12 = 15 # change the number of shooting nodes for use-cases 1 and 2
condN12 = max(1, round(N12/1)) # change the number of cond_N for use-cases 1 and 2 (for PARTIAL_* solvers only)
Tf_01 = 1.0 # original final time and for use-case 1
Tf_2 = Tf_01 * 0.7 # change final time for use-case 2 (but keep N identical)
# set dimensions
ocp.dims.N = N0
# 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])
ocp.constraints.idxbu = np.array([0])
ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0])
# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
# PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM,
# PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
# ocp.solver_options.print_level = 1
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
# set prediction horizon
ocp.solver_options.tf = Tf_01
print(80*'-')
print('generate code and compile...')
if interface_type == 'cython':
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')
elif interface_type == 'ctypes':
ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
elif interface_type == 'cython_prebuilt':
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)
# test setting HPIPM options
ocp_solver.options_set('qp_tol_ineq', 1e-8)
ocp_solver.options_set('qp_tau_min', 1e-10)
ocp_solver.options_set('qp_mu0', 1e0)
# --------------------------------------------------------------------------------
# 0) solve the problem defined here (original from code export), analog to 'minimal_example_ocp.py'
nvariant = 0
simX0 = np.ndarray((N0 + 1, nx))
simU0 = np.ndarray((N0, nu))
print(80*'-')
print(f'solve original code with N = {N0} and Tf = {Tf_01} s:')
status = ocp_solver.solve()
if status != 0:
ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics")
raise Exception(f'acados returned status {status}.')
# get solution
for i in range(N0):
simX0[i, :] = ocp_solver.get(i, "x")
simU0[i, :] = ocp_solver.get(i, "u")
simX0[N0, :] = ocp_solver.get(N0, "x")
ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics")
ocp_solver.store_iterate(filename=f'final_iterate_{interface_type}_variant{nvariant}.json', overwrite=True)
if PLOT:# plot but don't halt
plot_pendulum(np.linspace(0, Tf_01, N0 + 1), Fmax, simU0, simX0, latexify=False, plt_show=False, X_true_label=f'original: N={N0}, Tf={Tf_01}')
simX[0,:] = xcurrent
# closed loop
for i in range(Nsim):
# solve ocp
acados_ocp_solver.set(0, "lbx", xcurrent)
acados_ocp_solver.set(0, "ubx", xcurrent)
status = acados_ocp_solver.solve()
if status != 0:
raise Exception('acados acados_ocp_solver returned status {}. Exiting.'.format(status))
simU[i,:] = acados_ocp_solver.get(0, "u")
# simulate system
acados_integrator.set("x", xcurrent)
acados_integrator.set("u", simU[i,:])
status = acados_integrator.solve()
if status != 0:
raise Exception('acados integrator returned status {}. Exiting.'.format(status))
# update state
xcurrent = acados_integrator.get("x")
simX[i+1,:] = xcurrent
# plot results
plot_pendulum(np.linspace(0, Tf/N*Nsim, Nsim+1), Fmax, simU, simX)
sim.solver_options.T = Tf
# set options
sim.solver_options.num_stages = 4
sim.solver_options.num_steps = 3
sim.solver_options.newton_iter = 3 # for implicit integrator
# create
acados_integrator = AcadosSimSolver(sim)
simX = np.ndarray((N+1, nx))
x0 = np.array([0.0, np.pi+1, 0.0, 0.0])
u0 = np.array([0.0])
acados_integrator.set("u", u0)
simX[0,:] = x0
for i in range(N):
# set initial state
acados_integrator.set("x", simX[i,:])
# solve
status = acados_integrator.solve()
# get solution
simX[i+1,:] = acados_integrator.get("x")
if status != 0:
raise Exception('acados returned status {}. Exiting.'.format(status))
# plot results
plot_pendulum(Tf/N, 10, np.zeros((N, nu)), simX, latexify=False)
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
simX[N, :] = acados_solver_ocp.get(N, "x")
simY[N, :] = simX[N, :] + np.transpose(
np.diag(v_stds) @ np.random.standard_normal((nx, 1)))
# set measurements and controls
for j in range(N):
yref = np.zeros((3 * nx, ))
yref[:nx] = simY[j, :]
yref[2 * nx:] = x0_bar
acados_solver_mhe.set(j, "yref", yref)
acados_solver_mhe.set(j, "p", simU[j, :])
# solve mhe problem
status = acados_solver_mhe.solve()
if status != 0:
raise Exception('acados returned status {}. Exiting.'.format(status))
# get solution
for i in range(N):
simXest[i, :] = acados_solver_mhe.get(i, "x")
simWest[i, :] = acados_solver_mhe.get(i, "u")
simXest[N, :] = acados_solver_mhe.get(N, "x")
print('difference |x0_est - x0_bar|', np.linalg.norm(x0_bar - simXest[0, :]))
print('difference |x_est - x_true|', np.linalg.norm(simXest - simX))
plot_pendulum(h, Fmax, simU, simX, simXest, simY)
