def path_track4(path, thetas, x_lim, y_lim, x_traj_tot, y_traj_tot, goals):
# thetas = 0 #cancelling user defined theta
win_zoom = 7
"""
SAMPLING STARTS HERE
"""
final_path = []
x, y = path[0]
sample_rate = 1 #best was 2 #changed again from 0.2
final_path.append([x, y])
for x, y in path:
xf, yf = final_path[-1]
if m.sqrt((xf - x)**2 + (yf - y)**2) > sample_rate:
final_path.append([x, y])
else:
continue
if path[-1] not in final_path:
final_path.append(path[-1])
"""
SAMPLING FINISHES HERE
"""
prev_path = path
path = final_path
prev_path_array = np.array(prev_path)
tic = time.time()
xstart, ystart = path[0]
start = [xstart, ystart, thetas]
goal_points = path
dummy_gp = copy.deepcopy(goal_points)
#need to calculate goal theta last two points
last_pt = dummy_gp[-1]
second_last_pt = dummy_gp[-2]
theta_g = -m.atan2((last_pt[1] - second_last_pt[1]),
(last_pt[0] - second_last_pt[0]))
goalx, goaly = goal_points[-1]
goal = [goalx, goaly, theta_g]
x, y, theta = start
v = 1
s = 0
gp_array = np.array(goal_points)
x_traj = []
y_traj = []
skip = False
dist_thresh = 0.1 #0.05
while m.sqrt((x - goal[0])**2 + (y - goal[1])**2) > dist_thresh:
#first step would be to find the target point
target, proximity, skip, perp, phi, beta = calc_target(
x, y, theta, dummy_gp)
xt, yt = target
if (proximity < 0.1 or skip == True) and len(dummy_gp) > 2:
dummy_gp.pop(0)
if len(dummy_gp) == 2:
print(dummy_gp)
angle = theta_g
xt = last_pt[0] + 0 * m.cos(theta_g)
yt = last_pt[1] + 0 * m.sin(-theta_g)
plt.plot(xt, yt, 'ko')
if skip == True:
#need to set the value of target to something
target, proximity, skip, perp, phi, beta = calc_target(
x, y, theta, dummy_gp)
print(skip)
plt.cla()
plt.axis('scaled')
plt.xlim(x - win_zoom, x + win_zoom)
plt.ylim(y - win_zoom, y + win_zoom)
plt.plot(gp_array[:, 0],
gp_array[:, 1],
'm',
label="Sampled-Target-path")
plt.plot(prev_path_array[:, 0],
prev_path_array[:, 1],
'c--',
label="Actual-Target-path")
plt.plot(x_traj_tot, y_traj_tot, 'g--', label="REPLANNED PATH")
plot_goals(goals)
plt.plot(start[0], start[1], 'co')
plt.plot(xt, yt, 'ro')
if DRAW_DIFF:
draw_robot(x, y, theta)
if DRAW_PALLET:
dpj(x, y, theta, s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj, y_traj, 'r', label="Actual-Path-taken")
# x,y,theta,s = seek_one_pid([x,y,theta],[xt,yt],goal,perp,phi,beta)
x, y, theta, s = seek_one([x, y, theta], [xt, yt], goal)
# print(m.degrees(s))
plt.pause(0.0001)
if ALIGN_TO_GOAL_LINE:
pt1 = goal_points[-2]
pt2 = goal_points[-1]
while wrapToPi(abs(theta - goal[2])) > 0.1:
_, _, xt, yt, _ = calc_perp(x, y, theta, pt1, pt2)
plt.cla()
plt.axis('scaled')
plt.xlim(x - win_zoom, x + win_zoom)
plt.ylim(y - win_zoom, y + win_zoom)
plt.plot(gp_array[:, 0],
gp_array[:, 1],
'm',
label="Sampled-Target-path")
plt.plot(prev_path_array[:, 0],
prev_path_array[:, 1],
'c--',
label="Actual-Target-path")
plt.plot(x_traj_tot, y_traj_tot, 'g--', label="REPLANNED PATH")
plot_goals(goals)
plt.plot(start[0], start[1], 'co')
plt.plot(xt, yt, 'ro')
if DRAW_DIFF:
draw_robot(x, y, theta)
if DRAW_PALLET:
dpj(x, y, theta, s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj, y_traj, 'r', label="Actual-Path-taken")
x, y, theta, s = seek_one_pid([x, y, theta], [xt, yt], goal, perp,
phi, beta)
# print(m.degrees(s))
plt.pause(0.0001)
if ROTATE_AT_POINT:
"""
This will come into picture when the final goal orienation has been met
"""
#first need to decide whether to rotate clockwise or counter clockwise
Kpv = 0.3
heading_error = wrapToPi(theta_g + theta)
while abs(heading_error) > 0.05:
plt.cla()
plt.axis('scaled')
plt.xlim(x - win_zoom, x + win_zoom)
plt.ylim(y - win_zoom, y + win_zoom)
plt.plot(gp_array[:, 0],
gp_array[:, 1],
'm',
label="Sampled-Target-path")
plt.plot(prev_path_array[:, 0],
prev_path_array[:, 1],
'c--',
label="Actual-Target-path")
plt.plot(x_traj_tot, y_traj_tot, 'g--', label="REPLANNED PATH")
plot_goals(goals)
plt.plot(start[0], start[1], 'co')
v = Kpv * heading_error
if v > 0.325:
v = 0.325
if v < -0.325:
v = -0.325
s = m.pi / 2
x, y, theta = update_rear(x, y, theta, v, s)
heading_error = wrapToPi(theta_g + theta)
if DRAW_DIFF:
draw_robot(x, y, theta)
if DRAW_PALLET:
dpj(x, y, theta, s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj, y_traj, 'r', label="Actual-Path-taken")
plt.pause(0.0001)
print("Time taken: {} s".format(time.time() - tic))
# plt.title('PID BASED CONSTANT SPEED PATH TRACKING OF A PALLET JACK')
plt.title('PURE-PURSUIT BASED PATH TRACKING OF A PALLET JACK')
plt.legend()
# plt.show()
return x, y, theta
def path_track2(path):
"""
SAMPLING STARTS HERE
"""
final_path = []
x, y = path[0]
sample_rate = 2
final_path.append([x, y])
for x, y in path:
xf, yf = final_path[-1]
if m.sqrt((xf - x)**2 + (yf - y)**2) > sample_rate:
final_path.append([x, y])
else:
continue
"""
SAMPLING FINISHES HERE
"""
prev_path = path
path = final_path
prev_path_array = np.array(prev_path)
tic = time.time()
xstart, ystart = path[0]
start = [xstart, ystart, 0]
goal_points = path
dummy_gp = copy.deepcopy(goal_points)
#need to calculate goal theta last two points
last_pt = dummy_gp[-1]
second_last_pt = dummy_gp[-2]
theta_g = m.atan2((last_pt[1] - second_last_pt[1]),
(last_pt[0] - second_last_pt[0]))
goalx, goaly = goal_points[-1]
goal = [goalx, goaly, theta_g]
x, y, theta = start
v = 1
s = 0
gp_array = np.array(goal_points)
x_traj = []
y_traj = []
skip = False
while len(dummy_gp) > 1:
#first step would be to find the target point
target, proximity, skip, perp = calc_target(x, y, theta, dummy_gp)
xt, yt = target
if proximity < 0.3 or skip == True:
dummy_gp.pop(0)
if skip == True:
#need to set the value of target to something
target, proximity, skip, perp = calc_target(x, y, theta, dummy_gp)
print(skip)
plt.cla()
plt.axis('scaled')
plt.xlim(-10, 15)
plt.ylim(-10, 15)
plt.plot(gp_array[:, 0],
gp_array[:, 1],
'm--',
label="Sampled-Target-path")
plt.plot(prev_path_array[:, 0],
prev_path_array[:, 1],
'c--',
label="Actual-Target-path")
plt.plot(start[0], start[1], 'co')
plt.plot(xt, yt, 'ro')
if DRAW_DIFF:
draw_robot(x, y, theta)
if DRAW_PALLET:
dpj(x, y, theta, s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj, y_traj, 'r', label="Actual-Path-taken")
x, y, theta, s = seek_one_pure_pursuit([x, y, theta], [xt, yt], goal,
perp)
# print(m.degrees(s))
plt.pause(0.0001)
if ALIGN_TO_GOAL_LINE:
pt1 = goal_points[-2]
pt2 = goal_points[-1]
while wrapToPi(abs(theta - goal[2])) > 0.1:
_, _, xt, yt, _ = calc_perp(x, y, theta, pt1, pt2)
plt.cla()
plt.axis('scaled')
plt.xlim(-10, 15)
plt.ylim(-10, 15)
plt.plot(gp_array[:, 0],
gp_array[:, 1],
'm--',
label="Sampled-Target-path")
plt.plot(prev_path_array[:, 0],
prev_path_array[:, 1],
'c--',
label="Actual-Target-path")
plt.plot(start[0], start[1], 'co')
plt.plot(xt, yt, 'ro')
if DRAW_DIFF:
draw_robot(x, y, theta)
if DRAW_PALLET:
dpj(x, y, theta, s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj, y_traj, 'r', label="Actual-Path-taken")
x, y, theta, s = seek_one_pure_pursuit([x, y, theta], [xt, yt],
goal, perp)
# print(m.degrees(s))
plt.pause(0.0001)
print("Time taken: {} s".format(time.time() - tic))
plt.title('PID BASED CONSTANT SPEED PATH TRACKING OF A PALLET JACK')
plt.legend()
plt.show()
X_wtlqr[1, K] = r2[1]
X_wtlqr[2, K] = r2[2]
if not ANIMATION:
plt.plot(X_tlqr[0, :], X_tlqr[1, :], '--', label='tlqr_trajectory')
# plt.plot(X_wtlqr[0,:],X_wtlqr[1,:],'--',label='without_tlqr_trajectory')
plt.legend()
plt.show()
for i in range(1):
if ANIMATION:
#show animation
for i in range(K):
plt.cla()
plot_setup()
plt.plot(start[0], start[1], 'co', label="start")
plt.plot(goal[0], goal[1], '*g', label="goal")
plt.plot(X_tlqr[0, 0:i + 1],
X_tlqr[1, 0:i + 1],
'--',
label='tlqr')
plt.plot(X_nom[0, 0:i + 1],
X_nom[1, 0:i + 1],
'--',
label='Nominal')
draw_robot(X_tlqr[0, i], X_tlqr[1, i], X_tlqr[2, i])
plt.pause(0.05)
plt.legend()
plt.show()
def path_track(path):
xstart, ystart = path[0]
start = [xstart, ystart, 0]
goal_points = path
dummy_gp = copy.deepcopy(goal_points)
#need to calculate goal theta last two points
last_pt = dummy_gp[-1]
second_last_pt = dummy_gp[-2]
theta_g = m.atan2((last_pt[1] - second_last_pt[1]),
(last_pt[0] - second_last_pt[0]))
goalx, goaly = path[-1]
goal = [goalx, goaly, theta_g]
# goal_points = [[2,2]]
x, y, theta = start
v = 1
s = 0
gp_array = np.array(goal_points)
x_traj = []
y_traj = []
skip = False
while len(dummy_gp) > 1:
#first step would be to find the target point
target, proximity, skip, perp = calc_target(x, y, theta, dummy_gp)
xt, yt = target
if proximity < 0.1 or skip == True:
dummy_gp.pop(0)
if skip == True:
print(skip)
plt.cla()
plt.axis('scaled')
plt.xlim(-10, 15)
plt.ylim(-10, 15)
plt.plot(gp_array[:, 0], gp_array[:, 1], '--')
plt.plot(start[0], start[1], 'co')
plt.plot(xt, yt, 'ro')
if DRAW_DIFF:
draw_robot(x, y, theta)
if DRAW_PALLET:
dpj(x, y, theta, s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj, y_traj, 'r')
x, y, theta, s = seek_one_pure_pursuit([x, y, theta], [xt, yt], goal,
perp)
# print(m.degrees(s))
plt.pause(0.0001)
if ALIGN_TO_GOAL_LINE:
pt1 = goal_points[-2]
pt2 = goal_points[-1]
while wrapToPi(abs(theta - goal[2])) > 0.1:
_, _, xt, yt, _ = calc_perp(x, y, pt1, pt2)
plt.cla()
plt.axis('scaled')
plt.xlim(-5, 15)
plt.ylim(-5, 15)
plt.plot(gp_array[:, 0], gp_array[:, 1], '--')
plt.plot(start[0], start[1], 'co')
plt.plot(xt, yt, 'ro')
if DRAW_DIFF:
draw_robot(x, y, theta)
if DRAW_PALLET:
dpj(x, y, theta, s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj, y_traj, 'r')
x, y, theta, s = seek_one_pure_pursuit([x, y, theta], [xt, yt],
goal, perp)
# print(m.degrees(s))
plt.pause(0.0001)
target, proximity, skip = calc_target(x, y, theta, dummy_gp)
xt, yt = target
if proximity < 0.1 or skip == True:
dummy_gp.pop(0)
if skip == True:
print(skip)
plt.cla()
plt.axis('scaled')
plt.xlim(-10, 15)
plt.ylim(-10, 15)
plt.plot(gp_array[:, 0], gp_array[:, 1], '--', label="Target-path")
plt.plot(start[0], start[1], 'co')
plt.plot(xt, yt, 'ro')
if DRAW_DIFF:
draw_robot(x, y, theta)
if DRAW_PALLET:
dpj(x, y, theta, s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj, y_traj, 'r', label="Actual-Path-taken")
x, y, theta, s = seek_one([x, y, theta], [xt, yt])
print(m.degrees(s))
plt.pause(0.0001)
if ALIGN_TO_GOAL_LINE:
pt1 = goal_points[-2]
pt2 = goal_points[-1]
while wrapToPi(abs(theta - goal[2])) > 0.1:
_, _, xt, yt, _ = calc_perp(x, y, theta, pt1, pt2)
plt.cla()
