示例#1
0
def gen_long_model():
    model = AcadosModel()
    model.name = MODEL_NAME

    # 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')
    model.p = vertcat(a_min, a_max, x_obstacle, prev_a)

    # 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
示例#2
0
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
    x_obstacle = SX.sym('x_obstacle')
    desired_TR = SX.sym('desired_TR')
    a_min = SX.sym('a_min')
    a_max = SX.sym('a_max')
    model.p = vertcat(a_min, a_max, x_obstacle, desired_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
示例#3
0
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
示例#4
0
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')
    model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego)

    # parameters
    v_ego = SX.sym('v_ego')
    rotation_radius = SX.sym('rotation_radius')
    model.p = vertcat(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')

    model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_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)
    model.f_impl_expr = model.xdot - f_expl
    model.f_expl_expr = f_expl
    return model