def gen_lat_mpc_solver():
ocp = AcadosOcp()
ocp.model = gen_lat_model()
Tf = np.array(T_IDXS)[N]
# set dimensions
ocp.dims.N = N
# set cost module
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
Q = np.diag([0.0, 0.0])
QR = np.diag([0.0, 0.0, 0.0])
ocp.cost.W = QR
ocp.cost.W_e = Q
y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2]
curv_rate = ocp.model.u[0]
v_ego = ocp.model.p[0]
ocp.parameter_values = np.zeros((P_DIM, ))
ocp.cost.yref = np.zeros((3, ))
ocp.cost.yref_e = np.zeros((2, ))
# TODO hacky weights to keep behavior the same
ocp.model.cost_y_expr = vertcat(y_ego,
((v_ego +5.0) * psi_ego),
((v_ego +5.0) * 4 * curv_rate))
ocp.model.cost_y_expr_e = vertcat(y_ego,
((v_ego +5.0) * psi_ego))
# set constraints
ocp.constraints.constr_type = 'BGH'
ocp.constraints.idxbx = np.array([2,3])
ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)])
ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)])
x0 = np.zeros((X_DIM,))
ocp.constraints.x0 = x0
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
ocp.solver_options.qp_solver_iter_max = 1
ocp.solver_options.qp_solver_cond_N = 1
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1]
ocp.code_export_directory = EXPORT_DIR
return ocp
def gen_long_ocp():
ocp = AcadosOcp()
ocp.model = gen_long_model()
Tf = T_IDXS[-1]
# set dimensions
ocp.dims.N = N
# set cost module
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
QR = np.zeros((COST_DIM, COST_DIM))
Q = np.zeros((COST_E_DIM, COST_E_DIM))
ocp.cost.W = QR
ocp.cost.W_e = Q
x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
j_ego = ocp.model.u[0]
a_min, a_max = ocp.model.p[0], ocp.model.p[1]
x_obstacle = ocp.model.p[2]
prev_a = ocp.model.p[3]
ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
desired_dist_comfort = get_safe_obstacle_distance(v_ego)
# The main cost in normal operation is how close you are to the "desired" distance
# from an obstacle at every timestep. This obstacle can be a lead car
# or other object. In e2e mode we can use x_position targets as a cost
# instead.
costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
x_ego, v_ego, a_ego, a_ego - prev_a, j_ego]
ocp.model.cost_y_expr = vertcat(*costs)
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
# Constraints on speed, acceleration and desired distance to
# the obstacle, which is treated as a slack constraint so it
# behaves like an asymmetrical cost.
constraints = vertcat(v_ego, (a_ego - a_min), (a_max - a_ego),
((x_obstacle - x_ego) - (3 / 4) *
(desired_dist_comfort)) / (v_ego + 10.))
ocp.model.con_h_expr = constraints
x0 = np.zeros(X_DIM)
ocp.constraints.x0 = x0
ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0])
# We put all constraint cost weights to 0 and only set them at runtime
cost_weights = np.zeros(CONSTR_DIM)
ocp.cost.zl = cost_weights
ocp.cost.Zl = cost_weights
ocp.cost.Zu = cost_weights
ocp.cost.zu = cost_weights
ocp.constraints.lh = np.zeros(CONSTR_DIM)
ocp.constraints.uh = 1e4 * np.ones(CONSTR_DIM)
ocp.constraints.idxsh = np.arange(CONSTR_DIM)
# The HPIPM solver can give decent solutions even when it is stopped early
# Which is critical for our purpose where compute time is strictly bounded
# We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
# does not cause issues since the problem is well bounded.
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
ocp.solver_options.qp_solver_cond_N = 1
# More iterations take too much time and less lead to inaccurate convergence in
# some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
ocp.solver_options.qp_solver_iter_max = 10
ocp.solver_options.qp_tol = 1e-3
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.shooting_nodes = T_IDXS
ocp.code_export_directory = EXPORT_DIR
return ocp
def gen_lead_mpc_solver():
ocp = AcadosOcp()
ocp.model = gen_lead_model()
Tf = np.array(MPC_T)[-1]
# set dimensions
ocp.dims.N = N
# set cost module
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
QR = np.diag([0.0, 0.0, 0.0, 0.0])
Q = np.diag([0.0, 0.0, 0.0])
ocp.cost.W = QR
ocp.cost.W_e = Q
x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
j_ego = ocp.model.u[0]
ocp.cost.yref = np.zeros((4, ))
ocp.cost.yref_e = np.zeros((3, ))
x_lead, v_lead = ocp.model.p[0], ocp.model.p[1]
G = 9.81
TR = 1.8
desired_dist = (v_ego * TR - (v_lead - v_ego) * TR + v_ego * v_ego /
(2 * G) - v_lead * v_lead / (2 * G))
dist_err = (desired_dist + 4.0 -
(x_lead - x_ego)) / (sqrt(v_ego + 0.5) + 0.1)
# TODO hacky weights to keep behavior the same
ocp.model.cost_y_expr = vertcat(
exp(.3 * dist_err) - 1.,
((x_lead - x_ego) - (desired_dist + 4.0)) / (0.05 * v_ego + 0.5),
a_ego * (.1 * v_ego + 1.0), j_ego * (.1 * v_ego + 1.0))
ocp.model.cost_y_expr_e = vertcat(
exp(.3 * dist_err) - 1.,
((x_lead - x_ego) - (desired_dist + 4.0)) / (0.05 * v_ego + 0.5),
a_ego * (.1 * v_ego + 1.0))
ocp.parameter_values = np.array([0., .0])
# set constraints
ocp.constraints.constr_type = 'BGH'
ocp.constraints.idxbx = np.array([
1,
])
ocp.constraints.lbx = np.array([
0,
])
ocp.constraints.ubx = np.array([
100.,
])
x0 = np.array([0.0, 0.0, 0.0])
ocp.constraints.x0 = x0
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
#ocp.solver_options.nlp_solver_tol_stat = 1e-3
#ocp.solver_options.tol = 1e-3
ocp.solver_options.qp_solver_iter_max = 10
#ocp.solver_options.qp_tol = 1e-3
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.shooting_nodes = np.array(MPC_T)
ocp.code_export_directory = EXPORT_DIR
return ocp