def __init__(self):
ffi, self.libmpc = libmpc_py.get_libmpc(1)
# Alocate some memory for the C++ code to read/store the solution
self.mpc_solution = ffi.new("log_t *")
self.cur_state = ffi.new("state_t *")
self.reset()
def setup_mpc(self):
ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
self.libmpc.init()
self.mpc_solution = ffi.new("log_t *")
self.cur_state = ffi.new("state_t *")
self.cur_state[0].v_ego = 0
self.cur_state[0].a_ego = 0
self.l = _LEAD_ACCEL_TAU
def setup_mpc(self):
ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.mpc_solution = ffi.new("log_t *")
self.cur_state = ffi.new("state_t *")
self.cur_state[0].v_ego = 0
self.cur_state[0].a_ego = 0
self.a_lead_tau = _LEAD_ACCEL_TAU
# v_ego = 7.02661012716
# a_ego = -1.26143024772
# x_lead = 29.625 + 20
# v_lead = 0.725235462189 + 1
# a_lead = -1.00025629997
# a_lead_tau = 2.90729817665
#min_a_lead_tau = (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2)
min_a_lead_tau = 0.0
print(a_lead_tau, min_a_lead_tau)
a_lead_tau = max(a_lead_tau, min_a_lead_tau)
ffi, libmpc = libmpc_py.get_libmpc(1)
libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau)
cur_state = ffi.new("state_t *")
cur_state[0].x_ego = 0.0
cur_state[0].v_ego = v_ego
cur_state[0].a_ego = a_ego
cur_state[0].x_l = x_lead
cur_state[0].v_l = v_lead
mpc_solution = ffi.new("log_t *")
for _ in range(10):
print(libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau, a_lead))
