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
# NOTE: all dimensions but N ar detected
ocp.dims.N = N
# 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])
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])
# 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'
ocp.solver_options.tol = 1e-1 * tol
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
# get slack values at stage 1
sl = acados_ocp_solver.get(1, "sl")
su = acados_ocp_solver.get(1, "su")
print("sl", sl, "su", su)
# plot results
# plot_pendulum(np.linspace(0, Tf, N+1), 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.")
j, "y_ref",
nmp.array([
-1, 1, 0, 0.770, -0.421, 0.421, 0.230, 0, 0, 0, uSS, uSS,
uSS, uSS
]))
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")
acados_integrator.set("x", x0)
acados_integrator.set("u", simU[i, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(
'acados integrator returned status {}. Exiting.'.format(status))
# update state
x0 = acados_integrator.get("x")
simX[i + 1, :] = x0
qtest = YPRtoQuat(nmp.array([nmp.pi / 4, nmp.pi / 4, 0]))
print("q\n", qtest)
R = QuattoR(qtest)
print("RMat:\n", R)
# plot results
plot_quad(dt, uSS, uDel, simU, simX)
x0 = np.zeros((nx, 1))
x0[:3*(M+1):3] = pos0_x[1:].reshape((M+1,1))
u0 = np.zeros((nu, 1))
acados_integrator.set("u", u0)
simX[0,:] = x0.flatten()
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_chain_position_traj(simX)
#%% find steady state
xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef)
if os.environ.get('ACADOS_ON_TRAVIS') is None:
plot_chain_position(xrest, xPosFirstMass)
plot_chain_position_3D((xrest, simX[-1,:]), xPosFirstMass)
nmp.sin(q2), 0, 0, 0
])
acados_ocp_solver.cost_set(j, "y_ref", refVec)
acados_ocp_solver.cost_set(Nmpc, "y_ref", refVec[:-1])
acados_ocp_solver.cost_set(
j, "W", nmp.diag([1e2, 1e2, 1e2, 1e2, 1e0, 1e0, 3.0e1]))
# acados_ocp_solver.cost_set(Nmpc, "W", nmp.diag([1e2, 1e2, 1e2, 1e2, 1e0, 1e0]))
# elif COST_MODULE == 'LS':
# acados_ocp_solver.set(j, "y_ref", nmp.array([1.5707, 0, 0, 0, 0]))
status = acados_ocp_solver.solve()
if status != 0 and status != 2:
raise Exception(
'acados acados_ocp_solver returned status {}. Exiting.'.format(
status))
simU[i, :] = acados_ocp_solver.get(0, "u")
# ~~~~~'Real' System~~~~~~
acados_integrator.set("x", x0)
acados_integrator.set("u", simU[i, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(
'acados integrator returned status {}. Exiting.'.format(status))
simXref[i, :] = nmp.array([q1, q2])
simX[i + 1, :] = acados_integrator.get("x") #True state.
# plot results
plot_double_pendulum(dt, uMax, simU, simX, simXref, simXest, simUest, simY)
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
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)
for i in range(N):
# 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 + 1), Fmax, simU, simX)
# create
acados_integrator = AcadosSimSolver(sim, cmake_builder=cmake_builder)
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,:])
# initialize IRK
if sim.solver_options.integrator_type == 'IRK':
acados_integrator.set("xdot", np.zeros((nx,)))
# 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")
print("S_forw, sensitivities of simulaition result wrt x,u:\n", S_forw)
# plot results
plot_pendulum(np.linspace(0, Tf, N+1), 10, np.zeros((N, nu)), simX, latexify=False)
k = 0
for i in range(N):
u = simU[i, 0]
acados_integrator.set("u", np.hstack((u, np.ones(1, ))))
# set simulation time
acados_integrator.set("T", dts_fine[i])
for j in range(Ns_fine[i]):
acados_integrator.set("x", simX_fine[k, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(
'acados returned status {}. Exiting.'.format(status))
simX_fine[k + 1, :] = acados_integrator.get("x")
simU_fine[k, :] = u
ts_fine[k + 1] = ts_fine[k] + dts_fine[i]
k += 1
# visualize
plt.figure()
ts = np.hstack((np.zeros((1, )), np.cumsum(dts)))
state_labels = ['p1', 'v1', 'p2', 'v2']
for i, l in enumerate(state_labels):
plt.subplot(5, 1, i + 1)
plt.plot(ts_fine, simX_fine[:, i], label='time optimal solution')
# Pick which control input actuates the body, allows for varying time delay.
p_dt = nmp.array([zeta, ts, Kp, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
acados_integrator_ct.set("p", p)
acados_integrator_ct.set("x", x0)
acados_integrator_ct.set("u", u0)
acados_integrator_dt.set("p", p_dt)
acados_integrator_dt.set("x", x0disc)
acados_integrator_dt.set("u", u0)
status = acados_integrator_dt.solve()
if status != 0:
raise Exception('acados returned status {}. Exiting.'.format(status))
# get solution
x0disc = acados_integrator_dt.get("x")
status = acados_integrator_ct.solve()
if status != 0:
raise Exception('acados returned status {}. Exiting.'.format(status))
# get solution
x0 = acados_integrator_ct.get("x")
simXdisc[i + 1, :] = x0disc
simX[i + 1, :] = x0
simY[i + 1, :] = x0[0] + nmp.transpose(
nmp.diag(v_stds) @ nmp.random.standard_normal((1, 1)))
simYdisc[i + 1, :] = x0disc[0] + nmp.transpose(
nmp.diag(v_stds) @ nmp.random.standard_normal((1, 1)))
# test setter
acados_integrator.set("u", np.array([2.0]))
acados_integrator.set("u", 2)
acados_integrator.set("u", 2.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(f'acados returned status {status}.')
S_algebraic = acados_integrator.get("S_algebraic")
print("S_algebraic (dz_dxu) = ", S_algebraic)
z = acados_integrator.get("z")
print("z = ", z)
S_forw = acados_integrator.get("S_forw")
print("S_forw, sensitivities of simulaition result wrt x,u:\n", S_forw)
# plot results
plot_pendulum(np.linspace(0, Tf, N+1), 10, np.zeros((N, nu)), simX, latexify=False)
def main(use_cython=True):
# (very) simple crane model
beta = 0.001
k = 0.9
a_max = 10
dt_max = 2.0
# states
p1 = SX.sym('p1')
v1 = SX.sym('v1')
p2 = SX.sym('p2')
v2 = SX.sym('v2')
x = vertcat(p1, v1, p2, v2)
# controls
a = SX.sym('a')
dt = SX.sym('dt')
u = vertcat(a, dt)
f_expl = dt * vertcat(v1, a, v2, -beta * v2 - k * (p2 - p1))
model = AcadosModel()
model.f_expl_expr = f_expl
model.x = x
model.u = u
model.name = 'crane_time_opt'
# create ocp object to formulate the OCP
x0 = np.array([2.0, 0.0, 2.0, 0.0])
xf = np.array([0.0, 0.0, 0.0, 0.0])
ocp = AcadosOcp()
ocp.model = model
# N - maximum number of bangs
N = 7
Tf = N
nx = model.x.size()[0]
nu = model.u.size()[0]
# set dimensions
ocp.dims.N = N
# set cost
ocp.cost.cost_type = 'EXTERNAL'
ocp.cost.cost_type_e = 'EXTERNAL'
ocp.model.cost_expr_ext_cost = dt
ocp.model.cost_expr_ext_cost_e = 0
ocp.constraints.lbu = np.array([-a_max, 0.0])
ocp.constraints.ubu = np.array([+a_max, dt_max])
ocp.constraints.idxbu = np.array([0, 1])
ocp.constraints.x0 = x0
ocp.constraints.lbx_e = xf
ocp.constraints.ubx_e = xf
ocp.constraints.idxbx_e = np.array([0, 1, 2, 3])
# set prediction horizon
ocp.solver_options.tf = Tf
# set options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' #'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 3
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
ocp.solver_options.globalization = 'MERIT_BACKTRACKING'
ocp.solver_options.nlp_solver_max_iter = 5000
ocp.solver_options.nlp_solver_tol_stat = 1e-6
ocp.solver_options.levenberg_marquardt = 0.1
ocp.solver_options.sim_method_num_steps = 15
ocp.solver_options.qp_solver_iter_max = 100
ocp.code_export_directory = 'c_generated_code'
ocp.solver_options.hessian_approx = 'EXACT'
ocp.solver_options.exact_hess_constr = 0
ocp.solver_options.exact_hess_dyn = 0
if use_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')
else: # ctypes
## Note: skip generate and build assuming this is done before (in cython run)
ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp.json',
build=False,
generate=False)
ocp_solver.reset()
for i, tau in enumerate(np.linspace(0, 1, N)):
ocp_solver.set(i, 'x', (1 - tau) * x0 + tau * xf)
ocp_solver.set(i, 'u', np.array([0.1, 0.5]))
simX = np.zeros((N + 1, nx))
simU = np.zeros((N, nu))
status = ocp_solver.solve()
if status != 0:
ocp_solver.print_statistics()
raise Exception(f'acados returned status {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")
dts = simU[:, 1]
print(
"acados solved OCP successfully, creating integrator to simulate the solution"
)
# simulate on finer grid
sim = AcadosSim()
# set model
sim.model = model
# set options
sim.solver_options.integrator_type = 'ERK'
sim.solver_options.num_stages = 4
sim.solver_options.num_steps = 3
sim.solver_options.T = 1.0 # dummy value
dt_approx = 0.0005
dts_fine = np.zeros((N, ))
Ns_fine = np.zeros((N, ), dtype='int16')
# compute number of simulation steps for bang interval + dt_fine
for i in range(N):
N_approx = max(int(dts[i] / dt_approx), 1)
dts_fine[i] = dts[i] / N_approx
Ns_fine[i] = int(round(dts[i] / dts_fine[i]))
N_fine = int(np.sum(Ns_fine))
simU_fine = np.zeros((N_fine, nu))
ts_fine = np.zeros((N_fine + 1, ))
simX_fine = np.zeros((N_fine + 1, nx))
simX_fine[0, :] = x0
acados_integrator = AcadosSimSolver(sim)
k = 0
for i in range(N):
u = simU[i, 0]
acados_integrator.set("u", np.hstack((u, np.ones(1, ))))
# set simulation time
acados_integrator.set("T", dts_fine[i])
for j in range(Ns_fine[i]):
acados_integrator.set("x", simX_fine[k, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(f'acados returned status {status}.')
simX_fine[k + 1, :] = acados_integrator.get("x")
simU_fine[k, :] = u
ts_fine[k + 1] = ts_fine[k] + dts_fine[i]
k += 1
# visualize
if os.environ.get('ACADOS_ON_TRAVIS'):
plt.figure()
state_labels = ['p1', 'v1', 'p2', 'v2']
for i, l in enumerate(state_labels):
plt.subplot(5, 1, i + 1)
plt.plot(ts_fine, simX_fine[:, i], label='time optimal solution')
plt.grid(True)
plt.ylabel(l)
if i == 0:
plt.legend(loc=1)
plt.subplot(5, 1, 5)
plt.step(ts_fine,
np.hstack((simU_fine[:, 0], simU_fine[-1, 0])),
'-',
where='post')
plt.grid(True)
plt.ylabel('a')
plt.xlabel('t')
plt.show()