def long_term_MPC(ego_car: Car, route_xs, route_ys, dt):
assert len(route_xs) == len(route_ys)
N = len(route_xs)
ego_car_x, ego_car_y, ego_car_v, ego_car_theta, ego_car_a, ego_car_steer = ego_car.get_car_state(
)
x_init = [ego_car_x] * N
y_init = [ego_car_y] * N
theta_init = [ego_car_theta] * N
v_init = [ego_car_v] * N
steer_init = [ego_car_steer] * (N - 1)
a_init = [MAX_a] * (N - 1)
initial_value = x_init + y_init + theta_init + v_init + \
steer_init + a_init
# vars to be optimize
x = SX.sym('x', N)
y = SX.sym('y', N)
theta = SX.sym('theta', N)
v = SX.sym('v', N)
steer = SX.sym('steer', N - 1)
a = SX.sym('a', N - 1)
all_vars = vertcat(x, y, theta, v, steer, a)
# vars upper bound
ub_constrains_x = np.array([ego_car_x] + [INF] * (N - 1))
ub_constrains_y = np.array([ego_car_y] + [INF] * (N - 1))
ub_constrains_theta = np.array([ego_car_theta] + [INF] * (N - 1))
ub_constrains_v = np.array([ego_car_v] + [MAX_v] * (N - 1))
ub_constrains_steer = np.array([ego_car_steer] + [MAX_steer] * (N - 2))
ub_constrains_a = np.array([ego_car_a] + [MAX_a] * (N - 2))
ub_constrains_vars = np.hstack([
ub_constrains_x, ub_constrains_y, ub_constrains_theta, ub_constrains_v,
ub_constrains_steer, ub_constrains_a
])
# vars lower bound
lb_constrains_x = np.array([ego_car_x] + [-INF] * (N - 1))
lb_constrains_y = np.array([ego_car_y] + [-INF] * (N - 1))
lb_constrains_theta = np.array([ego_car_theta] + [-INF] * (N - 1))
lb_constrains_v = np.array([ego_car_v] + [-MIN_v] * (N - 1))
lb_constrains_steer = np.array([ego_car_steer] + [MIN_steer] * (N - 2))
lb_constrains_a = np.array([ego_car_a] + [MIN_a] * (N - 2))
lb_constrains_vars = np.hstack([
lb_constrains_x, lb_constrains_y, lb_constrains_theta, lb_constrains_v,
lb_constrains_steer, lb_constrains_a
])
# define constrain function g (variables update equation)
x_constrain = SX.sym('x_constrain', N - 1)
y_constrain = SX.sym('y_constrain', N - 1)
theta_constrain = SX.sym('theta_constrain', N - 1)
v_constrain = SX.sym('v_constrain', N - 1)
SCALE = 0.002
for i in range(N - 1):
theta_diff = atan(tan(steer[i]) / 2) * v[i] * dt * SCALE
# theta_diff = steer[i] * dt
x_constrain[i] = x[i + 1] - (x[i] + v[i] * dt * np.cos(theta[i]))
y_constrain[i] = y[i + 1] - (y[i] + v[i] * dt * np.sin(theta[i]))
theta_constrain[i] = theta[i + 1] - (theta[i] - theta_diff)
v_constrain[i] = v[i + 1] - (v[i] + a[i] * dt)
all_constrain = vertcat(x_constrain, y_constrain, theta_constrain,
v_constrain)
ub_constrains_g = np.zeros([4 * (N - 1)])
lb_constrains_g = np.zeros([4 * (N - 1)])
# define cost function f
cost = 0
for i in range(N):
# deviation
cost += 20 / N**3 * (N - i)**4 * (x[i] - route_xs[i])**2
cost += 20 / N**3 * (N - i)**4 * (y[i] - route_ys[i])**2
# control cost
if i < N - 2:
cost += 5 * N * steer[i]**2
cost += 0.01 * N * a[i]**2
cost += 20 * N * (steer[i + 1] - steer[i])**2
# cost += 0.1 * N * (a[i + 1] - a[i]) ** 2
nlp = {'x': all_vars, 'f': cost, 'g': all_constrain}
S = nlpsol('S', 'ipopt', nlp, {
"print_time": False,
"ipopt": {
"print_level": 0
}
})
result = S(x0=initial_value,
lbg=lb_constrains_g,
ubg=ub_constrains_g,
lbx=lb_constrains_vars,
ubx=ub_constrains_vars)
# def print_result():
# print('route_x', [i[0] for i in route])
# print('x', result['x'][0:N])
# print('route_y', [i[1] for i in route])
# print('y', result['x'][N:2 * N])
#
# print('theta', result['x'][2 * N:3 * N])
# print('v', result['x'][3 * N:4 * N])
# print('vx', result['x'][3 * N:4 * N] * np.cos(result['x'][2 * N:3 * N]))
# print('vy', result['x'][3 * N:4 * N] * np.sin(result['x'][2 * N:3 * N]))
#
# print('steer', result['x'][4 * N:5 * N - 1])
# print('a', result['x'][5 * N - 1:6 * N - 2])
# print_result()
xs = result['x'][0:N].elements()
ys = result['x'][N:2 * N].elements()
steer = float(result['x'][4 * N + 1])
a = float(result['x'][5 * N])
a = a / MAX_a
if a > 0:
control = Control(steer, a / 10, 0)
else:
control = Control(steer, 0, -a)
# controls = []
# for a, steer in zip(result['x'][5 * N - 1:6 * N - 2].elements(), result['x'][4 * N:5 * N - 1].elements()):
# # steer = float(result['x'][4 * N + 1])
# # a = float(result['x'][5 * N])
# a = a / MAX_a
# if a > 0:
# controls.append(Control(steer, a / 10, 0))
# else:
# controls.append(Control(steer, 0, -a))
return control, xs, ys
def short_term_MPC(ego_car: Car, route_xs, route_ys, dt, verbose=False):
assert len(route_xs) == len(route_ys)
N = len(route_xs)
ego_car_x, ego_car_y, ego_car_v, ego_car_theta, ego_car_a, ego_car_steer = ego_car.get_car_state(
)
x_init = [ego_car_x] * N
y_init = [ego_car_y] * N
theta_init = [ego_car_theta] * N
v_init = [ego_car_v] * N
steer_init = [ego_car_steer] * (N - 1)
a_init = [MAX_a] * (N - 1)
initial_value = x_init + y_init + theta_init + v_init + \
steer_init + a_init
# vars to be optimize
x = SX.sym('x', N)
y = SX.sym('y', N)
theta = SX.sym('theta', N)
v = SX.sym('v', N)
steer = SX.sym('steer', N - 1)
a = SX.sym('a', N - 1)
all_vars = vertcat(x, y, theta, v, steer, a)
# vars upper bound
ub_constrains_x = np.array([ego_car_x] + [INF] * (N - 1))
ub_constrains_y = np.array([ego_car_y] + [INF] * (N - 1))
ub_constrains_theta = np.array([ego_car_theta] + [INF] * (N - 1))
ub_constrains_v = np.array([ego_car_v] + [MAX_v] * (N - 1))
ub_constrains_steer = np.array([ego_car_steer] + [MAX_steer] * (N - 2))
ub_constrains_a = np.array([ego_car_a] + [MAX_a] * (N - 2))
ub_constrains_vars = np.hstack([
ub_constrains_x, ub_constrains_y, ub_constrains_theta, ub_constrains_v,
ub_constrains_steer, ub_constrains_a
])
# vars lower bound
lb_constrains_x = np.array([ego_car_x] + [-INF] * (N - 1))
lb_constrains_y = np.array([ego_car_y] + [-INF] * (N - 1))
lb_constrains_theta = np.array([ego_car_theta] + [-INF] * (N - 1))
lb_constrains_v = np.array([ego_car_v] + [-MIN_v] * (N - 1))
lb_constrains_steer = np.array([ego_car_steer] + [MIN_steer] * (N - 2))
lb_constrains_a = np.array([ego_car_a] + [MIN_a] * (N - 2))
lb_constrains_vars = np.hstack([
lb_constrains_x, lb_constrains_y, lb_constrains_theta, lb_constrains_v,
lb_constrains_steer, lb_constrains_a
])
# define constrain function g (variables update equation)
x_constrain = SX.sym('x_constrain', N - 1)
y_constrain = SX.sym('y_constrain', N - 1)
theta_constrain = SX.sym('theta_constrain', N - 1)
v_constrain = SX.sym('v_constrain', N - 1)
SCALE = 0.002
for i in range(N - 1):
# theta_diff = atan(tan(steer[i]) / 2) * v[i] * dt * SCALE
theta_diff = steer[i] * v[i] * dt * SCALE
x_constrain[i] = x[i + 1] - (x[i] + v[i] * dt * np.cos(theta[i]))
y_constrain[i] = y[i + 1] - (y[i] + v[i] * dt * np.sin(theta[i]))
theta_constrain[i] = theta[i + 1] - (theta[i] - theta_diff)
v_constrain[i] = v[i + 1] - (v[i] + a[i] * dt)
all_constrain = vertcat(x_constrain, y_constrain, theta_constrain,
v_constrain)
ub_constrains_g = np.zeros([4 * (N - 1)])
lb_constrains_g = np.zeros([4 * (N - 1)])
# define cost function f
cost = 0
for i in range(N):
# deviation
cost += 20 * N * (x[i] - route_xs[i])**2
cost += 20 * N * (y[i] - route_ys[i])**2
if i < N - 2:
cost += 1 * N * steer[i]**2
# cost += 0.01 * N * a[i] ** 2
cost += 50 * N * (steer[i + 1] - steer[i])**2
nlp = {'x': all_vars, 'f': cost, 'g': all_constrain}
S = nlpsol('S', 'ipopt', nlp, {
"print_time": False,
"ipopt": {
"print_level": 0
}
})
result = S(x0=initial_value,
lbg=lb_constrains_g,
ubg=ub_constrains_g,
lbx=lb_constrains_vars,
ubx=ub_constrains_vars)
steer = float(result['x'][4 * N + 1])
a = float(result['x'][5 * N])
a = a / MAX_a
if a > 0:
action = Control(steer, a / 10, 0)
else:
action = Control(steer, 0, -a)
if verbose:
xs = result['x'][0:N].elements()
ys = result['x'][N:2 * N].elements()
pprint(xs)
pprint(ys)
return action, None, None