def target_make(self, a, VEL, BEZIER, x_start, time_start): ##init################################################################## acd = accel_designer(self.dt) inb = integrate_number() bz = bezier(number_of_points=1000) npBEZIER = np.array(BEZIER) ###################### making X REF ############################ X = [npBEZIER.T[0], npBEZIER.T[1], x_start] A, V, XX, curve_length, t = acd.making_accel(a, VEL, X, time_start) IN_INDEX, len_in_index = inb.integrate(X, XX) npREF, REF = bz.new_bezier_plt(BEZIER, IN_INDEX, len_in_index) ###################### making V REF ############################ alfa = self.making_angle(npREF, len(npREF)) vx, vy = self.making_vx_and_vy(V, alfa, len(npREF)) ##csv################################################################### with open('csv_item/vx_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(vx) with open('csv_item/vy_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(vy) with open('csv_item/alfa_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(alfa) ######################################################################## self.vx = vx self.vy = vy self.alfa = alfa print("3> target fin") #return npBEZIER, BEZIER, npREF, REF, t, curve_length, vx, vy, alfa return REF, t, curve_length, vx, vy, alfa
def target_make(self, a, VEL, BEZIER, x_start, time_start, num): ##init################################################################## acd = accel_designer(self.dt) inb = integrate_number() bz = bezier(number_of_points=1000) X=[] NBEZIER = [] NX = [] NA = [] NV = [] NXX= [] time = time_start length = x_start ###################### making X REF ############################ for i in range(num): npBEZIER = np.array(BEZIER[i]) X.append([npBEZIER.T[0], npBEZIER.T[1], length]) #X := [x_ref, y_ref, s_start] #print("X[{}] = {}".format(i, X[i])) A, V, XX, curve_length, t = acd.making_accel(a[i], VEL[i], X[i], time) print("{}> making accel fin".format(i)) NBEZIER.extend(BEZIER[i]) NA.extend(A) NV.extend(V) NXX.extend(XX) time += t length += curve_length npNBEZIER = np.array(NBEZIER) NX = [npNBEZIER.T[0], npNBEZIER.T[1], x_start] IN_INDEX, len_in_index = inb.integrate(NX, NXX) npREF, REF = bz.new_bezier_plt(NBEZIER, IN_INDEX, len_in_index) ###################### making V REF ############################ alfa = self.making_angle(npREF,len(npREF)) vx, vy= self.making_vx_and_vy(NV,alfa,len(npREF)) self.vx = vx self.vy = vy self.alfa = alfa plt.plot(NA, color ="red", marker="o", label='A')#marker=".", ls="" plt.plot(NV, color ="#FE9A2E", marker="o", label='V')#linewidth=3 plt.plot(NXX, color ="Green", marker="o", label='X') plt.title("accel_designer") plt.legend() plt.show() print("> target fin\n") #return npBEZIER, BEZIER, npREF, REF, t, curve_length, vx, vy, alfa return REF, time, length, vx, vy, alfa '''
def run_line(self, anime): print("+++ game start") ##init ################################################################# num = 5000 bz = bezier(number_of_points=num) tg = target(self.dt) ##init bezier########################################################### BEZIER_ANCER1= np.array([[0,0],[0,1.43 -0.7],[-1.43,0.1 -0.4],[-1.43,1.0]], dtype=np.float) LINE_ANCER1 = [-1.43, 1.0, 2.0, num] BEZIER_ANCER2= np.array([[-1.43,2.0],[-1.43,2.0 +0.5],[-0.01,2.5 -0.5],[-0.01,2.5]], dtype=np.float) LINE_ANCER2 = [-0.01, 2.5, 3.5, num] BEZIER_ANCER3= np.array([[-0.01,3.5],[-0.01,3.5 +0.5],[-1.43,4.0 -0.5],[-1.43,4.0]], dtype=np.float) LINE_ANCER3 = [-1.43, 4.0, 5.0, num] BEZIER_ANCER4= np.array([[-1.43,5.0],[-1.43,5.9 -0.6],[-0.725,6.0 -0.6],[-0.725,6.0]], dtype=np.float) BEZIER1= bz.bezier_making(BEZIER_ANCER1, 3) LINE1 = self.line_making(LINE_ANCER1) BEZIER2= bz.bezier_making(BEZIER_ANCER2, 3) LINE2 = self.line_making(LINE_ANCER2) BEZIER3= bz.bezier_making(BEZIER_ANCER3, 3) LINE3 = self.line_making(LINE_ANCER3) BEZIER4= bz.bezier_making(BEZIER_ANCER4, 3) ##init accel_designer################################################### xs = 0.0 #x start ts = 0.0 #t start '''1 A =[1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] VEL=[[1.4, 0.0, 0.6], #vell_want, vell_start, vell_end [1.1, 0.6, 0.6], #vell_want, vell_start, vell_end [1.4, 0.6, 0.6], #vell_want, vell_start, vell_end [1.1, 0.6, 0.6], #vell_want, vell_start, vell_end [1.4, 0.6, 0.6], #vell_want, vell_start, vell_end [1.1, 0.6, 0.6], #vell_want, vell_start, vell_end [1.2, 0.6, 0.6]] #vell_want, vell_start, vell_end 0.7, 0.6, 0.6 #''' '''2 A =[1.0, 2.0, 1.0, 2.0, 1.0, 2.0, 1.0] VEL=[[1.4, 0.0, 0.6], #vell_want, vell_start, vell_end [1.5, 0.6, 0.6], #vell_want, vell_start, vell_end [1.4, 0.6, 0.6], #vell_want, vell_start, vell_end [1.5, 0.6, 0.6], #vell_want, vell_start, vell_end [1.4, 0.6, 0.6], #vell_want, vell_start, vell_end [1.5, 0.6, 0.6], #vell_want, vell_start, vell_end [1.2, 0.6, 0.6]] #vell_want, vell_start, vell_end 0.7, 0.6, 0.6 #''' #'''3 A =[2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0] VEL=[[1.6, 0.0, 0.6], #vell_want, vell_start, vell_end [1.5, 0.6, 0.6], #vell_want, vell_start, vell_end [1.6, 0.6, 0.6], #vell_want, vell_start, vell_end [1.5, 0.6, 0.6], #vell_want, vell_start, vell_end [1.6, 0.6, 0.6], #vell_want, vell_start, vell_end [1.5, 0.6, 0.6], #vell_want, vell_start, vell_end [1.5, 0.6, 0.6]] #vell_want, vell_start, vell_end 0.7, 0.6, 0.6 #''' BEZIER=[BEZIER1, LINE1, BEZIER2, LINE2, BEZIER3, LINE3, BEZIER4] ##def target_make(self, a, BEZIER, VEL, x_start, time_start): * a := float REF, t, x, vx, vy, alfa = tg.target_make(A, VEL, BEZIER, xs, ts, 7) npREF = np.array(REF) print("game time := {}[s]".format(t)) print("game lenge := {}[m]".format(x)) self.plot(npREF) with open('csv_item/x_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(npREF.T[0]) with open('csv_item/y_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(npREF.T[1]) ''' with open('csv_item/test.csv', 'w') as f: writer = csv.writer(f) #writer.writeheader() writer.writerow(NEW_LOBS.T[0]) writer.writerow(NEW_LOBS.T[1]) ''' ##csv################################################################### with open('csv_item/vx_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(vx) with open('csv_item/vy_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(vy) with open('csv_item/alfa_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(alfa) ######################################################################## if anime == True: self.plot_size(npREF) self.anime_ff(vx, vy, alfa) print("+++ end game")
def run_calab(self, anime): print("+++ game start") ##init ################################################################# num = 1000 bz = bezier(number_of_points=num) tg = target(self.dt) ##init circle########################################################### xc = -0.725 yc=[1.5, 3.0, 4.5] r=0.53 +0.1 CIRCLE0 = self.circle_making2(xc, yc, r, 0) CIRCLE1 = self.circle_making2(xc, yc, r, 1) CIRCLE2 = self.circle_making2(xc, yc, r, 2) ##init line############################################################# xl0 = [0.0, CIRCLE0[0][0]] yl0 = [0.0, CIRCLE0[0][1]] LINE0 = self.line_making2(xl0, yl0, num) ##init bezier########################################################### BEZIER_ANCER0 = np.array([[CIRCLE0[len(CIRCLE0)-1][0], CIRCLE0[len(CIRCLE0)-1][1]], [CIRCLE0[len(CIRCLE0)-1][0], CIRCLE0[len(CIRCLE0)-1][1] +0.5], [CIRCLE0[len(CIRCLE0)-1][0] -xc, CIRCLE0[len(CIRCLE0)-1][1] +0.75], [CIRCLE1[0][0],CIRCLE1[0][1]]], dtype=np.float) BEZIER_ANCER1 = np.array([[CIRCLE1[len(CIRCLE1)-1][0], CIRCLE1[len(CIRCLE1)-1][1]], [CIRCLE1[len(CIRCLE1)-1][0], CIRCLE1[len(CIRCLE1)-1][1] +0.5], [CIRCLE1[len(CIRCLE1)-1][0] +xc, CIRCLE1[len(CIRCLE1)-1][1] +0.75], [CIRCLE2[0][0],CIRCLE2[0][1]]], dtype=np.float) BEZIER_ANCER2 = np.array([[CIRCLE2[len(CIRCLE2)-1][0], CIRCLE2[len(CIRCLE2)-1][1]], [CIRCLE2[len(CIRCLE2)-1][0], CIRCLE2[len(CIRCLE2)-1][1] +0.75], [xc, CIRCLE2[len(CIRCLE2)-1][1] +0.75 -0.2], [xc,6.0]], dtype=np.float) BEZIER0 = bz.bezier_making(BEZIER_ANCER0, 3) BEZIER1 = bz.bezier_making(BEZIER_ANCER1, 3) BEZIER2 = bz.bezier_making(BEZIER_ANCER2, 3) #BEZIER_ANCER1 = np.array([[0,0],[0,1.4],[-1.43,0.1],[-1.43,1.5]], dtype=np.float) #BEZIER4 = bz.bezier_making(BEZIER_ANCER4, 3) ##init accel_designer################################################### xs = 0.0 #x start ts = 0.0 #t start #'''1 A = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] VEL=[[1.3, 0.0, 0.6], #vell_want, vell_start, vell_end [0.6, 0.6, 0.6], #vell_want, vell_start, vell_end [1.3, 0.6, 0.6], #vell_want, vell_start, vell_end [0.6, 0.6, 0.6], #vell_want, vell_start, vell_end 0.7, 0.6, 0.6 [1.3, 0.6, 0.6], #vell_want, vell_start, vell_end [0.6, 0.6, 0.6], [0.6, 0.6, 0.6]] #''' '''1:1.0 A = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] VEL=[[1.4, 0.0, 1.0], #vell_want, vell_start, vell_end [1.0, 1.0, 1.0], #vell_want, vell_start, vell_end [1.5, 1.0, 1.0], #vell_want, vell_start, vell_end [1.0, 1.0, 1.0], #vell_want, vell_start, vell_end 0.7, 0.6, 0.6 [1.5, 1.0, 1.0], #vell_want, vell_start, vell_end [1.0, 1.0, 1.0], [1.0, 1.0, 1.0]] #''' '''2 A = [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0] VEL=[[1.7, 0.0, 0.6], #vell_want, vell_start, vell_end [0.6, 0.6, 0.6], #vell_want, vell_start, vell_end [1.7, 0.6, 0.6], #vell_want, vell_start, vell_end [0.6, 0.6, 0.6], #vell_want, vell_start, vell_end 0.7, 0.6, 0.6 [1.7, 0.6, 0.6], #vell_want, vell_start, vell_end [0.6, 0.6, 0.6], [0.6, 0.6, 0.6]] #''' '''2:1.0 A = [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0] VEL=[[1.8, 0.0, 1.0], #vell_want, vell_start, vell_end [1.0, 1.0, 1.0], #vell_want, vell_start, vell_end [1.9, 1.0, 1.0], #vell_want, vell_start, vell_end [1.0, 1.0, 1.0], #vell_want, vell_start, vell_end 0.7, 0.6, 0.6 [1.9, 1.0, 1.0], #vell_want, vell_start, vell_end [1.0, 1.0, 1.0], [1.0, 1.0, 1.0]] #''' '''3 A = [3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0] VEL=[[2.0, 0.0, 0.6], #vell_want, vell_start, vell_end [0.6, 0.6, 0.6], #vell_want, vell_start, vell_end [2.0, 0.6, 0.6], #vell_want, vell_start, vell_end [0.6, 0.6, 0.6], #vell_want, vell_start, vell_end 0.7, 0.6, 0.6 [2.0, 0.6, 0.6], #vell_want, vell_start, vell_end [0.6, 0.6, 0.6], [0.6, 0.6, 0.6]] #''' '''3:1.0 A = [3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0] VEL=[[2.2, 0.0, 1.0], #vell_want, vell_start, vell_end [1.0, 1.0, 1.0], #vell_want, vell_start, vell_end [2.2, 1.0, 1.0], #vell_want, vell_start, vell_end [1.0, 1.0, 1.0], #vell_want, vell_start, vell_end [2.2, 1.0, 1.0], #vell_want, vell_start, vell_end [1.0, 1.0, 1.0], [1.0, 1.0, 1.0]] #''' BEZIER = [LINE0, CIRCLE0, BEZIER0, CIRCLE1, BEZIER1, CIRCLE2, BEZIER2] ##def target_make(self, a, BEZIER, VEL, x_start, time_start, num): * a := float REF, t, x, vx, vy, alfa = tg.target_make(A, VEL, BEZIER, xs, ts, 7) npREF = np.array(REF) print("game time := {}[s]".format(t)) print("game lenge := {}[m]".format(x)) self.plot(npREF) with open('csv_item/x_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(npREF.T[0]) with open('csv_item/y_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(npREF.T[1]) ''' with open('csv_item/test.csv', 'w') as f: writer = csv.writer(f) #writer.writeheader() writer.writerow(NEW_LOBS.T[0]) writer.writerow(NEW_LOBS.T[1]) ''' ##csv################################################################### with open('csv_item/vx_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(vx) with open('csv_item/vy_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(vy) with open('csv_item/alfa_ref.csv', 'w') as f: writer = csv.writer(f) # writer writer.writerow(alfa) ######################################################################## if anime == True: self.plot_size(npREF) self.anime_ff(vx, vy, alfa) print("+++ end game")
def run(self, anime): print("+++ game start") ##init ################################################################# bz = bezier(number_of_points=5000) tg = target(self.dt) ##init accel_designer################################################### xs = 0.0 #x start ts = 0.0 #t start VEL1 = [3.0, 0.0, 1.0] #vell_want, vell_start, vell_end VEL2 = [3.0, 1.0, 2.0] #vell_want, vell_start, vell_end VEL3 = [4.0, 2.0, 3.0] #vell_want, vell_start, vell_end VEL4 = [4.0, 3.0, 3.0] #vell_want, vell_start, vell_end ##init bezier########################################################### BEZIER_ANCER1 = np.array( [[0, 0], [0, 1.4], [-1.43, 0.1], [-1.43, 1.5]], dtype=np.float) BEZIER_ANCER2 = np.array( [[-1.43, 1.5], [-1.43, 2.9], [-0.01, 1.6], [-0.01, 3.0]], dtype=np.float) BEZIER_ANCER3 = np.array( [[-0.01, 3.0], [-0.01, 4.4], [-1.43, 3.1], [-1.43, 4.5]], dtype=np.float) BEZIER_ANCER4 = np.array( [[-1.43, 4.5], [-1.43, 5.9], [-0.725, 4.6], [-0.725, 6.0]], dtype=np.float) BEZIER1 = bz.bezier_making(BEZIER_ANCER1, 3) BEZIER2 = bz.bezier_making(BEZIER_ANCER2, 3) BEZIER3 = bz.bezier_making(BEZIER_ANCER3, 3) BEZIER4 = bz.bezier_making(BEZIER_ANCER4, 3) ##def target_make(self, a, BEZIER, VEL, x_start, time_start): * a := float REF1, t1, x1, vx1, vy1, alfa1 = tg.target_make(5.0, VEL1, BEZIER1, xs, ts) REF2, t2, x2, vx2, vy2, alfa2 = tg.target_make(5.0, VEL2, BEZIER2, x1, t1) REF3, t3, x3, vx3, vy3, alfa3 = tg.target_make(5.0, VEL3, BEZIER3, x1 + x2, t1 + t2) REF4, t4, x4, vx4, vy4, alfa4 = tg.target_make(5.0, VEL4, BEZIER4, x1 + x2 + x3, t1 + t2 + t3) REF = [] REF.extend(REF1) REF.extend(REF2) REF.extend(REF3) REF.extend(REF4) npREF = np.array(REF) print("game time := {}[s]".format(t1 + t2 + t3 + t4)) print("game lenge := {}[m]".format(x1 + x2 + x3 + x4)) self.plot(npREF) if anime == True: self.plot_size(npREF) ##init anime######################################################## vx = [] vx.extend(vx1) vx.extend(vx2) vx.extend(vx3) vx.extend(vx4) vy = [] vy.extend(vy1) vy.extend(vy2) vy.extend(vy3) vy.extend(vy4) alfa = [] alfa.extend(alfa1) alfa.extend(alfa2) alfa.extend(alfa3) alfa.extend(alfa4) self.anime_ff(vx, vy, alfa) #################################################################### print("+++ end game")