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)
