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))