Python AcadosModel.f_impl_expr Examples

Example #1

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

Example #2

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

Example #3

File: lat_mpc.py Project: dk6688/openpilot

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