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 sampling_test():
k0 = 0.0
l_center = 10.0
l_heading = np.deg2rad(90.0)
l_width = 1.0
v_width = 1.0
d = 10
states = calc_lane_states_only_ones(l_center, l_heading, l_width, v_width,
d)
result = generate_path(states, k0)
table = result[0]
print('table type: ', type(table))
xc, yc, yawc = motion_model.generate_trajectory(table[3], table[4],
table[5], k0)
print('xc type: ', type(xc))
print('yc type: ', type(yc))
print('yawc type: ', type(yawc))
if show_animation:
plt.plot(xc, yc, 'bo')
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 = - np.deg2rad(-10.0)
a_max = np.deg2rad(45.0)
p_min = - np.deg2rad(20.0)
p_max = np.deg2rad(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)
if show_animation:
plt.plot(xc, yc, "-r")
if show_animation:
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)
if show_animation:
plt.plot(xc, yc, "-r")
if show_animation:
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)
if show_animation:
plt.plot(xc, yc, "-r")
if show_animation:
plt.grid(True)
plt.axis("equal")
plt.show()
print("Done")
def lane_state_sampling_test1():
k0 = 0.0
l_center = 10.0
l_heading = np.deg2rad(0.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)
if show_animation:
plt.close("all")
for table in result:
xc, yc, yawc = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
if show_animation:
plt.plot(xc, yc, "-r")
if show_animation:
plt.grid(True)
plt.axis("equal")
plt.show()
def lane_state_sampling_test1():
k0 = 0.0 # curvature constraint
l_center = 10.0 # y coordinate - goal state
d = 10 # x coordinate - goal state
l_heading = np.deg2rad(0.0) # goal state heading angle
l_width = 3.7
v_width = 2.6
nxy = 5 # number of lane center curve
states = calc_lane_states(l_center, l_heading, l_width, v_width, d, nxy)
result = generate_path(states, k0)
if not os.path.isfile("./robotpath.txt"):
os.mknod("./robotpathxc.txt")
else:
os.remove("./robotpath.txt")
os.mknod("./robotpath.txt")
with open("./robotpath.txt", "w") as savefile:
for table in result:
xc, yc, yawc = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
savefile.write(str(xc))
savefile.write("\n")
savefile.write(str(yc))
savefile.write("\n")
if show_animation:
plt.plot(xc, yc, "-r")
savefile.close()
if show_animation:
plt.grid(True)
plt.axis("equal")
plt.show()
def biased_terminal_state_sampling_test2():
k0 = 0.0
nxy = 30
nh = 1
d = 20
a_min = np.deg2rad(0.0)
a_max = np.deg2rad(45.0)
p_min = - np.deg2rad(20.0)
p_max = np.deg2rad(20.0)
ns = 100
goal_angle = np.deg2rad(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)
if show_animation:
plt.plot(xc, yc, "-r")
if show_animation:
plt.grid(True)
plt.axis("equal")
plt.show()
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.matrix(calc_diff(target, xc, yc, yawc)).T
# print(dc.T)
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:
show_trajectory(target, xc, yc)
else:
xc, yc, yawc, p = None, None, None, None
print("cannot calc path")
return xc, yc, yawc, p
def uniform_terminal_state_sampling_test1(cur_states, ax=None):
k0 = 0.0
nxy = 3
nh = 2
d = 2
a_min = - math.radians(45.0)
a_max = math.radians(45.0)
p_min = - math.radians(40.0)
p_max = math.radians(40.0)
# a_min = - math.radians(45.0)
# a_max = math.radians(45.0)
# p_min = - math.radians(45.0)
# p_max = math.radians(45.0)
target_states = calc_uniform_polar_states(cur_states, nxy, nh, d, a_min, a_max, p_min, p_max)
result = generate_path(cur_states,target_states, k0)
for table in result:
print("table", table)
xc, yc, yawc = motion_model.generate_trajectory(cur_states,
table[3], table[4], table[5], k0)
if show_animation:
if ax==None:
plt.plot(xc, yc, "-r")
else:
ax.plot(xc, yc, "-r")
def sampling_test_state_2():
#
# l_center = 10
# l_heading = np.deg2rad(10.0)
# l_width = 1.0
# v_width = 1.0
# d = 20
# print("before calc")
# states = calc_lane_states_only_ones(l_center, l_heading, l_width, v_width, d)
states = [[20, 10, np.deg2rad(70)]]
k0 = 0.0
result = generate_path(states, k0)
print("after generate_path")
n = 1
for table in result:
print(n)
print("before motion")
xc, yc, yawc = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
print("after motion")
n += 1
if show_animation:
plt.plot(xc, yc, 'bo')
plt.grid(True)
plt.axis("equal")
plt.show()
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 biased_terminal_state_sampling_test2(LatticeParameter):
#def biased_terminal_state_sampling_test2():
# k0: radian angle with x-axis;
# nxy: number of curves
# nh: number of heading sampleing
# a_min: position sampling min angle
# a_max: position sampling max angle
# p_min: heading sampling min angle
# p_max: heading sampling max angle
# ns: number of biased sampling
# goal_angle: goal orientation for biased sampling\
# k0 = 0.0 # turning angle
# nxy = 13
# nh = 1
# d = 20 # curve distance
# a_min = np.deg2rad(-60.0)
# a_max = np.deg2rad(45.0)
# p_min = np.deg2rad(40)
# p_max = np.deg2rad(20)
# ns = 100
# goal_angle = np.deg2rad(30)
# velocity = 5
LatticeParameter = LatticeParameter.tolist()
k0 = LatticeParameter[0]
nxy = int(LatticeParameter[1])
nh = int(LatticeParameter[2])
d = int(LatticeParameter[3])
a_min = np.deg2rad(LatticeParameter[4])
a_max = np.deg2rad(LatticeParameter[5])
p_min = -np.deg2rad(LatticeParameter[6])
p_max = np.deg2rad(LatticeParameter[7])
ns = int(LatticeParameter[8])
goal_angle = np.deg2rad(LatticeParameter[9])
velocity = np.deg2rad(LatticeParameter[10])
states = calc_biased_polar_states(goal_angle, ns, nxy, nh, d, a_min, a_max,
p_min, p_max)
result = generate_path(states, k0, velocity)
curvex = []
curvey = []
biased_terminal_state_sampling_test2.index = 0
for table in result:
xc, yc, yawc = motion_model.generate_trajectory(
table[3], table[4], table[5], k0, velocity)
biased_terminal_state_sampling_test2.index += 1
curvex += xc
curvey += yc
if show_animation:
plt.plot(xc, yc, "-r")
if show_animation:
plt.grid(True)
plt.axis("equal")
plt.show()
return [curvex, curvey]
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 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 optimizePath(target, state, params):
p = copy.deepcopy(params)
foundPath = False
for i in range(max_iter):
traj = motion_model.generate_trajectory(p, state)
x = [traj[i].x for i in range(len(traj))]
y = [traj[i].y for i in range(len(traj))]
theta = [traj[i].yaw for i in range(len(traj))]
if np.any(np.array(theta) < 0.01) or np.any(np.array(theta) > 5):
xc, yc, yawc, p = None, None, None, None
break
if ReachedTarget(target, traj[-1]):
# print("found path")
foundPath = True
# snap the last configuration to target state, to avoid discont
# during planning
traj[-1].x = target.x
traj[-1].y = target.y
traj[-1].yaw = target.yaw
traj[-1].k = target.k
break
else:
error = [(target.x - traj[-1].x), (target.y - traj[-1].y),
(target.yaw - traj[-1].yaw), (target.k - traj[-1].k)]
error = np.array(error)
error = error.reshape(error.shape[0], 1)
J = GetJacobian(target, params, deltaParams, state)
try:
dp = -np.dot(np.linalg.inv(J), error)
except np.linalg.linalg.LinAlgError:
print("cannot calc path LinAlgError")
xc, yc, yawc, p = None, None, None, None
break
alpha = learningRate
p += alpha * dp.reshape(dp.shape[0])
if show_animation:
x = [traj[i].x for i in range(len(traj))]
y = [traj[i].y for i in range(len(traj))]
theta = [traj[i].yaw for i in range(len(traj))]
show_trajectory(target, x, y)
return traj, params, foundPath
def sampling_test_state():
k0 = 0.0
states = [[30, 10, np.deg2rad(50)]]
result = generate_path(states, k0)
xc, yc, yawc = motion_model.generate_trajectory(result[0][3], result[0][4],
result[0][5], k0)
if show_animation:
plt.plot(xc, yc, 'bo')
plt.grid(True)
plt.axis("equal")
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 optimization_trajectory(target, k0, p):
"""
轨迹优化
target: end point
k0: initial
p : [s,km,kf]
:return: xc,yc,yawc,p
"""
# 迭代max_iter次
for i in range(max_iter):
# xc,yc,yawc, 按照当前运动模型生成的轨迹的集合
xc, yc, yawc = mm.generate_trajectory(p[0], p[1], p[2], k0)
# 计算目标点与各个轨迹点的diff
dc = np.array(calc_diff(target, xc, yc, yawc)).reshape(3, 1)
# 计算cost
# cost1 = (dx)^2
# cost2 = (dy)^2
# cost3 = (dyaw)^2
cost = np.linalg.norm(dc)
# 选出符合约束的路径
# 未考虑其他因素 如comfor、behavior、safety ...
if cost <= cost_th:
print("Path is ok, current cost is:" + str(cost))
break
# p[s,km,kf]
J = clac_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_params(dp, p, k0, target)
# Update params
p += alpha * np.array(dp)
if show_animation:
show_trajectory(target, xc, yc, yawc)
else:
xc, yc, yawc, p = None, None, None, None
print("cannot calc path!")
return xc, yc, yawc, p
def test_trajectory_generate():
s = 5.0 # [m]
k0 = 0.0
km = math.radians(30.0)
kf = math.radians(-30.0)
# plt.plot(xk, yk, "xr")
# plt.plot(t, kp)
# plt.show()
x, y = motion_model.generate_trajectory(s, km, kf, k0)
plt.plot(x, y, "-r")
plt.axis("equal")
plt.grid(True)
plt.show()
def uniform_terminal_state_sampling1():
k0 = 0.0
np = 5
nh = 3
d = 20
a_min = -math.radians(45.0)
a_max = math.radians(45.0)
p_min = -math.radians(45.0)
p_max = math.radians(45.0)
states = calc_uniform_states(np, 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 sampling_test_3():
k0 = 0.0
deg = 90.0
l_center = 10.0
l_heading = np.deg2rad(deg)
l_width = 3.0
v_width = 1.0
d = 10
for i in range(4):
states = calc_lane_states_only_ones(l_center, l_heading, l_width,
v_width, d)
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, 'bo')
deg = deg - (i + 1) * 10
l_heading = np.deg2rad(deg)
plt.show()
def sampling_test_bo_2():
k0 = 0.0
deg = 90.0
l_center = 15.0
l_heading = np.deg2rad(deg)
l_width = 1.0
v_width = 1.0
d = 10
for i in range(3):
states = calc_lane_states_only_ones(l_center, l_heading, l_width,
v_width, d)
result = generate_path(states, k0)
xc, yc, yawc = motion_model.generate_trajectory(
result[0][3], result[0][4], result[0][5], k0)
if show_animation:
plt.plot(xc, yc, 'bo')
deg = deg - (i + 1) * 10
l_heading = np.deg2rad(deg)
if show_animation:
plt.grid(True)
plt.axis("equal")
plt.show()
def optimize_trajectory(target, k0, p):
mcfg = motion_model.ModelConfig()
for i in range(max_iter):
xc, yc, yawc = motion_model.generate_trajectory(p[0], p[1], p[2], k0)
dc = np.matrix(calc_diff(target, xc, yc, yawc)).T
# print(dc.T)
cost = np.linalg.norm(dc)
if cost <= cost_th:
## optimize success
print("path is ok cost is:" + str(cost))
break
J = calc_J(target, p, h, k0)
try:
dp = -np.linalg.inv(J) * dc # Eq. (14) in paper
dp = limitDp(dp, mcfg, p)
except np.linalg.linalg.LinAlgError:
## optimize fail
print(Fore.YELLOW + "cannot calc path LinAlgError")
xc, yc, yawc, p = None, None, None, None
break
alpha = selection_learning_param(dp, p, k0,
target) # choose learning rate
p += alpha * np.array(dp)
p = limitP(p, mcfg)
# print(p.T)
if show_graph:
show_trajectory(target, xc, yc)
else:
## optimize fail
## if no break, enter here
xc, yc, yawc, p = None, None, None, None
print(Fore.YELLOW + "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)
if show_animation:
plt.plot(xc, yc, "-r")
if show_animation:
plt.grid(True)
plt.axis("equal")
plt.show()
def lane_state_sampling_test1(cur_states, obstacles, params_global, showplot=False, ax=None):
k0 = 0.0
l_center = 1.0
l_center2 = -1.0
l_center3 = 1.0
l_center4 = -1.0
# cur_states[2]=1.0
# l_heading = cur_states[2]+math.radians(10.0)
# l_heading2 = cur_states[2]+math.radians(-10.0)
# l_heading = math.radians(10.0)
l_heading = cur_states[2]
if l_heading<0.0:
l_heading=abs(l_heading)
if l_heading>np.pi/4:
l_heading=np.pi/4
l_width = 1.0
v_width = 0.0
d = 4.0
d2 = 3.5
nxy = 2
targetstates0 = calc_lane_states_linear(l_center, l_heading, l_width, v_width, d, nxy)
targetstates = calc_lane_states(l_center, l_heading, l_width, v_width, d, nxy)
targetstates2 = calc_lane_states( l_center2, l_heading, l_width, v_width, d, nxy)
for tstate in targetstates0:
Target_Outside=False
if tstate[0] > params_global.xmax or tstate[0] < params_global.xmin:
Target_Outside=True
if tstate[1] > params_global.ymax or tstate[1] < params_global.ymin:
Target_Outside=True
if Target_Outside:
targetstates0.remove(tstate)
for tstate in targetstates:
Target_Outside=False
if tstate[0] > params_global.xmax or tstate[0] < params_global.xmin:
Target_Outside=True
if tstate[1] > params_global.ymax or tstate[1] < params_global.ymin:
Target_Outside=True
if Target_Outside:
targetstates.remove(tstate)
for tstate in targetstates2:
Target_Outside=False
if tstate[0] > params_global.xmax or tstate[0] < params_global.xmin:
Target_Outside=True
if tstate[1] > params_global.ymax or tstate[1] < params_global.ymin:
Target_Outside=True
if Target_Outside:
targetstates2.remove(tstate)
''' plot target goals
for tstates in targetstates0:
if ax==None:
plt.scatter(tstates[0], tstates[1], facecolor='black',edgecolor='black') #final point
else:
ax.scatter(tstates[0], tstates[1], facecolor='black',edgecolor='black') #final point
for tstates in targetstates:
if ax==None:
plt.scatter(tstates[0], tstates[1], facecolor='red',edgecolor='red') #final point
else:
ax.scatter(tstates[0], tstates[1], facecolor='red',edgecolor='red') #final point
for tstates in targetstates2:
if ax==None:
plt.scatter(tstates[0], tstates[1], facecolor='blue',edgecolor='blue') #final point
'''
cur_states_mod=[0,0,np.pi/4,0]
# cur_states_mod=[0,0,0.0,0]
result0 = generate_path(cur_states_mod,targetstates0, k0)
result = generate_path(cur_states_mod,targetstates, k0)
result2 = generate_path(cur_states_mod,targetstates2, k0)
# print("cur_states[0]", cur_states[0])
# results =[result0, result, result2]
# print("results", results)
# input()
trjs=[]
for table in result0:
xc, yc, yawc = motion_model.generate_trajectory(cur_states_mod,
table[3], table[4], table[5], k0)
for i in range(len(xc)):
if cur_states[2]>np.pi/2 and cur_states[2]<3*np.pi/2:
xc[i]=-xc[i]
xc[i]+=cur_states[0]
for j in range(len(yc)):
if cur_states[2]>np.pi:
yc[j]=-yc[j]
yc[j]+=cur_states[1]
#checking obstacles
obstacle_free=True
last_x = xc[-1]
last_y = yc[-1]
for obs in obstacles:
if obs.check_point_obstaclefree(last_x, last_y)==False:
obstacle_free=False
#checking global boundary
margin=1.0
if params_global.xmin+margin >last_x or params_global.xmax-margin <last_x:
obstacle_free=False
if params_global.ymin+margin >last_y or params_global.ymax-margin <last_y:
obstacle_free=False
if obstacle_free:
trjs.append([xc, yc, yawc])
for table in result:
xc, yc, yawc = motion_model.generate_trajectory(cur_states_mod,
table[3], table[4], table[5], k0)
for i in range(len(xc)):
if cur_states[2]>np.pi/2 and cur_states[2]<3*np.pi/2:
xc[i]=-xc[i]
xc[i]+=cur_states[0]
for j in range(len(yc)):
if cur_states[2]>np.pi:
yc[j]=-yc[j]
yc[j]+=cur_states[1]
#checking obstacles
obstacle_free=True
last_x = xc[-1]
last_y = yc[-1]
for obs in obstacles:
if obs.check_point_obstaclefree(last_x, last_y)==False:
obstacle_free=False
#checking global boundary
margin=1.0
if params_global.xmin+margin >last_x or params_global.xmax-margin <last_x:
obstacle_free=False
if params_global.ymin+margin >last_y or params_global.ymax-margin <last_y:
obstacle_free=False
if obstacle_free:
trjs.append([xc, yc, yawc])
for table in result2:
xc, yc, yawc = motion_model.generate_trajectory(cur_states_mod,
table[3], table[4], table[5], k0)
for i in range(len(xc)):
if cur_states[2]>np.pi/2 and cur_states[2]<3*np.pi/2:
xc[i]=-xc[i]
xc[i]+=cur_states[0]
for j in range(len(yc)):
if cur_states[2]>np.pi:
yc[j]=-yc[j]
yc[j]+=cur_states[1]
#checking obstacles
obstacle_free=True
last_x = xc[-1]
last_y = yc[-1]
for obs in obstacles:
if obs.check_point_obstaclefree(last_x, last_y)==False:
obstacle_free=False
#checking global boundary
margin=1.0
if params_global.xmin+margin >last_x or params_global.xmax-margin <last_x:
obstacle_free=False
if params_global.ymin+margin >last_y or params_global.ymax-margin <last_y:
obstacle_free=False
if obstacle_free:
trjs.append([xc, yc, yawc])
# if show_animation:
# if ax==None:
# plt.plot(xc, yc, "-r")
# else:
# ax.plot(xc, yc, "-r")
# Rot1 = np.matrix([[math.cos(yaw), math.sin(yaw)],
# [-math.sin(yaw), math.cos(yaw)]])
yaw=np.pi/3
Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)],
[math.sin(yaw), math.cos(yaw)]])
Rot2 = np.array([[math.cos(-yaw), -math.sin(-yaw)],
[math.sin(-yaw), math.cos(-yaw)]])
#reconstructing trjaectories
totaltrjs=[]
newtrjs=[]
for trj in trjs:
y_temps=[]
x_temps=[]
xtemp_rots = []
ytemp_rots = []
for i in range(len(trj[1])):
ytemp=-(1*trj[1][i]-cur_states[1])+cur_states[1]
xtemp=-(1*trj[0][i]-cur_states[0])+cur_states[0]
y_temps.append(ytemp)
x_temps.append(xtemp)
# xtemp_rot = trj[0][i]
# ytemp_rot = trj[1][i]
xtemp_rot = trj[0][i]-cur_states[0]
ytemp_rot = trj[1][i]-cur_states[1]
xtemp_rots.append(xtemp_rot)
ytemp_rots.append(ytemp_rot)
b = [xtemp_rots, ytemp_rots]
c = [xtemp_rots, ytemp_rots]
b = np.dot(Rot1, b)
c = np.dot(Rot2, c)
# d = np.zeros([2,len(xtemp_rots)])
dx = []
dy = []
ex = []
ey = []
for i in range(len(c[0])):
b[0][i]= b[0][i]+cur_states[0]
b[1][i]= b[1][i]+cur_states[1]
c[0][i]= c[0][i]+cur_states[0]
c[1][i]= c[1][i]+cur_states[1]
dx.append(b[0][i])
dy.append(b[1][i])
ex.append(c[0][i])
ey.append(c[1][i])
newtrj=[trj[0], y_temps, trj[2]]
newtrj2=[x_temps, trj[1], trj[2]]
newtrj3=[x_temps, y_temps, trj[2]]
rottrjs=[dx, dy,trj[2]]
rottrjs2=[ex, ey,trj[2]]
newtrjs.append(newtrj)
newtrjs.append(newtrj2)
newtrjs.append(newtrj3)
newtrjs.append(rottrjs)
newtrjs.append(rottrjs2)
totaltrjs.append(trj)
for trj in newtrjs:
#check_trajectories_with_obstalces()
#checking obstacles
obstacle_free=True
last_x = trj[0][-1]
last_y = trj[1][-1]
for obs in obstacles:
if obs.check_point_obstaclefree(last_x, last_y)==False:
obstacle_free=False
#checking global boundary
margin=1.0
if params_global.xmin+margin >last_x or params_global.xmax-margin <last_x:
obstacle_free=False
if params_global.ymin+margin >last_y or params_global.ymax-margin <last_y:
obstacle_free=False
if obstacle_free:
totaltrjs.append(trj)
# if showplot:
# for trj in totaltrjs:
# if ax==None:
# plt.plot(trj[0], trj[1], "-r")
# else:
# ax.plot(trj[0], trj[1], "-r")
return totaltrjs
'''
def GetJacobian(target, params, deltaParams, state):
J = []
newParam = params
newParam[0] = newParam[0] + deltaParams[0]
traj = motion_model.generate_trajectory(newParam, state)
dp = [(target.x - traj[-1].x), (target.y - traj[-1].y),
(target.yaw - traj[-1].yaw), (target.k - traj[-1].k)]
newParam = params
newParam[0] = newParam[0] - deltaParams[0]
traj = motion_model.generate_trajectory(newParam, state)
dn = [(target.x - traj[-1].x), (target.y - traj[-1].y),
(target.yaw - traj[-1].yaw), (target.k - traj[-1].k)]
dp = np.array(dp)
dn = np.array(dn)
J1 = (dp - dn) / (2 * deltaParams[0])
J.append(J1)
newParam = params
newParam[1] = newParam[1] + deltaParams[1]
traj = motion_model.generate_trajectory(newParam, state)
dp = [(target.x - traj[-1].x), (target.y - traj[-1].y),
(target.yaw - traj[-1].yaw), (target.k - traj[-1].k)]
newParam = params
newParam[1] = newParam[1] - deltaParams[1]
traj = motion_model.generate_trajectory(newParam, state)
dn = [(target.x - traj[-1].x), (target.y - traj[-1].y),
(target.yaw - traj[-1].yaw), (target.k - traj[-1].k)]
dp = np.array(dp)
dn = np.array(dn)
J1 = (dp - dn) / (2 * deltaParams[1])
J.append(J1)
newParam = params
newParam[2] = newParam[2] + deltaParams[2]
traj = motion_model.generate_trajectory(newParam, state)
dp = [(target.x - traj[-1].x), (target.y - traj[-1].y),
(target.yaw - traj[-1].yaw), (target.k - traj[-1].k)]
newParam = params
newParam[2] = newParam[2] - deltaParams[2]
traj = motion_model.generate_trajectory(newParam, state)
dn = [(target.x - traj[-1].x), (target.y - traj[-1].y),
(target.yaw - traj[-1].yaw), (target.k - traj[-1].k)]
dp = np.array(dp)
dn = np.array(dn)
J1 = (dp - dn) / (2 * deltaParams[2])
J.append(J1)
newParam = params
newParam[3] = newParam[3] + deltaParams[3]
traj = motion_model.generate_trajectory(newParam, state)
dp = [(target.x - traj[-1].x), (target.y - traj[-1].y),
(target.yaw - traj[-1].yaw), (target.k - traj[-1].k)]
newParam = params
newParam[3] = newParam[3] - deltaParams[3]
traj = motion_model.generate_trajectory(newParam, state)
dn = [(target.x - traj[-1].x), (target.y - traj[-1].y),
(target.yaw - traj[-1].yaw), (target.k - traj[-1].k)]
dp = np.array(dp)
dn = np.array(dn)
J1 = (dp - dn) / (2 * deltaParams[3])
J.append(J1)
J = np.array(J).T
return J
