def gen_long_model():
model = AcadosModel()
model.name = 'long'
# set up states & controls
x_ego = SX.sym('x_ego')
v_ego = SX.sym('v_ego')
a_ego = SX.sym('a_ego')
model.x = vertcat(x_ego, v_ego, a_ego)
# controls
j_ego = SX.sym('j_ego')
model.u = vertcat(j_ego)
# xdot
x_ego_dot = SX.sym('x_ego_dot')
v_ego_dot = SX.sym('v_ego_dot')
a_ego_dot = SX.sym('a_ego_dot')
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
# live parameters
a_min = SX.sym('a_min')
a_max = SX.sym('a_max')
x_obstacle = SX.sym('x_obstacle')
prev_a = SX.sym('prev_a')
tr = SX.sym('tr')
model.p = vertcat(a_min, a_max, x_obstacle, prev_a, tr)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model
def gen_lead_model():
model = AcadosModel()
model.name = 'lead'
# set up states & controls
x_ego = SX.sym('x_ego')
v_ego = SX.sym('v_ego')
a_ego = SX.sym('a_ego')
model.x = vertcat(x_ego, v_ego, a_ego)
# controls
j_ego = SX.sym('j_ego')
model.u = vertcat(j_ego)
# xdot
x_ego_dot = SX.sym('x_ego_dot')
v_ego_dot = SX.sym('v_ego_dot')
a_ego_dot = SX.sym('a_ego_dot')
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
# live parameters
x_lead = SX.sym('x_lead')
v_lead = SX.sym('v_lead')
model.p = vertcat(x_lead, v_lead)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model
def gen_lat_model():
model = AcadosModel()
model.name = 'lat'
# set up states & controls
x_ego = SX.sym('x_ego')
y_ego = SX.sym('y_ego')
psi_ego = SX.sym('psi_ego')
curv_ego = SX.sym('curv_ego')
v_ego = SX.sym('v_ego')
rotation_radius = SX.sym('rotation_radius')
model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego, v_ego, rotation_radius)
# controls
curv_rate = SX.sym('curv_rate')
model.u = vertcat(curv_rate)
# xdot
x_ego_dot = SX.sym('x_ego_dot')
y_ego_dot = SX.sym('y_ego_dot')
psi_ego_dot = SX.sym('psi_ego_dot')
curv_ego_dot = SX.sym('curv_ego_dot')
v_ego_dot = SX.sym('v_ego_dot')
rotation_radius_dot = SX.sym('rotation_radius_dot')
model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot,
v_ego_dot, rotation_radius_dot)
# dynamics model
f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego),
v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego),
v_ego * curv_ego,
curv_rate,
0.0,
0.0)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model