def Polynomial(x_init, x_goal): s0 = x_init[0] rho0 = x_init[1] theta0 = x_init[2] sg = x_goal[0] rhog = x_goal[1] thetag = x_goal[2] A = np.array([[1, s0, s0 ** 2, s0 ** 3], [1, sg, sg ** 2, sg ** 3], [0, 1, 2 * s0, 3 * s0 ** 2], [0, 1, 2 * sg, 3 * sg ** 2]]) b = np.array([rho0, rhog, np.tan(theta0), np.tan(thetag)]) a = solve(A, b) # print(a) s = np.arange(s0, sg, 0.1) rho = a[0] + a[1] * s + a[2] * s ** 2 + a[3] * s ** 3 frenet_theta = [] for i in range(len(s)-1): state0 = [s[i], rho[i]] state1 = [s[i + 1], rho[i + 1]] tmp_theta = heading.calHeadingFrenet(state0, state1) frenet_theta.append(tmp_theta) frenet_theta.extend([frenet_theta[-1]]) return s, rho, frenet_theta
def ToMPC(): efficients = cubicSpline.saveEfficients() rho_vector = ipoptSolver(efficients) theta_rho = [] for i in range(len(s) - 1): state0 = [s[i], rho_vector[i]] state1 = [s[i + 1], rho_vector[i + 1]] tmp_theta = heading.calHeadingFrenet(state0, state1) theta_rho.append(tmp_theta) theta_rho.append(theta_rho[-1]) x = [] y = [] theta = [] # print(s) for j in range(len(s)): tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j], theta_rho[j], efficients) x.append(tmpX) y.append(tmpY) theta.append(tmpTheta) kappa_list = calKappa.path_kappa(rho_vector, s, efficients) return x[0:n_s + 1], y[0:n_s + 1], theta[0:n_s + 1], kappa_list
def plotGraph():
# plot graph
plt.figure(figsize=(3.5, 3.0)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体，则在此处设为：SimHei
p1 = [0.15, 0.15, 0.80, 0.8]
plt.axes(p1)
efficients = cubicSpline.saveEfficients()
rho_vector = ipoptSolver(efficients)
theta_rho = []
for i in range(len(s) - 1):
state0 = [s[i], rho_vector[i]]
state1 = [s[i + 1], rho_vector[i + 1]]
tmp_theta = heading.calHeadingFrenet(state0, state1)
theta_rho.append(tmp_theta)
theta_rho.append(theta_rho[-1])
x = []
y = []
theta = []
# print(s)
for j in range(len(s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j], theta_rho[j], efficients)
x.append(tmpX)
y.append(tmpY)
theta.append(tmpTheta)
tmp_s = []
tmp_rho = []
tmp_thetaRho = []
if showRoughPath:
tmp_s, tmp_rho, tmp_thetaRho = pathPlanner.ToPathOptimizer2(efficients)
if showRefinedPath:
plt.plot(x, y, c='cyan', linewidth=0.6, alpha=1, label='Refined path')
# plot obstacles
if show_obstacle:
for i in range(len(static_obs)):
tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[i][0], static_obs[i][1], obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, 'r', 1)
if show_vehicle_start:
x0, y0, theta0 = ftc.frenetToXY(start_SRho[0], start_SRho[1], start_SRho[2], efficients)
car.simVehicle([x0, y0], theta0, 'blue', 1)
if show_vehicle_animation:
for i in range(0, n_s, 1):
time_stamp = i / n_s + 0.3
if time_stamp > 1.0:
time_stamp = 1.0
if i <= 5:
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
if 5 <= i <= 10:
if (i % 2) == 0:
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
if 10 <= i <= n_s:
if (i % 3) == 0:
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
# plt.pause(0.001)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
# plt.title('x-y Graph', font1)
plt.xlabel('x (m)', font1)
plt.ylabel('y (m)', font1)
plt.xticks(fontproperties='Times New Roman', fontsize=10)
plt.yticks(fontproperties='Times New Roman', fontsize=10)
# plt.legend(loc=0) # 图例位置自动
plt.xlim(-10, 30)
plt.ylim(-5, 80)
plt.savefig('../SimGraph/pathOptimization2Path060406.svg')
plt.savefig('../SimGraph/pathOptimization2Path060406.tiff', dpi=600)
plt.figure(figsize=(3.5, 1.8)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体，则在此处设为：SimHei
p2 = [0.2, 0.25, 0.75, 0.60]
plt.axes(p2)
if showRoughKappa:
rough_kappa = calKappa.path_kappa(tmp_rho, tmp_s, efficients)
print("--------------")
print(len(rough_kappa))
print(len(tmp_s))
plt.plot(tmp_s[0:-2], rough_kappa, c='magenta', linestyle="-", linewidth=0.5, alpha=1,label='Rough Path Curvature Profile')
if showRefinedKappa:
kappa_refined = calKappa.path_kappa(rho_vector, s, efficients)
plt.plot(s[0:n_s + 1], kappa_refined, c='cyan', linestyle="-", linewidth=0.5, alpha=1,
label='Refined Path Curvature Profile')
y = [max(kappa_refined) for i in range(n_s + 1)]
y2 = [min(kappa_refined) for i in range(n_s + 1)]
# plt.plot(s[0:n_s + 1], y, c='k', linestyle="--", linewidth=0.5, alpha=1)
# plt.plot(s[0:n_s + 1], y2, c='k', linestyle="--", linewidth=0.5, alpha=1)
plt.title('Curvature Profile', font1)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
plt.xlabel('s (m)', font1)
plt.ylabel('kappa (1/m)', font1)
plt.xlim(-1, 110)
plt.ylim(-0.1, 0.2)
plt.legend(loc=0) # 图例位置自动
plt.savefig('../SimGraph/pathOptimization2Kappa060406.svg')
plt.savefig('../SimGraph/pathOptimization2Kappa060406.tiff', dpi=600)
plt.show()
return None
def plotGraph():
# plot graph
plt.figure(figsize=(3.5, 3.5 * 0.62)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
p1 = [0.2, 0.2, 0.7, 0.7]
plt.axes(p1)
efficients = cubicSpline.saveEfficients()
rho_vector = ipoptSolver(efficients)
theta_rho = []
for i in range(len(s) - 1):
state0 = [s[i], rho_vector[i]]
state1 = [s[i + 1], rho_vector[i + 1]]
tmp_theta = heading.calHeadingFrenet(state0, state1)
theta_rho.append(tmp_theta)
theta_rho.append(theta_rho[-1])
x = []
y = []
theta = []
# print(s)
for j in range(len(s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j],
theta_rho[j], efficients)
x.append(tmpX)
y.append(tmpY)
theta.append(tmpTheta)
tmp_s = []
tmp_rho = []
tmp_thetaRho = []
if showRoughPath:
tmp_s, tmp_rho, tmp_thetaRho = pathPlanner.ToPathOptimizer5(efficients)
if showRefinedPath:
plt.plot(x[0:-3],
y[0:-3],
c='cyan',
linewidth=0.6,
alpha=1,
label='Refined path')
if showVehicleStart:
tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1)
# print(len(x), len(y), len(theta))
# print(x[0], y[0], theta[0])
# plot obstacles
if showObsDyn:
c = ['r', 'gold', 'lightseagreen']
for i in range(len(static_obs)):
obs_s = [
j for j in np.arange(static_obs[-(i + 1)][0],
static_obs[-(i + 1)][0] + 100, 1)
]
obs_rho = [
static_obs[-(i + 1)][1] for q in np.arange(
static_obs[-(i + 1)][0], static_obs[-(i + 1)][0] + 100, 1)
]
for m in range(0, len(obs_s), 2):
time_stamp = m / len(obs_s) + 0.05
if time_stamp > 1.0:
time_stamp = 1.0
tmpx, tmpy, tmptheta = ftc.frenetToXY(obs_s[m], obs_rho[m],
obstacleHeading,
efficients)
car.simVehicle([tmpx, tmpy], tmptheta, c[i], time_stamp)
if showStaticObstacle:
for i in range(len(static_obs)):
c = ['lightseagreen', 'orange', 'green']
tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[i][0],
static_obs[i][1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, c[i], 1)
if showVehicleAnimation:
for i in range(0, 6, 1):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
for i in range(6, 30, 2):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
for i in range(30, 60, 4):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
# plt.pause(0.001)
for i in range(60, 80, 5):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
for i in range(80, 120, 4):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
for i in range(120, 160, 3):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
# plt.title('x-y Graph', font1)
plt.xlabel('x (m)', font1)
plt.ylabel('y (m)', font1)
plt.xticks(fontproperties='Times New Roman', fontsize=10)
plt.yticks(fontproperties='Times New Roman', fontsize=10)
# plt.legend(loc=0) # 图例位置自动
plt.xlim(300, 500)
plt.ylim(60, 190)
# plt.axis('equal')
plt.savefig('../SimGraph/pathOptimizer_5Path060406.svg')
plt.savefig('../SimGraph/pathOptimizer_5Path060406.tiff', dpi=600)
plt.figure(figsize=(3.5, 1.8)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
p2 = [0.2, 0.25, 0.7, 0.6]
plt.axes(p2)
if showRoughKappa:
print("==================")
print("len:tmp_rho,tmp_s", len(tmp_rho), len(tmp_s))
rough_kappa = calKappa.path_kappa(tmp_rho, tmp_s, efficients)
print("--------------")
print(len(rough_kappa))
plt.plot(tmp_s[0:-2],
rough_kappa,
c='magenta',
linestyle="-",
linewidth=0.5,
alpha=1,
label='Rough Path Curvature Profile')
if showRefinedKappa:
kappa_refined = calKappa.path_kappa(rho_vector, s, efficients)
plt.plot(s[0:-2],
kappa_refined,
c='cyan',
linestyle="-",
linewidth=0.5,
alpha=1,
label='Refined Path Curvature Profile')
# y = [kappa_max for i in range(n_s + 1)]
# y2 = [-kappa_max for i in range(n_s + 1)]
# plt.plot(s[0:n_s + 1], y, c= 'k', linestyle="--", linewidth=0.5, alpha=1)
# plt.plot(s[0:n_s + 1], y2, c= 'k', linestyle="--", linewidth=0.5, alpha=1)
plt.title('Curvature Profile', font1)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
plt.xlabel('s (m)', font1)
plt.ylabel('kappa (1/m)', font1)
plt.xlim(400, 580)
plt.ylim(-0.1, 0.3)
plt.legend(loc=0) # 图例位置自动
plt.savefig('../SimGraph/pathOptimization5Kappa060406.svg')
plt.savefig('../SimGraph/pathOptimization5Kappa060406.tiff', dpi=600)
plt.show()
def plotGraph():
# plot graph
plt.figure(figsize=(3.5, 3.5 * 0.62)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
p1 = [0.2, 0.2, 0.7, 0.7]
plt.axes(p1)
efficients = cubicSpline.saveEfficients()
rho_vector = ipoptSolver(efficients)
theta_rho = []
for i in range(len(s) - 1):
state0 = [s[i], rho_vector[i]]
state1 = [s[i + 1], rho_vector[i + 1]]
tmp_theta = heading.calHeadingFrenet(state0, state1)
theta_rho.append(tmp_theta)
theta_rho.append(theta_rho[-1])
x = []
y = []
theta = []
# print(s)
for j in range(len(s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j],
theta_rho[j], efficients)
x.append(tmpX)
y.append(tmpY)
theta.append(tmpTheta)
tmp_s = []
tmp_rho = []
tmp_thetaRho = []
if showRoughPath:
tmp_s, tmp_rho, tmp_thetaRho = pathPlanner.ToPathOptimizer3(efficients)
if showRefinedPath:
plt.plot(x, y, c='cyan', linewidth=0.6, alpha=1, label='Refined path')
if showVehicleStart:
tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, 'b', 0.8)
# print(len(x), len(y), len(theta))
# print(x[0], y[0], theta[0])
# plot obstacles
# if show_obstacle:
# c=['r', 'gold', 'darkorange']
# for i in range(len(dyn_obs)):
# obs_s = [j for j in np.arange(dyn_obs[-(i+1)][0], dyn_obs[-(i+1)][0]+ 50, 1)]
# obs_rho = [dyn_obs[-(i+1)][1] for q in np.arange(dyn_obs[-(i+1)][0], dyn_obs[-(i+1)][0]+ 50, 1)]
# for m in range(0, len(obs_s), 1):
# time_stamp = m / len(obs_s) + 0.08
# if time_stamp > 1.0:
# time_stamp = 1.0
# tmpx, tmpy, tmptheta = ftc.frenetToXY(obs_s[m], obs_rho[m], obstacleHeading, efficients)
# car.simVehicle([tmpx, tmpy], tmptheta, c[i], time_stamp)
if show_obstacle:
c = ['r', 'gold', 'darkorange']
for i in range(len(static_obs)):
tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[-(i + 1)][0],
static_obs[-(i + 1)][1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, c[i], 0.8)
if show_vehicle_animation:
for i in range(0, n_s, 2):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
# plt.pause(0.001)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
# plt.title('x-y Graph', font1)
plt.xlabel('x (m)', font1)
plt.ylabel('y (m)', font1)
plt.xticks(fontproperties='Times New Roman', fontsize=10)
plt.yticks(fontproperties='Times New Roman', fontsize=10)
# plt.legend(loc=0) # 图例位置自动
plt.xlim(30, 140)
plt.ylim(80, 130)
# plt.axis('equal')
plt.savefig('../SimGraph/pathOptimization3Path060404.svg')
plt.savefig('../SimGraph/pathOptimization3Path060404.tiff', dpi=600)
plt.figure(figsize=(3.5, 1.8)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
p2 = [0.2, 0.25, 0.7, 0.6]
plt.axes(p2)
if showRoughKappa:
rough_kappa = calKappa.path_kappa(tmp_rho, tmp_s, efficients)
print("--------------")
print(len(rough_kappa))
plt.plot(tmp_s[0:-2],
rough_kappa,
c='magenta',
linestyle="-",
linewidth=0.5,
alpha=1,
label='Rough Path Curvature Profile')
if showRefinedKappa:
kappa_list = calKappa.path_kappa(rho_vector, s, efficients)
plt.plot(s[0:n_s + 1],
kappa_list,
c='cyan',
linestyle="-",
linewidth=0.5,
alpha=1,
label='Refined Path Curvature Profile')
y = [max(kappa_list) for i in range(n_s + 1)]
y2 = [min(kappa_list) for i in range(n_s + 1)]
plt.plot(s[0:n_s + 1],
y,
c='k',
linestyle="--",
linewidth=0.5,
alpha=1)
plt.plot(s[0:n_s + 1],
y2,
c='k',
linestyle="--",
linewidth=0.5,
alpha=1)
plt.title('Curvature Profile', font1)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
plt.xlabel('s (m)', font1)
plt.ylabel('kappa (1/m)', font1)
plt.xlim(90, 210)
plt.ylim(-0.1, 0.3)
plt.legend(loc=0) # 图例位置自动
plt.savefig('../SimGraph/pathOptimization3Kappa060404.svg')
plt.savefig('../SimGraph/pathOptimization3Kappa060404.tiff', dpi=600)
plt.show()