ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' #'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
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
#x = q1 q2 dq1 dq2 b1 b2
Q0e = nmp.diag((1e0, 1e0, 2e0, 2e0, 1e3, 1e4))
acados_mhe_solver = export_pend_mhe_solver(model, Nmhe, dt, Qe, Q0e, Re)
#################################
### Dynamics Simulation
#################################
sim = AcadosSim()
sim.model = model
# set simulation time
sim.solver_options.T = dt
# set options
sim.solver_options.num_stages = 4
sim.solver_options.num_steps = 6
acados_integrator = AcadosSimSolver(sim)
acados_integrator.set("p", p)
Tsim = 40
Nsim = int(Tsim / dt)
simX = nmp.zeros((Nsim + 1, nx + 2))
simXref = nmp.zeros((Nsim + 1, 2))
simU = nmp.zeros((Nsim, nu))
v_stds = [0.05, 0.05]
simY = nmp.zeros((Nsim + 1, 2)) #q1+w1 q2+w2
simXest = nmp.zeros((Nsim + 1, nx + 2))
simUest = nmp.zeros((Nsim, 1))
# Initial conditions
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.")
dt = 0.01
nx = model.x.size()[0]
nu = model.u.size()[0]
print("StateSize:", nx)
print("ControlSize:", nu)
# set simulation time
sim.solver_options.T = dt
# 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)
Nsim = int(Tf / dt)
uMax = 40 #rad/s control input max
simX = np.ndarray((Nsim + 1, nx))
simU = np.ndarray((Nsim, nu))
x0 = np.zeros((nx))
x0[0:4] = YPRtoQuat(np.array([0, 0, 0]))
u0 = np.zeros((nu))
simX[0, :] = x0
#Parameters
rho = 1.225
sim.model = model
Tf = 0.1
nx = model.x.size()[0]
nu = model.u.size()[0]
N = 200
# 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))
# 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)
N = 200
# set simulation time
sim.solver_options.T = Tf
# set options
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,)))
# 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)
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()
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(
'acados returned status {}. Exiting.'.format(status))
dt = 0.01 Tf = 2.0 N = int(Tf/dt) sim.model = model t = 0 sim.parameter_values = np.array([t]) # set simulation time sim.solver_options.T = dt # set options sim.solver_options.integrator_type = 'ERK' 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) #Used to mock data for testing mhe nx_augmented = model.x.size()[0] nu = model.u.size()[0] nx = nx_augmented - 4 x0 = nmp.zeros((nx_augmented)) u0 = nmp.zeros((nu)) #Storage structs simX = nmp.ndarray((N+1, nx_augmented)) simU = nmp.ndarray((N, nu)) simY = nmp.ndarray((N+1, 1)) simXest = nmp.zeros((N+1, nx_augmented)) simWest = nmp.zeros((N, 1)) simPest = nmp.zeros((N+1, 4))
sim.solver_options.integrator_type = 'IRK'
sim.solver_options.num_stages = 4
sim.solver_options.num_steps = 3
sim.solver_options.newton_iter = 3 # for implicit integrator
sim.solver_options.sens_forw = True
sim.solver_options.sens_adj = True
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
sim_ct.parameter_values = np.array([zeta, ts, Kp])
sim_dt.parameter_values = np.array(
[zeta, ts, Kp, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
# set simulation time
sim_ct.solver_options.T = dt
sim_dt.solver_options.T = dt
# set options
sim_ct.solver_options.integrator_type = 'ERK'
sim_ct.solver_options.num_stages = 4
sim_ct.solver_options.num_steps = 3
sim_ct.solver_options.newton_iter = 3 # for implicit integrator
sim_dt.solver_options.integrator_type = 'DISCRETE'
# create
acados_integrator_ct = AcadosSimSolver(
sim_ct) #Used to mock data for testing mhe
acados_integrator_dt = AcadosSimSolver(
sim_dt) #Used to mock data for testing mhe
nx = model_ct.x.size()[0]
nx_dt = model_dt.x.size()[0]
nu = model_ct.u.size()[0]
u0 = nmp.zeros(nu)
#Storage structs
simX = nmp.zeros((N + 1, nx))
simXdisc = nmp.zeros((N + 1, nx_dt))
simU = nmp.zeros((N, nu))
simY = nmp.zeros((N + 1, 1))
simYdisc = nmp.zeros((N + 1, 1))
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()
def quadrotor_optimizer_setup(self, ):
Q_m_ = np.diag([
10,
10,
10,
0.3,
0.3,
0.3,
0.3,
0.05,
0.05,
0.05,
]) # position, q, v
P_m_ = np.diag([10, 10, 10, 0.05, 0.05, 0.05]) # only p and v
R_m_ = np.diag([5.0, 5.0, 5.0, 0.6])
nx = self.model.x.size()[0]
self.nx = nx
nu = self.model.u.size()[0]
self.nu = nu
ny = nx + nu
n_params = self.model.p.size()[0] if isinstance(self.model.p,
ca.SX) else 0
acados_source_path = os.environ['ACADOS_SOURCE_DIR']
sys.path.insert(0, acados_source_path)
# create OCP
ocp = AcadosOcp()
ocp.acados_include_path = acados_source_path + '/include'
ocp.acados_lib_path = acados_source_path + '/lib'
ocp.model = self.model
ocp.dims.N = self.N
ocp.solver_options.tf = self.T
# initialize parameters
ocp.dims.np = n_params
ocp.parameter_values = np.zeros(n_params)
# cost type
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.W = scipy.linalg.block_diag(Q_m_, R_m_)
ocp.cost.W_e = P_m_
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
ocp.cost.Vu = np.zeros((ny, nu))
ocp.cost.Vu[-nu:, -nu:] = np.eye(nu)
ocp.cost.Vx_e = np.zeros((nx - 4, nx))
# ocp.cost.Vx_e[:6, :6] = np.eye(6)
ocp.cost.Vx_e[:3, :3] = np.eye(3)
ocp.cost.Vx_e[-3:, -3:] = np.eye(3)
# initial reference trajectory_ref
x_ref = np.zeros(nx)
x_ref[3] = 1.0
x_ref_e = np.zeros(nx - 4)
u_ref = np.zeros(nu)
u_ref[-1] = self.g_
ocp.cost.yref = np.concatenate((x_ref, u_ref))
ocp.cost.yref_e = x_ref_e
# Set constraints
ocp.constraints.lbu = np.array([
self.constraints.roll_rate_min, self.constraints.pitch_rate_min,
self.constraints.yaw_rate_min, self.constraints.thrust_min
])
ocp.constraints.ubu = np.array([
self.constraints.roll_rate_max, self.constraints.pitch_rate_max,
self.constraints.yaw_rate_max, self.constraints.thrust_max
])
ocp.constraints.idxbu = np.array([0, 1, 2, 3])
# initial state
ocp.constraints.x0 = x_ref
# solver options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
# explicit Runge-Kutta integrator
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_type = 'SQP' # 'SQP_RTI'
ocp.solver_options.nlp_solver_max_iter = 400
# compile acados ocp
## files are stored in .ros/
json_file = os.path.join('./' + self.model.name + '_acados_ocp.json')
self.solver = AcadosOcpSolver(ocp, json_file=json_file)
if self.simulation_required:
self.integrator = AcadosSimSolver(ocp, json_file=json_file)
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
def quadrotor_optimizer_setup(self, ):
# Q_m_ = np.diag([80.0, 80.0, 120.0, 20.0, 20.0,
# 30.0, 10.0, 10.0, 0.0]) # position, velocity, roll, pitch, (not yaw)
# P_m_ = np.diag([86.21, 86.21, 120.95,
# 6.94, 6.94, 11.04]) # only p and v
# P_m_[0, 3] = 6.45
# P_m_[3, 0] = 6.45
# P_m_[1, 4] = 6.45
# P_m_[4, 1] = 6.45
# P_m_[2, 5] = 10.95
# P_m_[5, 2] = 10.95
# R_m_ = np.diag([50.0, 60.0, 1.0])
Q_m_ = np.diag(
[
10.0,
10.0,
10.0,
3e-1,
3e-1,
3e-1,
#3e-1, 3e-1, 3e-2, 3e-2,
100.0,
100.0,
1e-3,
1e-3,
10.5,
10.5,
10.5
]
) # position, velocity, load_position, load_velocity, [roll, pitch, yaw]
P_m_ = np.diag([
10.0, 10.0, 10.0, 0.05, 0.05, 0.05
# 10.0, 10.0, 10.0,
# 0.05, 0.05, 0.05
]) # only p and v
# P_m_[0, 8] = 6.45
# P_m_[8, 0] = 6.45
# P_m_[1, 9] = 6.45
# P_m_[9, 1] = 6.45
# P_m_[2, 10] = 10.95
# P_m_[10, 2] = 10.95
R_m_ = np.diag([3.0, 3.0, 3.0, 1.0])
nx = self.model.x.size()[0]
self.nx = nx
nu = self.model.u.size()[0]
self.nu = nu
ny = nx + nu
n_params = self.model.p.size()[0] if isinstance(self.model.p,
ca.SX) else 0
acados_source_path = os.environ['ACADOS_SOURCE_DIR']
sys.path.insert(0, acados_source_path)
# create OCP
ocp = AcadosOcp()
ocp.acados_include_path = acados_source_path + '/include'
ocp.acados_lib_path = acados_source_path + '/lib'
ocp.model = self.model
ocp.dims.N = self.N
ocp.solver_options.tf = self.T
# initialize parameters
ocp.dims.np = n_params
ocp.parameter_values = np.zeros(n_params)
# cost type
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.W = scipy.linalg.block_diag(Q_m_, R_m_)
ocp.cost.W_e = P_m_ # np.zeros((nx-3, nx-3))
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
ocp.cost.Vu = np.zeros((ny, nu))
ocp.cost.Vu[-nu:, -nu:] = np.eye(nu)
ocp.cost.Vx_e = np.zeros((nx - 7, nx)) # only consider p and v
ocp.cost.Vx_e[:nx - 7, :nx - 7] = np.eye(nx - 7)
# initial reference trajectory_ref
x_ref = np.zeros(nx)
x_ref_e = np.zeros(nx - 7)
u_ref = np.zeros(nu)
u_ref[-1] = self.g
ocp.cost.yref = np.concatenate((x_ref, u_ref))
ocp.cost.yref_e = x_ref_e
# Set constraints
ocp.constraints.lbu = np.array([
self.constraints.roll_min, self.constraints.pitch_min,
self.constraints.yaw_min, self.constraints.thrust_min
])
ocp.constraints.ubu = np.array([
self.constraints.roll_max, self.constraints.pitch_max,
self.constraints.yaw_max, self.constraints.thrust_max
])
ocp.constraints.idxbu = np.array([0, 1, 2, 3])
# initial state
ocp.constraints.x0 = x_ref
# solver options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
# explicit Runge-Kutta integrator
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_type = 'SQP' # 'SQP_RTI'
ocp.solver_options.levenberg_marquardt = 0.12 # 0.0
# compile acados ocp
json_file = os.path.join('./' + self.model.name + '_acados_ocp.json')
self.solver = AcadosOcpSolver(ocp, json_file=json_file)
if self.simulation_required:
self.integrator = AcadosSimSolver(ocp, json_file=json_file)
def generate(self, dae, 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)
# 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
n_in = dae.n_in()
if n_in == 2:
# xdot = f(x, u)
if 'integrator_type' in opts:
if opts['integrator_type'] == 'IRK':
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':
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
# 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'
else:
# set weighting matrices
if self.__type == 'tracking':
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))
# 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)
ocp.cost.zu = np.squeeze(self.__scost.full(), axis=1)
# 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()):
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
