def motion_demo():
v = 1
s = 0.5
x = 5
y = 2
theta = math.pi / 6
x_traj = []
y_traj = []
for i in range(500):
plt.cla()
plot_setup()
# draw_vehicle(x,y,theta)
dpj(x, y, theta, v, s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj, y_traj, 'k--')
x, y, theta = update(x, y, theta, v, s)
v = random.uniform(0, 2)
s = random.uniform(0, math.pi / 6)
plt.pause(PAUSE)
def pure_pursuit():
enablePrint()
print(
"\n ##################################################################### \n"
)
global DISABLE_SIGN, ROTATE_TO_ANGLE
ld = 5
# goal = [1,-4,math.pi/2]
goal = [
random.uniform(0, 10),
random.uniform(0, 10),
random.uniform(-math.pi, math.pi)
]
pt1, pt2 = calc_pts(
goal
) # """ THIS IS WHERE THE LOGIC OF GENERATING WAY POINTS WOULD GO"""
start = [goal[0] + 5, goal[1] + 5, random.uniform(-math.pi, math.pi)]
# start = [0,0,math.pi/3]
v = 10
s = 0
L = 2.33 #base length
vmax = 0.325
x = start[0]
y = start[1]
theta = start[2]
x_traj = []
y_traj = []
x_front = []
y_front = []
iter = 0
sign = 1
blockPrint()
dec = False
#decide on whether the ROTATE_TO_ANGLE should work or not
RTA_decider = abs(wrapToPi(theta - goal[2]))
enablePrint()
if RTA_decider > math.pi / 2:
ROTATE_TO_ANGLE = True
print("[INFO] ROTATE_TO_ANGLE required!")
else:
ROTATE_TO_ANGLE = False
print("[INFO] ROTATE_TO_ANGLE not required!")
blockPrint()
if ROTATE_TO_ANGLE:
enablePrint()
print("[INFO] Initiating ROTATE_TO_ANGLE Sequence")
blockPrint()
while True:
plt.clf()
plot_setup(x, y)
x_traj.append(x)
y_traj.append(y)
# find the cordinates of the rear
x_rear, y_rear = calc_rear(x, y, theta, L)
xp, yp, xg, yg, ld = closest_pt(pt1, pt2, x_rear, y_rear, ld, dec)
#find the value of beta
beta = math.atan2((yg - y_rear), (xg - x_rear))
#find alpha
alpha = beta - theta
line_theta = math.atan2(goal[1] - y, goal[0] - x)
decision_angle = wrapToPi(theta - line_theta)
# v should be calculated based on angle between goal and actual theta
RTA_angle = wrapToPi(theta -
goal[2]) # ROTATE_TO_ANGLE decision angle
abs_RTA_angle = abs(RTA_angle)
# v = 0.5*abs_RTA_angle
v = vmax
# v = 3
# ld = 0.5*v #+ #2 0.5*math.sqrt((x-goal[0])**2 + (y-goal[1])**2)
if v > vmax:
v = vmax
if v < -vmax:
v = -vmax
# if RTA_angle is positive then steering should be -90 else it should be +90
if RTA_angle > 0:
s = -math.pi / 2
else:
s = math.pi / 2
x, y, theta = update(x, y, theta, v, s)
# a = draw_vehicle(x,y,theta)
# x_front.append(a[0])
# y_front.append(a[1])
dpj(x, y, theta, s)
plt.plot(x_traj, y_traj, 'm--')
plt.plot([x, xg], [y, yg], 'k--')
plt.plot([x, xp], [y, yp], 'g--')
plt.plot(xp, yp, 'ro')
plt.plot(xg, yg, 'bo')
plt.plot(x, y, 'co')
plot_theta(start, goal)
plt.pause(PAUSE)
if abs_RTA_angle < math.radians(60):
v = 0
print("[INFO] Goal Reached")
enablePrint()
print("[INFO] Exiting ROTATE_TO_ANGLE Sequence")
blockPrint()
break
iter += 1
if iter > 500:
v = 0
print("[INFO] Goal not reached Forcing v = 0")
enablePrint()
print("[INFO] Exiting ROTATE_TO_ANGLE Sequence")
blockPrint()
break
sign = calc_sign(x, y, theta, x_rear, y_rear, goal)
dec = False
if ALIGN_TO_LINE:
enablePrint()
print("[INFO] Initiating ALIGN_TO_LINE Sequence")
blockPrint()
# one time decider for dec
dec = one_time_password(x, y, theta, goal)
if dec == False:
sign = 1
else:
sign = -1
enablePrint()
print("[INFO] ONE TIME PASSWORD DEC: {}".format(dec))
blockPrint()
while True:
plt.clf()
plot_setup(x, y)
x_traj.append(x)
y_traj.append(y)
# find the cordinates of the rear
x_rear, y_rear = calc_rear(x, y, theta, L)
xp, yp, xg, yg, ld = closest_pt(pt1, pt2, x_rear, y_rear, ld, dec)
#find the value of beta
beta = math.atan2((yg - y_rear), (xg - x_rear))
#find alpha
alpha = beta - theta
line_theta = math.atan2(goal[1] - y, goal[0] - x)
decision_angle = wrapToPi(theta - line_theta)
# sign = 1
# v = sign*1.5
v = 0.4 * sign * math.sqrt((x - goal[0])**2 + (y - goal[1])**2)
# v = 3
# ld = 0.5*v #+ #2 0.5*math.sqrt((x-goal[0])**2 + (y-goal[1])**2)
if v > vmax:
v = vmax
if v < -vmax:
v = -vmax
s = math.atan2(2 * L * math.sin(alpha), ld)
if s > math.pi / 2:
s = math.pi / 2
if s < -math.pi / 2:
s = -math.pi / 2
# if decision_angle > math.pi/2 or decision_angle < -math.pi/2:
# sign = 1
# else:
# sign = -1
# s = -s
enablePrint()
# print("v: {} s: {}".format(v,math.degrees(s)))
blockPrint()
x, y, theta = update(x, y, theta, v, s)
# a = draw_vehicle(x,y,theta)
# x_front.append(a[0])
# y_front.append(a[1])
dpj(x, y, theta, s)
plt.plot(x_traj, y_traj, 'm--')
# plt.plot(x_front,y_front,'b--')
plt.plot([x, xg], [y, yg], 'k--')
plt.plot([x, xp], [y, yp], 'g--')
plt.plot(xp, yp, 'ro')
plt.plot(xg, yg, 'bo')
plt.plot(x, y, 'co')
plot_theta(start, goal)
plt.pause(PAUSE)
if break_logic(xp, yp, xg, yg, x, y, x_rear, y_rear):
v = 0
print("[INFO] Goal Reached")
enablePrint()
print("[INFO] Exiting ALIGN_TO_LINE Sequence")
blockPrint()
break
iter += 1
if iter > 500:
print("[INFO] Goal not reached")
enablePrint()
print("[INFO] Exiting ALIGN_TO_LINE Sequence")
blockPrint()
break
sign = calc_sign(x, y, theta, x_rear, y_rear, goal)
if TRANSLATE_TO_GOAL:
while True:
dist = sign * math.sqrt((x - goal[0])**2 + (y - goal[1])**2)
plt.clf()
plot_setup(x, y)
x_traj.append(x)
y_traj.append(y)
# find the cordinates of the rear
x_rear, y_rear = calc_rear(x, y, theta, L)
v = dist * 0.5
s = 0
x, y, theta = update(x, y, theta, v, s)
# draw_vehicle(x,y,theta)
# x_front.append(a[0])
# y_front.append(a[1])
dpj(x, y, theta, s)
plt.plot(x_traj, y_traj, 'm--')
plt.plot([pt1[0], pt2[0]], [pt1[1], pt2[1]], 'b', label='path')
print(v)
if abs(dist) < 0.2:
v = 0
print("Goal Reached Hurray!")
break
plt.pause(PAUSE)
if sign > 0:
dec = False
else:
dec = True
if MOVE_TO_GOAL:
enablePrint()
print("[INFO] Initiating MOVE_TO_GOAL Sequence")
blockPrint()
iter = 0
while True:
if not DISABLE_SIGN:
if iter % 10 == 0:
sign = calc_sign(x, y, theta, x_rear, y_rear, goal)
if sign > 0:
dec = False
else:
dec = True
plt.clf()
plot_setup(x, y)
x_traj.append(x)
y_traj.append(y)
# find the cordinates of the rear
x_rear, y_rear = calc_rear(x, y, theta, L)
xp, yp, xg, yg, ld = closest_pt(pt1, pt2, x_rear, y_rear, ld, dec)
#find the value of beta
beta = math.atan2((yg - y_rear), (xg - x_rear))
#find alpha
alpha = beta - theta
line_theta = math.atan2(goal[1] - y, goal[0] - x)
decision_angle = wrapToPi(theta - line_theta)
v = 0.4 * sign * math.sqrt((x - goal[0])**2 + (y - goal[1])**2)
# v = 3
# ld = math.sqrt((x-goal[0])**2 + (y-goal[1])**2) # + 0.5*v
if v > vmax:
v = vmax
if v < -vmax:
v = -vmax
s = math.atan2(2 * L * math.sin(alpha), ld)
if s > math.pi / 2:
s = math.pi / 2
if s < -math.pi / 2:
s = -math.pi / 2
# if decision_angle > math.pi/2 or decision_angle < -math.pi/2:
# sign = 1
# else:
# sign = -1
# s = -s
enablePrint()
# print("v: {} s: {}".format(v,math.degrees(s)))
blockPrint()
x, y, theta = update(x, y, theta, v, s)
# a = draw_vehicle(x,y,theta)
# x_front.append(a[0])
# y_front.append(a[1])
dpj(x, y, theta, s)
plt.plot(x_traj, y_traj, 'm--')
# plt.plot(x_front,y_front,'b--')
plt.plot([x, xg], [y, yg], 'k--')
plt.plot([x, xp], [y, yp], 'g--')
plt.plot(xp, yp, 'ro')
plt.plot(xg, yg, 'bo')
plt.plot(x, y, 'co')
plt.plot(x_rear, y_rear, 'ko')
plot_theta(start, goal)
plt.pause(PAUSE)
if math.sqrt((x - goal[0])**2 + (y - goal[1])**2) < 0.1:
v = 0
print("[INFO] Goal Reached")
enablePrint()
print("[INFO] Exiting MOVE_TO_GOAL Sequence")
blockPrint()
break
iter += 1
if iter > 500:
print("[INFO] Goal not reached")
enablePrint()
print("[INFO] Exiting MOVE_TO_GOAL Sequence")
blockPrint()
break
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 = 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([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, 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([x, y, theta], [xt, yt])
# print(m.degrees(s))
plt.pause(0.0001)
def path_track3(path, thetas):
thetas = 0 #cancelling user defined theta
"""
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, 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
while len(dummy_gp) > 1:
#first step would be to find the target point
target, proximity, skip = 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 = 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([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()
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([x, y, theta], [xt, yt])
# print(m.degrees(s))
plt.pause(0.0001)
print("Time taken: {} s".format(time.time() - tic))
plt.title('PID BASED PATH TRACKING OF A PALLET JACK')
plt.legend()
plt.show()
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()
plt.axis('scaled')
plt.xlim(-10, 15)
