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.")
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'IRK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP_RTI
ocp.solver_options.print_level = 0
# set prediction horizon
ocp.solver_options.tf = Tf
acados_ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp_' + model.name +
'.json')
acados_integrator = AcadosSimSolver(ocp,
json_file='acados_ocp_' + model.name +
'.json')
acados_integrator.set("p", p)
Nsim = 200
simX = nmp.zeros((Nsim + 1, nx))
simU = nmp.zeros((Nsim, nu))
simX[0, :] = x0
for i in range(Nsim):
# solve ocp
acados_ocp_solver.set(0, "lbx", x0)
acados_ocp_solver.set(0, "ubx", x0)
# update params
for j in range(N):
acados_ocp_solver.set(j, "p", p)
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)
acados_integrator = AcadosSimSolver(sim)
simX = np.ndarray((N+1, nx))
# position of last mass
xPosFirstMass = np.zeros((3,1))
xEndRef = np.zeros((3,1))
xEndRef[0] = L * (M+1) * 6
# initial 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))
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)
# 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)
sim.gnsf_model = gnsf_dict
# 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
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}.')
u0 = np.zeros((nu))
simX[0, :] = x0
#Parameters
rho = 1.225
A = 0.1
Cl = 0.125
Cd = 0.075
m = 10.0
g = 9.81
J3 = 0.25
J2 = J3 * 4
J1 = J3 * 4
p = np.array([rho, A, Cl, Cd, m, g, J1, J2, J3])
acados_integrator.set("p", p)
for i in range(Nsim):
# set initial state
acados_integrator.set("x", x0)
# set control inputs
# u0 = 0.25 * uMax * np.random.randn(4)
#u0 = np.array([10,0,10,0]) #Positive yaw.
#u0 = np.array([0,10,0,10]) # Negative yaw.
# u0 = np.array([10, np.sqrt(10*10*0.5), 0, np.sqrt(10*10*0.5)]) #Positive pitch.
u0 = np.array([0, np.sqrt(10 * 10 * 0.5), 10,
np.sqrt(10 * 10 * 0.5)]) # Negative pitch.
# u0 = np.array([np.sqrt(10*10*0.5), 10, np.sqrt(10*10*0.5), 0]) #Positive roll.
# u0 = np.array([np.sqrt(10 * 10 * 0.5), 0, np.sqrt(10 * 10 * 0.5),10]) # Negative roll.
sim.solver_options.integrator_type = 'IRK'
sim.solver_options.num_stages = 3
sim.solver_options.num_steps = 3
sim.solver_options.newton_iter = 3 # for implicit integrator
sim.solver_options.collocation_type = "GAUSS_RADAU_IIA"
# use the CMake build pipeline
cmake_builder = sim_get_default_cmake_builder()
# 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")
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(
'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]
########################
simX[0, :] = x0
simXdisc[0, :] = x0disc
v_stds = [0.05]
simU[round(0.8 / dt):round(3.8 / dt), 0] = 1
simU[round(5.8 / dt):round(7.8 / dt), 0] = -1
for i in range(N):
# set initial state
u0 = simU[i, :]
p = nmp.array([zeta, ts, Kp])
# 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:
sim.solver_options.sens_algebraic = True
sim.solver_options.sens_hess = True
sim.solver_options.output_z = True
sim.solver_options.sens_algebraic = True
sim.solver_options.sim_method_jac_reuse = True
# 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([2.0])
# 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:
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()