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()
def path_track4(path,thetas, x_traj_tot, y_traj_tot,goals): SAMPLING = False # thetas = 0 #cancelling user defined theta win_zoom = 7 """ SAMPLING STARTS HERE """ if SAMPLING: final_path = [] x,y = path[0] sample_rate = 0.2 #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]) else: final_path = path 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])) theta_g = -m.pi/2 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.2# 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 [xt, yt], last_flag = calc_target_new(x,y,theta,dummy_gp) 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_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") if PID: x,y,theta,s = seek_one_pid([x,y,theta],[xt,yt],goal) if PURE_PURSUIT: x,y,theta,s = seek_one_pure_pursuit([x,y,theta],[xt,yt],goal) if MPC: x,y,theta,s = seek_one_MPC([x,y,theta],[xt,yt],goal,s) plt.pause(0.0001) """ BREAK logic """ if last_flag: if m.sqrt((x-xt)**2 + (y-yt)**2)<0.2: break 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_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") if PID: x,y,theta,s = seek_one_pid([x,y,theta],[xt,yt],goal) if PURE_PURSUIT: x,y,theta,s = seek_one_pure_pursuit([x,y,theta],[xt,yt],goal) plt.pause(0.0001) #if abs(wrapToPi(theta_g - theta))>m.pi/2: # theta_g = m.atan2((-last_pt[1]+second_last_pt[1]),(-last_pt[0]+second_last_pt[0])) #else: # pass ##check for actual goal if possible #if abs(wrapToPi(theta_g + m.pi/2))<0.5: # theta_g = -m.pi/2 heading_error = wrapToPi(theta_g - theta) print("heading_error initial value {}".format(m.degrees(heading_error))) print("thresh {}".format(m.degrees(0.005))) 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 = 2 while abs(heading_error) > 0.005: 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 if MOCAP_AVAILABLE: state_val = obtain_rear(state.x, state.y, resolve_val(state.theta)) x,y,theta = state_val #NEED TO PUBLISH HERE cmd_pub, rate = cmd_vel_publisher() v = -v s = s - m.radians(8) publish_control(v, s,0, cmd_pub, rate) else: x,y,theta = update_rear(x,y,theta,v,s) heading_error = wrapToPi(theta_g - 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) enablePrint() print("HEADING ERROR: {}".format(m.degrees(heading_error))) print("Time taken: {} s".format(time.time()-tic)) TIME_TAKEN_IND.append(time.time()-tic) # plt.title('PID BASED CONSTANT SPEED PATH TRACKING OF A PALLET JACK') if PURE_PURSUIT: plt.title('PURE-PURSUIT BASED PATH TRACKING OF A PALLET JACK') if PID: plt.title('PID BASED PATH TRACKING OF A PALLET JACK') if MPC: plt.title('MPC BASED PATH TRACKING OF A PALLET JACK') plt.legend() # plt.show() return x,y,theta
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)
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)