def optimize_trajectory(target, k0, p):
for i in range(max_iter):
xc, yc, yawc = motion_model.generate_trajectory(p[0], p[1], p[2], k0)
dc = np.array(calc_diff(target, xc, yc, yawc)).reshape(3, 1)
cost = np.linalg.norm(dc)
if cost <= cost_th:
print("path is ok cost is:" + str(cost))
break
J = calc_j(target, p, h, k0)
try:
dp = -np.linalg.inv(J) @ dc
except np.linalg.linalg.LinAlgError:
print("cannot calc path LinAlgError")
xc, yc, yawc, p = None, None, None, None
break
alpha = selection_learning_param(dp, p, k0, target)
p += alpha * np.array(dp)
# print(p.T)
if show_animation: # pragma: no cover
show_trajectory(target, xc, yc)
else:
xc, yc, yawc, p = None, None, None, None
print("cannot calc path")
return xc, yc, yawc, p
def lane_state_sampling_test1():
k0 = 0.0
l_center = 10.0
l_heading = math.radians(90.0)
l_width = 3.0
v_width = 1.0
d = 10
nxy = 5
states = calc_lane_states(l_center, l_heading, l_width, v_width, d, nxy)
result = generate_path(states, k0)
for table in result:
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()
def uniform_terminal_state_sampling_test2():
k0 = 0.1
nxy = 6
nh = 3
d = 20
a_min = -math.radians(-10.0)
a_max = math.radians(45.0)
p_min = -math.radians(20.0)
p_max = math.radians(20.0)
states = calc_uniform_polar_states(nxy, nh, d, a_min, a_max, p_min, p_max)
result = generate_path(states, k0)
for table in result:
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 biased_terminal_state_sampling_test2():
k0 = 0.0
nxy = 30
nh = 1
d = 20
a_min = math.radians(0.0)
a_max = math.radians(45.0)
p_min = -math.radians(20.0)
p_max = math.radians(20.0)
ns = 100
goal_angle = math.radians(30.0)
states = calc_biased_polar_states(goal_angle, ns, nxy, nh, d, a_min, a_max,
p_min, p_max)
result = generate_path(states, k0)
for table in result:
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()