def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0., lane_width=3.6, poly_shift=0.): libmpc = libmpc_py.libmpc libmpc.init(1.0, 1.0, 1.0) mpc_solution = libmpc_py.ffi.new("log_t *") y_pts = poly_shift * np.ones(MPC_N + 1) heading_pts = np.zeros(MPC_N + 1) CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") VM = VehicleModel(CP) curvature_factor = VM.curvature_factor(v_ref) cur_state = libmpc_py.ffi.new("state_t *") cur_state.x = x_init cur_state.y = y_init cur_state.psi = psi_init cur_state.delta = delta_init # converge in no more than 20 iterations for _ in range(20): libmpc.run_mpc(cur_state, mpc_solution, [0, 0, 0, v_ref], curvature_factor, CAR_ROTATION_RADIUS, list(y_pts), list(heading_pts)) return mpc_solution
def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0., l_prob=1., r_prob=1., p_prob=1., poly_l=np.array([0., 0., 0., 1.8]), poly_r=np.array([0., 0., 0., -1.8]), poly_p=np.array([0., 0., 0., 0.]), lane_width=3.6, poly_shift=0.): libmpc = libmpc_py.libmpc libmpc.init(1.0, 3.0, 1.0, 1.0) mpc_solution = libmpc_py.ffi.new("log_t *") p_l = poly_l.copy() p_l[3] += poly_shift p_r = poly_r.copy() p_r[3] += poly_shift p_p = poly_p.copy() p_p[3] += poly_shift d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width, v_ref) CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") VM = VehicleModel(CP) v_ref = v_ref curvature_factor = VM.curvature_factor(v_ref) l_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_l))) r_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_r))) d_poly = libmpc_py.ffi.new("double[4]", list(map(float, d_poly))) cur_state = libmpc_py.ffi.new("state_t *") cur_state[0].x = x_init cur_state[0].y = y_init cur_state[0].psi = psi_init cur_state[0].delta = delta_init # converge in no more than 20 iterations for _ in range(20): libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, d_poly, l_prob, r_prob, curvature_factor, v_ref, lane_width) return mpc_solution
def model_polyfit(points): path_pinv = compute_path_pinv() return np.dot(path_pinv, map(float, points)) xx = [] yy = [] deltas = [] psis = [] times = [] CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") VM = VehicleModel(CP) v_ref = 32.00 # 45 mph curvature_factor = VM.curvature_factor(v_ref) print(curvature_factor) LANE_WIDTH = 3.9 p_l = map(float, model_polyfit(points_l)) p_r = map(float, model_polyfit(points_r)) p_p = map(float, model_polyfit(points_c)) l_poly = libmpc_py.ffi.new("double[4]", p_l) r_poly = libmpc_py.ffi.new("double[4]", p_r) p_poly = libmpc_py.ffi.new("double[4]", p_p) l_prob = 1.0 r_prob = 1.0 p_prob = 1.0 # This is always 1 mpc_x_points = np.linspace(0., 2.5 * v_ref, num=50)
out = VM.steady_state_sol(sa_r[i], u[i]) x[i + 1] = x[i] + np.cos(psi[i]) * u[i] * dt - np.sin(psi[i]) * out[0] * dt y[i + 1] = y[i] + np.sin(psi[i]) * u[i] * dt + np.cos(psi[i]) * out[0] * dt psi[i + 1] = psi[i] + out[1] * dt return x, y, psi if __name__ == "__main__": CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") print(CP) VM = VehicleModel(CP) print(VM.steady_state_sol(.1, 0.15)) print(calc_slip_factor(VM)) print("Curv", VM.curvature_factor(30.)) dt = 0.05 st = 20 u = np.ones(st) * 1. sa = np.ones(st) * 1. out = mpc_path_prediction(sa, u, dt, VM) out_model = model_path_prediction(sa, u, dt, VM) print("mpc", out) print("model", out_model)