def test_optimize_path(x=25.0, y=5.0):
# target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0))
target = motion_model.State(x=x, y=y, yaw=np.deg2rad(0.0), s=0.0, k=0.0)
initial_state = motion_model.State(x=0.0,
y=0.0,
yaw=np.deg2rad(0.0),
s=0.0,
k=0.0)
init_p = np.array([x, 0.0, 0.0, 0.0])
traj, params, foundPath = optimizePath(target, initial_state, init_p)
if foundPath:
x = [traj[i].x for i in range(len(traj))]
y = [traj[i].y for i in range(len(traj))]
yaw = [traj[i].yaw for i in range(len(traj))]
for i in range(len(x)):
plot_arrow(x[i], y[i], yaw[i], length=0.1)
# if plot_trajectories:
# show_trajectory(target, x, y)
# plt.axis("equal")
# plt.grid(True)
# plt.show()
return traj
else:
return []
def test_optimize_speed_profile(initialSpeed=10.,
initial_aceleration=0.,
final_s=30.,
final_t=20.,
maxSpeed=20.,
minSpeed=1.,
max_acc=4.):
speeds = np.arange(maxSpeed, minSpeed, -1.)
curvatures = np.arange(max_acc, -max_acc, -1.)
# for l in range(curvatures.shape[0]):
for k in range(speeds.shape[0]):
initial_state = motion_model.State(x=0.,
y=0.,
yaw=1. / initialSpeed,
s=0.0,
k=initial_aceleration)
target = motion_model.State(x=final_s,
y=final_t,
yaw=1. / speeds[k],
s=0.0,
k=0.)
# initialize parameters
init_p = np.array(
[final_s / math.cos(1.0 / initialSpeed), 0.0, 0.0, 0.0])
traj, params, foundPath = optimizePath(target, initial_state, init_p)
if foundPath:
x = [traj[i].x for i in range(len(traj))]
y = [traj[i].y for i in range(len(traj))]
yaw = [traj[i].yaw for i in range(len(traj))]
curvatures = [traj[i].k for i in range(len(traj))]
pathValid = True
for j in range(len(curvatures)):
if math.fabs(curvatures[j]) > max_acc or yaw[j] < (
1. / maxSpeed) or yaw[j] < 0.:
pathValid = False
break
if pathValid:
for i in range(len(x)):
plot_arrow(x[i], y[i], yaw[i], length=0.1)
if plot_trajectories:
show_trajectory(target, x, y)
plt.axis("equal")
plt.grid(True)
# plt.show()
return traj
return []
def GetTrajectory(path, speed, acc):
if len(path) == 0:
return []
traj = []
newState = motion_model.State(x=path[0].x,
y=path[0].y,
yaw=path[0].yaw,
s=path[0].s,
k=path[0].k)
newState.v = speed
newState.a = acc
newState.t = 0
traj.append(newState)
lastCurvature = path[0].k
if acc == -1:
acc = ((speed - speedDiscretization)**2 - speed**2) / (2 * path[-1].s)
elif acc == 1:
acc = ((speed + speedDiscretization)**2 - speed**2) / (2 * path[-1].s)
for i in range(1, len(path)):
state = path[i]
if speed**2 + 2 * acc * state.s < 0:
return []
else:
v = np.sqrt(speed**2 + 2 * acc * state.s)
if acc == 0.0:
t = state.s / speed
else:
t = (v - speed) / acc
if v >= initialSpeed and t > 0 and v <= maxSpeed:
newState = motion_model.State(x=state.x,
y=state.y,
yaw=state.yaw,
s=state.s,
k=state.k)
newState.v = v
newState.a = acc
newState.t = t
if np.abs(state.k - lastCurvature) / t > maxCurvatureRate:
return []
else:
traj.append(newState)
else:
return []
return traj
def generate_path(target_states, k0):
"""
产生路径
:param target_states: 目标状态
:param k0: 出事curvature
:return:
"""
# x,y,yaw,s,km,kf (lookuptable[i])
lookup_table = get_loopup_table()
result = []
for state in target_states:
# from current state to target_state
bestp = search_nearest_one_from_lookuptable(
state[0], state[1], state[2],
lookup_table) # best control factor --- (s,km,kf)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
# initial state
init_p = np.array(
[math.sqrt(state[0]**2 + state[1]**2), bestp[4],
bestp[5]]).reshape(3, 1)
x, y, yaw, p = planner.optimization_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
result.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
print("finish path generation")
return result
def generate_path(states, k0):
# x, y, yaw, s, km, kf
lookup_table = get_lookup_table()
result = []
for state in states:
bestp = search_nearest_one_from_lookuptable(state[0], state[1],
state[2], lookup_table)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.matrix(
[math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).T
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
result.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
print("finish path generation")
return result
def generate_path(target_states, k0):
# x, y, yaw, s, km, kf
lookup_table = get_lookup_table()
result = []
for state in target_states:
bestp = search_nearest_one_from_lookuptable(state[0], state[1],
state[2], lookup_table)
print("x,y,yaw,s,km,kf")
print("bestp : ", bestp)
target = motion_model.State(x=state[0], y=state[1],
yaw=state[2]) # State class constructor
init_p = np.array(
[math.sqrt(state[0]**2 + state[1]**2), bestp[4],
bestp[5]]).reshape(3, 1) # 거리, s, km
print("init_p : ", init_p)
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
result.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
print("finish path generation")
return result
def generate_lookup_table():
states = calc_states_list()
k0 = 0.0
# x, y, yaw, s, km, kf
lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
for state in states:
bestp = search_nearest_one_from_lookuptable(state[0], state[1],
state[2], lookuptable)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.matrix(
[math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).T
print("bestp", bestp)
print("initp", init_p)
if init_p[0] == 0.0:
print("hello")
init_p[0] = 0.1
cur_states = [0, 0, 0, 0]
x, y, yaw, p = planner.optimize_trajectory(cur_states, target, k0,
init_p)
if x is not None:
print("find good path")
lookuptable.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
print("finish lookup table generation")
save_lookup_table("lookuptables.csv", lookuptable)
# print("table[3]", table[4])
# print("table[4]", table[4])
# print("table[5]", table[5])
for table in lookuptable:
if table[3] != 0:
xc, yc, yawc = motion_model.generate_trajectory(
cur_states, table[3], table[4], table[5], k0)
plt.plot(xc, yc, "-r")
# xc, yc, yawc = motion_model.generate_trajectory(cur_states,
# table[3], -table[4], -table[5], k0)
# plt.plot(xc, yc, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()
print("Done")
def generate_lookup_table():
states = calc_states_list() # calculate target states
k0 = 0.0
# target_x, target_y, target_yaw -> s, km, kf
lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
print("Lookup Table is being generated......")
for idx, state in enumerate(states):
bestp = search_nearest_one_from_lookuptable(state[0], state[1],
state[2], lookuptable)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
# init_p = np.matrix(
# [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
init_p = np.matrix([bestp[3], bestp[4], bestp[5]]).T
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
lookuptable.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
## print percent of progress
percent = 100 * (idx + 1.0) / len(states)
print("Complete %{}...".format(percent))
print("finish lookup table generation")
save_lookup_table("lookuptable.csv", lookuptable)
for table in lookuptable:
xc, yc, yawc = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
plt.plot(xc, yc, "-r")
xc, yc, yawc = motion_model.generate_trajectory(
table[3], -table[4], -table[5], k0
) # symmetrical, this is why target_yaw inlcude only -30 degree and exclude +30 degree
# similar for target_y, note that not for target_x
# also note that here change the sign of steering make the sign of target_yaw
# and target_y change at the same time
plt.plot(xc, yc, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()
print("Done")
def test_optimize_trajectory():
# target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0))
target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(90.0))
k0 = 0.0
init_p = np.matrix([6.0, 0.0, 0.0]).T
x, y, yaw, p = optimize_trajectory(target, k0, init_p)
show_trajectory(target, x, y)
# plt.plot(x, y, "-r")
plot_arrow(target.x, target.y, target.yaw)
plt.axis("equal")
plt.grid(True)
plt.show()
def test_optimize_trajectory():
# target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0))
target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(90.0))
k0 = 0.0
init_p = np.array([6.0, 0.0, 0.0]).reshape(3, 1)
x, y, yaw, p = optimize_trajectory(target, k0, init_p)
if show_animation:
show_trajectory(target, x, y)
plot_arrow(target.x, target.y, target.yaw)
plt.axis("equal")
plt.grid(True)
plt.show()
def generate_lookup_table():
states = calc_states_list()
k0 = 0.0
# x, y, yaw, s, km, kf
lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
for state in states:
bestp = search_nearest_one_from_lookuptable(state[0], state[1],
state[2], lookuptable)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.array(
[math.sqrt(state[0]**2 + state[1]**2), bestp[4],
bestp[5]]).reshape(3, 1)
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
lookuptable.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
print("finish lookup table generation")
save_lookup_table("lookuptable.csv", lookuptable)
for table in lookuptable:
xc, yc, yawc = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
plt.plot(xc, yc, "-r")
xc, yc, yawc = motion_model.generate_trajectory(
table[3], -table[4], -table[5], k0)
plt.plot(xc, yc, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()
print("Done")
def test_optimize_trajectory(): # pragma: no cover
# Target (preset)
target = mm.State(x=10.0, y=0.0, yaw=np.deg2rad(60.0))
k0 = 0.0 # initial curvature: 0 1/m
init_p = np.array([6.0, 0.0, np.deg2rad(45)]).reshape(3, 1)
x, y, yaw, p = optimization_trajectory(target, k0, init_p)
if show_animation:
show_trajectory(target, x, y, yaw)
plot_arrow(target.x, target.y, target.yaw)
plt.axis("equal")
plt.grid(True)
plt.show()
plt.plot(x, y, "r--")
plt.show()
def test_optimize_trajectory():
mcfg = motion_model.ModelConfig()
# target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0))
target = motion_model.State(x=1.0, y=0.0, yaw=math.radians(30.0))
k0 = 0.0
init_p_len = math.sqrt(target.x**2 + target.y**2)
init_p = np.matrix([init_p_len, 0.0, 0.0]).T
init_p = limitP(init_p, mcfg)
x, y, yaw, p = optimize_trajectory(target, k0, init_p)
p = limitP(p, mcfg)
show_trajectory(target, x, y)
matplotrecorder.save_movie("animation.mp4", 0.1)
# plt.plot(x, y, "-r")
plot_arrow(target.x, target.y, target.yaw)
plt.axis("equal")
plt.grid(True)
plt.show()
