From aaa3f1c23f5adb42de671a82fdcb8e64db0f6995 Mon Sep 17 00:00:00 2001 From: "Anwar, Malik Aqeel" Date: Tue, 4 Feb 2020 17:52:15 -0500 Subject: [PATCH] Major update Added inference interactive code --- aux_functions.py | 90 ++++++-- configs/config.cfg | 10 +- configs/read_cfg.py | 20 ++ environments/initial_positions.py | 2 +- images/infer_keys.PNG | Bin 0 -> 18265 bytes images/{keys.PNG => train_keys.PNG} | Bin main.py | 332 ++++++++++++++++------------ network/agent.py | 40 +++- scratch.py | 171 +++++++++++++- 9 files changed, 483 insertions(+), 182 deletions(-) create mode 100644 images/infer_keys.PNG rename images/{keys.PNG => train_keys.PNG} (100%) diff --git a/aux_functions.py b/aux_functions.py index 343dbfd..c5f1a16 100644 --- a/aux_functions.py +++ b/aux_functions.py @@ -9,6 +9,7 @@ import time import airsim import pygame from configs.read_cfg import read_cfg +import matplotlib.pyplot as plt def save_network_path(cfg): # Save the network to the directory network_path @@ -25,27 +26,51 @@ def save_network_path(cfg): def start_environment(env_name): - path = os.path.dirname(os.path.abspath(__file__)) + "/unreal_envs/" + env_name + "/" + env_name + ".exe" + env_folder = os.path.dirname(os.path.abspath(__file__)) + "/unreal_envs/" + env_name + "/" + path = env_folder + env_name + ".exe" # print(path) env_process = subprocess.Popen(path) time.sleep(6) print("Successfully loaded environment: " + env_name) - return env_process + return env_process, env_folder +def initialize_infer(env_cfg, client, env_folder): + c_z = (env_cfg.ceiling_z - env_cfg.player_start_z) / 100 + f_z = (env_cfg.floor_z - env_cfg.player_start_z) / 100 + + plt.ion() + fig_z = plt.figure() + ax_z = fig_z.add_subplot(111) + line_z, = ax_z.plot(0, 0) + ax_z.set_ylim(0, c_z) + plt.title("Altitude variation") + + start_posit = client.simGetVehiclePose() + + fig_nav = plt.figure() + ax_nav = fig_nav.add_subplot(111) + img = plt.imread(env_folder+ "floor.png") + ax_nav.imshow(img) + plt.axis('off') + plt.title("Navigational map") + plt.plot(env_cfg.o_x, env_cfg.o_y, 'b*', linewidth=20) + nav, = ax_nav.plot(env_cfg.o_x, env_cfg.o_y) + + + return f_z, fig_z, ax_z, line_z, fig_nav, ax_nav, nav def translate_action(action, num_actions): - action_word = ['Forward', 'Right', 'Left', 'Sharp Right', 'Sharp Left'] + # action_word = ['Forward', 'Right', 'Left', 'Sharp Right', 'Sharp Left'] sqrt_num_actions = np.sqrt(num_actions) - ind = np.arange(sqrt_num_actions) + # ind = np.arange(sqrt_num_actions) if sqrt_num_actions % 2 == 0: - v_string = list('U'*int(sqrt_num_actions/2) + 'F'+ 'D'*int(sqrt_num_actions/2)) - h_string = list('L' * int(sqrt_num_actions/2) + 'F' + 'R' * int(sqrt_num_actions/2)) + v_string = list('U' * int((sqrt_num_actions - 1) / 2) + 'D' * int((sqrt_num_actions - 1) / 2)) + h_string = list('L' * int((sqrt_num_actions - 1) / 2) + 'R' * int((sqrt_num_actions - 1) / 2)) else: - v_string = list('U' * int((sqrt_num_actions-1)/2) + 'D' * int((sqrt_num_actions-1)/2)) - h_string = list('L' * int((sqrt_num_actions-1)/2) + 'R' * int((sqrt_num_actions-1)/2)) - + v_string = list('U' * int(sqrt_num_actions / 2) + 'F' + 'D' * int(sqrt_num_actions / 2)) + h_string = list('L' * int(sqrt_num_actions / 2) + 'F' + 'R' * int(sqrt_num_actions / 2)) v_ind = int(action[0]/sqrt_num_actions) h_ind = int(action[0]%sqrt_num_actions) @@ -153,14 +178,19 @@ def reset_to_initial(level, reset_array, client): time.sleep(0.1) -def connect_drone(ip_address='127.0.0.0'): +def connect_drone(ip_address='127.0.0.0', phase='infer'): print('------------------------------ Drone ------------------------------') client = airsim.MultirotorClient(ip=ip_address, timeout_value=10) client.confirmConnection() old_posit = client.simGetVehiclePose() - client.simSetVehiclePose( - airsim.Pose(airsim.Vector3r(0, 0, -2.2), old_posit.orientation), - ignore_collison=True) + if phase == 'train': + client.simSetVehiclePose( + airsim.Pose(airsim.Vector3r(0, 0, -2.2), old_posit.orientation), + ignore_collison=True) + elif phase == 'infer': + client.enableApiControl(True) + client.armDisarm(True) + client.takeoffAsync().join() return client, old_posit @@ -181,13 +211,18 @@ def blit_text(surface, text, pos, font, color=pygame.Color('black')): x = pos[0] # Reset the x. y += word_height # Start on new row. -def pygame_connect(H=925, W=380): +def pygame_connect(phase): pygame.init() - screen_width = H - screen_height = W - screen = pygame.display.set_mode([screen_width, screen_height]) - carImg = pygame.image.load('images/keys.png') - screen.blit(carImg, (0, 0)) + + if phase == 'train': + img_path = 'images/train_keys.png' + elif phase == 'infer': + img_path = 'images/infer_keys.png' + img = pygame.image.load(img_path) + + screen = pygame.display.set_mode(img.get_rect().size) + + screen.blit(img, (0, 0)) pygame.display.set_caption('DLwithTL') # screen.fill((21, 116, 163)) # text = 'Supported Keys:\n' @@ -202,14 +237,15 @@ def pygame_connect(H=925, W=380): return screen -def check_user_input(active, automate, lr, epsilon, agent, network_path, client, old_posit, initZ): +def check_user_input(active, automate, lr, epsilon, agent, network_path, client, old_posit, initZ, phase, fig_z, fig_nav, env_folder): for event in pygame.event.get(): if event.type == pygame.QUIT: active = False pygame.quit() - if event.type == pygame.KEYDOWN: + # Training keys control + if event.type == pygame.KEYDOWN and phase =='train': if event.key == pygame.K_l: # Load the parameters - epsilon cfg = read_cfg(config_filename='configs/config.cfg', verbose=False) @@ -271,6 +307,18 @@ def check_user_input(active, automate, lr, epsilon, agent, network_path, client, client.reset() # agent.take_action(action) + elif event.type == pygame.KEYDOWN and phase == 'infer': + if event.key == pygame.K_s: + # Save the figures + file_path = env_folder + 'results/' + fig_z.savefig(file_path+'altitude_variation.png', dpi=1000) + fig_nav.savefig(file_path+'navigation.png', dpi=1000) + print('Figures saved') + + if event.key == pygame.K_BACKSPACE: + client.moveByVelocityAsync(vx=0, vy=0, vz=0, duration=0.1) + automate = automate ^ True + return active, automate, lr, client diff --git a/configs/config.cfg b/configs/config.cfg index bb69c36..7fea33a 100644 --- a/configs/config.cfg +++ b/configs/config.cfg @@ -3,7 +3,9 @@ run_name: Tello_indoor custom_load: True custom_load_path: models/trained/Indoor/indoor_condo/Imagenet/e2e/e2e env_type: Indoor -env_name: indoor_condo +env_name: indoor_cloud +phase: infer + [simulation_params] ip_address: 127.0.0.1 @@ -14,11 +16,11 @@ load_data_path: DeepNet/models/Tello_indoor/VanLeer/ input_size: 227 num_actions: 25 train_type: e2e -wait_before_train: 40000 +wait_before_train: 30000 max_iters: 300000 buffer_len: 60000 batch_size: 32 -epsilon_saturation: 1 +epsilon_saturation: 200000 crash_thresh: 1.3 Q_clip: True train_interval: 3 @@ -26,5 +28,5 @@ update_target_interval: 80000 gamma: 0.99 dropout_rate: 0.1 learning_rate: 1e-6 -switch_env_steps: 3000000 +switch_env_steps: 3000 epsilon_model: exponential diff --git a/configs/read_cfg.py b/configs/read_cfg.py index 5bb90ed..287d27b 100644 --- a/configs/read_cfg.py +++ b/configs/read_cfg.py @@ -5,6 +5,25 @@ import configparser as cp from dotmap import DotMap +def read_env_cfg(config_filename = 'configs/main.cfg'): + # Load from config file + cfg = DotMap() + + config = cp.ConfigParser() + config.read(config_filename) + + cfg.run_name = config.get('general_params', 'env_name') + cfg.custom_load_path = str(config.get('general_params', 'floorplan')) + cfg.o_x = float(config.get('general_params', 'o_x').split(',')[0]) + cfg.o_y = float(config.get('general_params', 'o_y').split(',')[0]) + cfg.alpha = float(config.get('general_params', 'alpha').split(',')[0]) + cfg.ceiling_z = float(config.get('general_params', 'ceiling_z').split(',')[0]) + cfg.floor_z = float(config.get('general_params', 'floor_z').split(',')[0]) + cfg.player_start_z = float(config.get('general_params', 'player_start_z').split(',')[0]) + + return cfg + + def read_cfg(config_filename = 'configs/main.cfg', verbose = False): # Load from config file cfg = DotMap() @@ -20,6 +39,7 @@ def read_cfg(config_filename = 'configs/main.cfg', verbose = False): cfg.custom_load_path = str(config.get('general_params', 'custom_load_path')) cfg.env_type = config.get('general_params', 'env_type') cfg.env_name = config.get('general_params', 'env_name') + cfg.phase = config.get('general_params', 'phase') # [Simulation Parameters] if str(config.get('simulation_params', 'load_data')) =='True': diff --git a/environments/initial_positions.py b/environments/initial_positions.py index 2ca62a8..9948785 100644 --- a/environments/initial_positions.py +++ b/environments/initial_positions.py @@ -83,7 +83,7 @@ def indoor_complex(): return orig_ip, level_name, crash_threshold, initZ # Test condo indoor initial positions -def indoor_condo(): +def indoor_cloud(): # The environment can be downloaded from # https://drive.google.com/drive/u/2/folders/1u5teth6l4JW2IXAkZAg1CbDGR6zE-v6Z orig_ip = [ diff --git a/images/infer_keys.PNG b/images/infer_keys.PNG new file mode 100644 index 0000000000000000000000000000000000000000..172c04e0e9cd69a4d78fabc3ffb5cbd6540f20ed GIT binary patch literal 18265 zcmeIac{JNy*Eb%jy)Kf_ea_iupS}0l=X3V=MBFve zKXFvxC;$LBVQ~AFIRJ2|8316j`tt~@0o+;vELVZjU#t@2vu>)huEBUOETgqgUJPtY3r+ zY)3T2(5!D^{vSEN?UHTxm<;_-cQ2hi`ufJfNBjM=+N2DhafvgTzt~u006?(D{g6BC z#}B@4Ylt@=e1;*f{iA^M|7=0jDP=SkzoxAv(m#Cl9laX69NdJNx%(hk9($s61`Ggz zA;FEvmuZaRjb_JV0DyIyhVw->jCM2*k+8-$ZVC%fjv+I0Jzosqz@^Uno?K3&uLZC_ z6^#_8w34qx_dXs4k#e^dN}X%{4b`Jl@Nh5T&`X#1l9jyz$`DUdu#6Fv?;k0<#h}an zjvX4`@NwR6$2MQoR44K&lNmc+4#dj=#FVR!>0z0!_Qcy^-a3~*M5n!-83W*d++>fc zf6x2S)z>-JOxjYFpPDCSzh#*YO44c>vx|=+tB7<&n!Cq?qN5`53<(H?uq(pwmw7VU zu7t}I+MgfBTfHd5MDK~snTsGQo{mNkM1R#Fl?l_enWM5KfSCt&r}3O zKUZ*yy<@qUcsmVZ4VNOsJyYBu57>MT@)iLx2D+r8TScJUJ@HXcK~6=F7NdvRs?M7~ zF!H}UXrkoZ{E{l{oW6Lotx>xswxek(a#Vw@3RDhjjdCBw$9jS?wMn@cT82b(3TQlZ zPv&ruaWmpqO;sF#42m*PgPC1%d@aGY=Oda1_yf46Vt*&hASCoh2Dg*r=q>3y&Tp4% z$?az}q0TV&#hVyZGzVX`H{Gn7k3NNwoU4C~=#u=Ypu;A)!{irD1d!&kN46Ew9BR~9 zXY~-Kq0E2u1zaHI8rWT{Y<aK*UoaEk86(E&P2PiMic5^DWE#bu2Fbdzeoez730V+ zuCg>#NlO6{x4WIFkb_5Gxml0%?zPx>uYeOBf>`#>``BN^&C5MNytYQaCp9e5*TQ$> zE-Y0HqWAa>G$=sj(gK!IT}S)3z-_>d6djE#q0tdL%5zj&`4fmz&Ek?=2}nZ#EQNPO z!qnwsDw7yNeS528wexS&C7W$^EWcV<*=Yikx?Qw;CbOfhytVgjo5;*rI!pl_OvWfG zv2CK3l23hH-x4_{(#?BAyEWnQbnCO{fz(@zA1xZY90sQMd`hH!x^mHRyj_UJ&nH

zm9)rV4gA&p`i_FRC+wowv2gELX)}kANBIZ@vHUcYGAEcH_?=kFxGTdPd4NqZ?3`&0 zzey~FdKkQ>lfH&VA(67pn|g|fgild@O$?PyI*!%=1`~Yg&r=#|%Bw3-zZDdWjC3j( zOFW$4MtBxi5Z`mFtgyefKRDJ9O3TjiFU9a;UI<=cdNqa&se!oxYXaiU*-w-|h)y5A z!O;2;IyCJR((9vbfT{>_ch@9D=o(#ncZY0ZqM+&NP-5BvOwpLzvYB#`K$iF6|5&R3 z!i%iGq|dI^6>29u%TO9ZmHa18|#1u}fYxr2*<}RrLOmExj8P zP=%38i}n1G1Ff_1(yn5 z@g)3Yp9YW4JLo)IgIt;CQro-h*(gWT$h7Z3>$#@j7L3~;BgljcDEV+D;a>Fp+Nnn+ z6ob|J#AQA8EHAsr%9=qsB?q!j)y1)~SXaj$QLEZ_AXBxTe{+nAe0&Ek^!jDdc1`_N zPnEYX3(h|AU9&MAs!LrlpR83N&bw;kCj*0_yM)XkrOmakTmoO!aUScjl-^+@vK%aYWxUByY-2$gLnUR?WR!OKPwVJtnV&NAcbS+lkCO$-V8z z?mC)R#(uK($wVaiczlx+1P!l}=D*vMocWcz5$~!tFoaGx_!1#j zyFSxQAAC5eIEGXcA(IARV2tVZL2o}?(;4Io*i^BYEf&})7@FNwQ!8zDMt{TJ$=Ggs zR9HQB#9uHFvro|5)S7sd(Btc>UF|38rY%!yT)kj-{u&0!a;->bdSiA1iB@4a|AS0g zKpBLcicF?}{7R7V_P?ia6;$E+wxI{w^=j^S*>P^7o}-tXf9Bj7c1`pdm4)hR_XmYi zylXdP2t!|f@97ZppL9aomI^X-XuJBcA_h&NTS_N=Zw9-IV4?#$@0op7OZ7k%Ad4g(#qKR&$+cRN{Zj2a@gL;D>L1ij+M60}?S z-sy0g-GuVS;T_($>&AAD@edW;DH~sX5OD5=v3qk&JXzONFjGLxbQ#P_1BE*PHUGrlxI2uxkixh%a&;3iF zUGzJzpeu49OW^6Wh7>P-LrCIcBLn(xtm+78f4nv$h6HD}cMGfJr_qQQ!6pxu{BXrsAJx6D{>^9~tvb!oM zl`n&Eh!J4&DD1Q?b_nQGRSj8OFE$Q`OB5YO2R+=%b_jJtc#Iyn3LTcKK!<6!eVwvF zJnj@?dsrwt#CF!596e;`a1c%rOtcNo{31cR!Q>W@-!=2-lm z>XC)$A~QqnW30g84C1K-oBY82|6nFFBfs+PzR@R+ZN<_@uzJj|k&nzm&SeMsfl@tT zDHY6}8{VCh_TGMAymHW_?}~Y(`*xURMCzf1vfqz)ZOQTh<6kY20O=Jur- zr#IfdReUDq@OqJ>N(r)wO+)^d_eKoq_WTQ6NIU=ub1xSnTv@ddQ{|TsW~g{H)O1IV zEu4i>mAk7(u`WiI)n#YZp3#r>&>O{wlaOEoc6;dPOt&3so~7NVAm=?pc#NlXu)t(3 zoPR2D(0me;hMZH1Jh-zt&C6YTxkX?S8e{Hg>=w8kEjg1@ThlxRqo2BH=*M?;os*H^I+bRGkDNE}N$927+&+?qg_& zuQmXLdF8+vPaJe$5Y#s!*5boLbdKcqf*a>I7i9Wou$6hz@@V?7vEFrmq+vwC{Peex zxcF3i`5ALNWEsOr+HA{wQ$sIGm+9DhV*7#+jgR=I+Au6hfZW~ikkDY64MGui74Nrn znnE3lA?YolUtzL`={;voefJgeeeftPziJ#K1i53K?lJODgY@|M(YjyK5Te8>H_CbJ zmG%hMJ?tbUXGqE`xRGj?p4;P>ln7FeH74FJxsm>6Lu65RBxFPDDrR%j%RylyFrHBy z_VbHXQYy2j-7mzR-MXrHY@w?$aohXBy8gt6b^o`er0{Q)@K{6uldW);jB zDzGpZF^}3&%G~Xu6+$DoP0ywSMCVHkAC+|94m(A2@MByMyb&hrV9)SL;jWz-_g24Ve>ykwbo&phE6<@6W70}(=GQ%J)uns` zMqoaktbH6e7l5>H>YoZeDUGSfhii4|lyEb&vr(lw8HEb(%AY+sr7=(Au36lU-xT~( znle(7mlVFcP+K|UWAdKxCm$}?>PfaqUgU~lG(TyrK%*3LarB{;UEAN|lHpH7R73K3*K7;EL*;S|MaylrGkF=A?wJT=DEc__8( z1-RVHA5!Q2eGn04kH=HnN50SrH3`U+jB5FzJpXh-A>XtXI^6=f_G{7Qw!vo)C~~5$y9fd zJL&eq6L4rA{hHiz>;m=c;VYHR9ug}V=SEI{U2)z_ACR6DqpF%6}x-2BJV`?K!Qj?K=9CB-62+pLl0a>M?YTDLK^5DjWZ z`tNeA+d5x5L1iz5sAwbre_qoj5O89)xJ500d{!P^XWM3Sw{=^GOkeM?QLlOlJYpZ6 zsHS7mN*tb=4}yQ*Bn*7=e17kW_Z!)PUe>lHHwMZ6g3+S=qJJ{UY0uqX&=It}_p!$kY4U0m8yve*aq>;LvN< zbNCt81XH15wY~`enE1HB>n`4WjjcZb+8%JS4*la=(hgi^t%)mZbK|)tq3I~?Q#_b| zG<(G}c%7ex*!;@;uE&1do@c)?E-E6BuF8X-RiYQdARFK%`T}W1?5J0JjnX`}NpAJ_wF2rNE!1rav#xy7xolD^fw9 z9^HMS{iXwX=Uw9w(Cc+MKqbYd;@*`R2dJwyym1^i>8-Z4;+pgo$8=FwR|K@gsk0gi z{=Ti{@n<}5ERM-f5G=lG{^P} zD(CWcNJ;6hj=wM$O#_`n*HrIwJADcjN|BZQp7~2sRZ#T#kudiwo-ET;s7VGS?CuH7 zK>@$N++r_sZ>n2+wpyssE6;-HSTsuqG`~y_ke9f1Rlrk=OKjt%Kk4U;;k+rV=_bSl z;#pi?cVsV;YHA4DyOV=s%St!rTu$!P9#_xX7eh21V1Be;4bY2h==0hKaLj|gB|wjH z0Y_@=n&g%J_vc_o{`J4y!T)}JTdeErnn~PZA;rh=x}VA{Qj5wi{M4L zEx6Fjw&)MS9-U&~?5rb2I|LXLmuO}X&ZPrlag*A|f3BCT*avT!;qPNlcGP!T*u-cv z##~7BiV|%%>Lgjoh*TlEC7xy`QD!0BMeV}d4-Nm!&M0ad_X(awY0udD!!f0(_*XcD z%eZM$1>LT$QWJ1i%s1{}QoPuA&5ymd9`ZiR6w@46gdNZy+aFMl{yc37VnSC zmM1UaiCd@iVMJj-d%XHD*8Dx!VXI0Yg^n;}; z*r#v_B8E;b^Qb>`E%3_zFiV%zOv6m$NxAlp91C4P9-9kSyv4C%t3hu&j{f_rND10u zz?d4>L~4A+!`xzN|110^VGh)#ouI@`kDL$F83PjYt*=HlIMikYR;YXJHU1OZX|$67 zm0!CMmVPy(PHSZnuSHKa+hc<|B$@AwGBo_B_(2~n;;`(7 zLq&+ELw-iDHaWj7eSAeF!k$+BQXkq(E-DDpyr?za}^H|vtEZpknZ{47=^UF^>xE?D5eoIa|++hyK-VSS|nk{V5$h(}= z>+s5c(N*(Si%EY@ul8djb=zz6Eae!uxE9M&4t6&e8Qo%|S)ow-d%20j_g*FRyox

3w=4hqaw1=mML(5A)}-&Iepo4>`Y$HGOA(pRdtZhvEp#qk!DD zzr--{KFUyCyeabMkINF9gCm39afT*gjayOHMOBpB#hN+BEECP)cm_VTYY;<}l5EDR zGFSAuGaU%hm-P#4Nw2;GLeXe_DaR9KFy)5%sI`tVzKR$M$RXmS_=PCT9>!7Ed zc*6XN3$!DD+feX63q`)bxtq^1T&7dQ+G>2Xs|T}XUHuTsT6TDAxNP@-%$hind2eEO zTDi?gniwk1y2a-bwNV(*C<*j#P7dYwQhCo`NaP z+*;j8SIc@wBnE5|G|STGA-wg0!?g?Kw^ z?)y7XS|!WFeWBvn-8yej3a;mT7Ln&+e*nhb0~BVa>EWBw3ExnLVX^SYVUE32021Z) z{}Y0{OTtgG3pZjhsmWZLw8R&-!L-whh`0+q4^Z>kXPL1p)ild zG*%OXGHm`uR3&8jQSonGTuu_j4m|!0IPeOJAPjgCw}U-c7@R-dlh9zZvZr7XX9-*3 zU+p(_FNIV<6zt8d8%o{-Il?>yB%7rT23NTCnOshI0tKA>lM2l*p{_A9zoRjM8AWt` z@#TjM#uoPmtJ(SYe>MAD=DD#!*kHNuc1_aR{TJnY#lEJVT8oi(oiZb7Ki6`#g=G8Q z0D2;!T|xPQjV2~sDQ|1%#U(072aH<==mLSkqx2`mcuW};x9e997GJK8wG735&tAFl z+AyS%W?nzWmc_z3SxRA}hkB${eAq|&l+Cr#raxR0UQ@P}e%EScKC$3xcx!8a1g2!W z*w{x+t!`7|rS-hw2s!AtT@5{6I#U=WQnFzva|#vm%iAJu!>tx0v3jG}pY&MRR-tGQ z3w3C^cVoMfOUoen&7?ZhgBBE{h4T#(3FzB>U+Qk|hs0mE$x9(;KsSP!dH#q5tEPf{ zGRi-$Zh;DIiR>5(QiveuS3ie5Pom?Plpx3Ay^a;h!oAJCYfaUc(b_kN{(pp(yDqY< zL+Ts&dGzIyqTUsyt6)y_McK#%+te6Ap$*US=QZDcO{3va7a!{b9!WsE;?19;!C*xz zYVRcIGQTa)C9nhj!Bu+T7w%48FGMTK$TOH)m~`(*F}}P+Sq=Y;G1gzf6?ETHqV6(3 zBEJgsQHCbE;sQ@$^?kz?&FcFn1RQ3QUvly9Q@Pt=aS!2nLC+HQ9rc3`!ZF%=z{NIt zSzf+Q`|I=n3ZdS3?=)r=42QNr7@016Pax7Qye!(DlbnkFb}#CAnOnitIzitR_IYmp z-AK(QNxZqKJpr&pTEi~wCExnDVjcJWlg%52m)4gKPiNzD&#=zf0M|6do7MP=6(}iF zj2pqz|6tCZ!8%gQI`c0s{#OGz><7R;li7$D`{O@O8jrqSJ)w&W$om*z@Jh-D5T^0p z99RC|j1U`0rCVz&&vdXeIxLajWd&h9XI5TsK~pi`mu>e1S@IZs4>%-z2HfNktRuQV zpvE^o&5FhW8xAI6;Fs>>B}oVIO(t%OO4ealLxcYgLX3Rl+@{IVUI{BpzS?Nl<0qFW zYq@$-9&W*C(LU3d>d*wn-h$lpjPR_}q}-$6x4pI9tI|iu@~1-5?Cu5by8jHuWk&6B zw8vYQOGq@=uV}}VZhe!z1?7KH(W!O4_&L)QHl(9^Mudc$bk^Fpza2IyNxm4Pi53}u zu7b>tHLy|Rf48Ls%Jm#wd$~SF-LMCGK5X*Th5eqU{In3(%v>*`Z{dAwW5@!rJv+@^ z4Fex%1v;h=%b=`!fb9)ZnlVYH#DretwZ~lgRh=BqoEwPm72ArIUdgsLmF)xKi>50N zg}jT36(6b(?2XJi1CGMNtwDRc&-5BEDM@v}NtEt5h979BRWCTBh}UTdd?EFi!S^2o z(_UbE#-?ilBx8@`U4mNqhC$dl_{*(1HgxE#xL1cfu7KjFC7a*zgQMCOlZ}nt#q2yn zKB~SD-50xvejC-+pl%R*EdIsVUYyO&Ib|1zQHW`>>d>5Nmh6XP?qggNevm5U{zxd_ z8xFn;tfp9E!M}oP>a1qy4=8z#k|{^HoPvuVd$ik08HAmo^1#cFL4>cVU*R3rkZh)N zsHB)W_S_CTBSya1mc%hkuW_R9TyhI6Q<&8K^RJ4I7uob2s)rLC_^5#PxY}m_LXPbO z)yLw^f5xrE<#@3haSgQf#Y(dliYQN&(j@WbeCkPfxy(QMxE*%v0-N4jme5kTCLqBp zgQ|#5=KD<<5%K12la9O#x5LbA_O(Y)Whb+QSi=$Ko>-#4mE=^s;TkM1y?auT)9X}+ znVDJJuD&8Y}oyC#n9<-GBoi`Lr*GCXS%HlpbNZQc0^F#$lX zkGgzzUCEd|s*X-;Ymf2+%-ohGXh<|~&T8$AdIxub{XOsbhHr~+PS%@ zT>6QyWH@%EtuevgAWYfO>2hxh*#kD~c8d~W7R~J!O;#=<|5^%xvfX_XWnmZ2 z@$ADd{F=De=kA0ye|1O6i%cQTAQkW8jrXrFXZn}aZFbQ6R{hLBmQV{Gb+QKkgz|c3 z-Dxc_B{Y){SNN{%Qagv5X(qjKHmE@NZnvhrug!KcB-fE*T2~hk>P}PDjs>lx<6{Nk z4}NJUur#l$jOSgK4?-GCh%7z3HH#^mOn>L*n;^`a%tO981_`Q4jIpaU3X}Na+JE&o ztUCHbAlcpX#RKk29Pdby<|q2Z&sjwb;y?_UO+8kxD+gq-&)PY8jEmCswpk12*bQ=( zP%W72BAP4ye52?~96n3oVs~7vv-W1Nuf-*Kf#}!`0d+WgW20}WQaVw#1ef!82;S~{ zM>NxuzOFHgS7;H^HwQgD8L37Q#4^wprqrImUel0wxRjEFp*z;3Lltrrq?7498385Jf~QC@ZJ>jrPzu6#kSWYqep?mFpE*bRhVB-A}d;pe0rrm#%@pXMh7S^Aou)Wx299Q!Q2pZZv>N#NV zLdp2Hqdogo8? z^NT4Q^_tdRlJFGH+i$AOiwuXIT&2h7jw$fqXet_bEVAvPnY?5mzJBi9B^GU z0lQOc=ugHEYjB3!iR$hf{p9gGrTXKjj#wNT=G*8Q0B7yNszhZZdSc?>SvG`^cUdFw zrkOzxMgW;F#Ywdp7?S)((jtLU}i^GABp5}y$hJ9K+=h;l=c{9diD87&`G@ETg z*<)%dKqkCRN8&AZi@NdAu{J$KLAR;f)AHILLl$su>5 z12g!!@6$s(@WXYZ-u-KwzNw3DSVRwNHHKFj@1Q`VRR&7Ar8g&ECd|G{TQfFY}V`Pl(<^D5ZaB9Ukwk5x^Hw*5(=5Rbhe1@ zs-l&0;+s(x5phM*nQYoy|8BG+)QoBK(svUx&x?!LnmYwmPO!Zl7P+2V(#@VG$VQ@zD}ioTn{=-S%?OGD45b%@n@bD$Sm-TrJeZOO5VTd`3Ks27`Cj)s42&$)GomDGkgUBUO#W zG_xngr>bG*MiSBcl`HnjR-cL4?OI-lA_mnn+JVcd`3~j_&7bp|GT%n5aDmUJgKNTD z1H)4b=HG+RS*Z!e&%`+S2ZOa0;;AYK7_zblw%*ohMzPz(jscbR%}d4gf^*lwFMc2j zp!H#Irat#>TN8e-&J?3@nX1Wq(Z!~{po+}z_k*|SuUMQVkU{SpdAHJ3P#JkK4~UzK zP`yRl(2&l{(qG~+p@e58{4)RaVK+DMjQ1RT%Pj3=gmDZXA96acAtjA^FS`#l&l(3V zxjrx|pJcS5esG(Na^zNfdUZSU(;&zI(q0ddJg@1ca=S798ROHdqlg0O#xTd|NFFjI zrkYC-(5O9?NqjtCv7LH(%uOP=Md-^;tGB4WXTVnK$NBlW={^{-9#*nh>=Bz#qm2n_ zbTU%M?tiFM|MWC;{s+y>_rN{f+)%}M^z9Z_lXraa3xZiH{OMKQoZr;h4AeXSh-k}( z#%q{do#mML@KTn{@LN6V+~<1E`cWQKR~|z|n2NZR-rw~Nq6Sg+6z7wHoY=|@%fBUA zc_-&GqP8Dv=EWBMyS~HQ3=u3#uDPo}SC(F{j-;QT9L)hKPgRY7p(xLO8(WHISa014 zgI9OWpa&PcxkUOIWoMLT`1UD%v# z%Ccx6MTWUO!7gBy!sTt|8(g#{J7mfWq*SOUTg)d6xOHr-QF$pAv)?Su1w+2qn4bBn z|2xwFCa4aAR~bj39cZ{SXvaQfD&%UgSKr6?1IP$(LHF49wQKp&HzAk4=p@j+&k$z~ z5DCLe*Ptwy&x&l{288PN723~YusQ}GwoaLftf6o*kspd&kPqsuYRrjclR*VsD~1)I zIrXJb#$L}N|3-UV6ZlTNhJ?~o;SJ%!jTx~ZvMcWJ;^9go`vCz_s&b;k?#A{B&l^S- zq+96v%0BYGe0nDC!%rR^mBs$w2y$K304V^cL9PH36X%E{{>2liL$T4%Vt%j4x)6iS z)U2L<-^|xKWiL$iW){q&s_qO74-%BTf-|fS(ciP2X(Axj)(c0;H02m-2?)xvU;D(q z;rFLkZHi_Xs_o{B&C0zy2<-~XQP*72y!JMZ98oee`}ivUm^S_lhMAM^rl#pXz*t*P z6YUWt{E!UoT^U`K9f!2X(Q|ruv*v834#}E#q0f3p7Hj2s(-QbD^YE%*ut8l@5QC-c zSJAC!JEVPA7h+gLO8ePzDw^R+NL9B7hs=uV4`ST%s-%ai+e3=IMyXpTsv!4_in$2E z=0K~blbiY4r|bo(E6jot)PT%Diruc(4}>ok1^6Wa=}-Ouh+XQX_pg}WAX%tpLE^S>xAAu|=edT4Tig#JX0HV(bTh zVMgE}ZpfQ2bGRd}Zu@q(U?I)#=Bk zJ%J2Lnd12@{RCZiA0e!MP`aGkW1|?dKBsD%F2nnzXy*+uE9m})OKa%L5ub5;!&uxV z%opXUzaiV7=DeHxNZo3PGti3PnNij!9}dC$Cqj!=Nrd2~LX=jHaBf#MbO|P}lr366 zbhCuD%PDax>ZB>fCedm(hEYq=L5~n6hAV;R{fbOCVyUM6p#3hKlZZ;!3K6%pSw4o4 z1NgMnRI+yXbiHYFo0sY;SBBbpjsoF*T*QaB54#Km%`{YTFm1Gp+qQFL*S@$&ww08N z$j};0V>;L$;qPS1d}izFBJX-)ORes1C4kHl?2Ig69)qG%LsWPCuLct~A5Y};HsKGv zp9;e~`Z7K>y_+N-!eh!flI?2m&f37|E`ATtFZlqRxWoKnt>RHtRW@)FxYVJe|5hM? zI9=vxg}XDt%MS8CNu*EPVVQ^L_a4^gqcv;2*H%Wp!Adq@og%KrY2-~AsOEgcc%V&* z@Zc=n@mKDxq89D!3K12`^WSBeeMqT+&t5_|38Q@ZOJjKU^C?mKcINC+I5I)&^QkH7 z9XUZZybN8%NU*j41bvGJOl9T4Uz0ybUf=F03v6JKXm%-KWX4d6;cS2~`x2%q=W+^U z?U`r`Uez`P^=TE04CVJ_on-!+A%rFXLe+t>Ti1gTpYXf9=5~nP9bTpXRB!78S!Oxh zz>D)gS4*EE<2bA=JDmZ5^C{4MWA@$GCQWxt&h5xo7nNrHc+i4Ru>tKFnOlh*)V*|l z^CatBp_F^e;n{At3%qY}n+N)FWrec(*C_bQ-3|_Oqfdjut~-lh&>kzB-V4k+#?^1Z z^G=d+EF`H~pZ)mLe}Hm-Lrwnx;aD_(A$u{(Y3My~@Hx08OLl)Tef11=Jm*W}h=ya0#OzX>9_2)hKGZ3lN zTity{V6Rcslq&jM^xD4=NEQ^bvJtHFrE7aR#8z{~o0a1Kv*B^*aZ^d^3-x(9)_)6#89r{P7t-K8*alHAW@P6 zMTq$fZ?6%0Ssvu?K4I6kT`?oo-0_e>f{X+s%>*~DOY(BEt2>@!wXVx*{n-gP`1fnn zJ~4kdJ65i;4fp<)hq3#*596e09&c={@6#@hZS2$(1bYOX6s2S5$gU;7WW*G_XN^^S zi9DeZa%N@YJLV(MQm}Yt3@ly51V4=IS6Czuc<&#By?D7&AfbsIm|SIF1zK@)O(c8A zp*Bac*q9T+=lDabqBx$B1|(pW_<%}t+#xi_%0_`}LgLn97<#*6GZe{$M6YV@kaz+D z$$Rxg4wIl4Th81WyJJy-F)_kaIkH$Ntv33Uh~T!!l|A=4&ShY##mg(ckW;~&Q`DQr zi1=#G3v3u{o#NsO1fUOr|hO@}LXTZqEiwOD+jXph7#d_snEm~hm9_eN{(w5}FKXj|qdu2(=dS#WQUloFyZYDtLm{VD zHVpIz&*2@TPP4dmy+>Z&NHa|LyeX~B8ax_wFc2`$KL@hTVSSiF$3NU!eWvp!kC0zs z&hEB&7EIm_(is$Ed|TYO%eiIoV02QmSQMwBfDQQUfUDvm1&9>VS#a2Qx(j0GGxNgN z63zh^7zqpBB@G5A1L~1(9*nCehYT_F_M36@zE#-6!!Ucd1;Av@R1!t>iuPY+n6qUJoafU#h%fs<7g+L zEW^HLw0xuaFey`rA@&;N2b@N4!>zj+hJZgG=ttYO-R_+ky#aWw8aVzB+n^g&&s+p8ydnUhdXn zLfjKOX`54Gk<2OjJkhcI92UFZo=RdF{;achF&n^Yj`6!_I?Fn4YNZc7wm0? zFoQU~qRZsT{smh@Z`^D#x_gZ$eKRgEay&CptPUX>+Oylk+>JKUVm+%$+B7aG z>Tdhp^MUf5_^rKZ8}~p~+TdQv=cdk=&gy5j&A5)0&9|gf<+tYq_{4;GQho$xDGVuz zZdfKTp1M0Zb_QbM)*8(}#|*O=9&NbefI01sS46U z^w8dhZqwquuUB0Pf%bd72A&E2-f?~kv22ER z_7G-oucyS$`^3j?3qNgbi_pm%gV}obs>NB^U?yR$&R%~QF1+Fhz8LZRY%BQ=vM1-G zhj=VE{H+;&;>aU3x*Cm{rV^K*q`#X$Smai}?6S_8wzLKdLy$W>uxy=l!qX0!wp?Tv zeBlJV|z&O4%ld!fACK=|PO%`gT!$$sA4}08ffLF~42_6$ z3Q^A((|gnW?yME0jcyi>ier~}c#|#oBL9BdJ4_1tSHaW=KtGS^nWnIA?uI2kgv+TW zyHB3%9VQA7R4+{4AA%5;sCjTns~jmb)7< z8E&>w8^h}Og_KnIXwm@5Uz=^H&8;7~wsfxOgum>}AF|7wuDf7!G4z*LL4WR>eTaH3 z@9&c#3EOAmGzj}Vh+5!q@aN|8z`i}a3WQlqj#wcT^;DG73BRkZ&{bVJEgu*12r^%- zIK9;@C9s%#Y?Kn(U3tqj;Y?)PWt6kgZnteq>(>Q>g(O>v8qlM%WDa`EYxg&frzqfu zmIZ=^kt3C`E$dyz229ukZl`qj;qY+ax0KaM&)h@mpv!13si6q;+l-;>JZ$?fJdA3T zaGy!4RV8MN)tt4a#Kh1lS7B_v{6h=qdU|z9kmgLZdR|V|lW)CvYQk0U9L*OUK?MPQ zC1U2LaF4~sI1xzhNR;?c?gMkesw46$Flwi4t?r9d#CQKYY1qr1$|Df%$-~F9N!;D_ z`~~&J9_$U}0*b0x&rSVt+vW!ARP{!J8uMhNcSddtE^$)VPSSnp#wYN_)p7co6zt2* z#VXEP3u0mj5Lq8>W-;&u=%4M{;?pfzaLdj?bn6qaSJmY)xi9K@g*V>AefF~QmpeLn zac^@fb*JWt3!;NVXdiyS>STykcx+>naglfM1vU^onE+9~Kd0nCSKpvYpmZO~&R!aO z0PdLQmfEt5}$=FQ7@(H1c_I7nCZx456n zGEI1Bx;1&5mutchLidgT$j;K9r@I1>Qa$_MB_ygZM$g`;`twru3rhn5DCe5dMwtn_ zY&qA&!jVTx2JEVX+UTNy2pn3$$LE3-PIBZi+X+XiNu={LZmys&<>~L?u(O7q-y}tq z^o$v(J|GHHQE&oBR}M_l8AwZba$slSgj=5brKT*8HP?qL04o&8$+x&9zJsL-HmOXx z1TY&Rx5FEY00vyL(k;fJWFq3@dUZG4sVzWYNs2_7}?qKcwg#|fY_SL^Mmforw# z+vqA>)$0NGLMjb-!Ktf$4zF zi2(U~u|3)}|K&*HTT(9UoenWPx_59~6zvBa(Mq0w*$(sbWBEpdO*O~vmFU`G^22l7 z6Yb1E!rX?BwZk4K|SB| z=$nVy`Lhf4t5?xRO+9qaTd-sl^q}|Sx{Kt{MVdfAl4gn!VeY2h7p9Q!n99-FSqM&L z?NTOvX9hU61x**jZF)nVR$+@uyBYOTv6ME^s|IGZ#U(MU)y8bYmSpx$087xk`IiORoxCufh7N$tgj%$9|+2 zWvs}mC*)P}{Q_Uts@Aeg;&r?F#?I9=j-?R=JJ!@ar zj{UhD62v`2fce2ju~ltPi*p@6g%Al#XWpL3wH=ixB${qyS`=0X2$yFn%o7UctDJYO z&mAn5_4nOH+xe~#WUZ^;LC8m_gmL{dO1V;tHDENtwWx{L_Z6|h7R>vGOW4cs&EVisRp7qACZKVp) zCHYzA^}@9Q9{Ov{05g4+_ZWraEQvx zV{H>dfzPS+o9BV#wmiH%rm<+V)M0ddet6 zw*v5pCjH%J=a9#uyf+B9I`1~YEToV+P7BzUO3j}-AfX7Z1GBZu+pb`i6Zen|EhcX zL!3WVsqSO;Ak}){i;H>?%ENvX`$s$Bi@XZ)a?mIR6>%Ntc-c%%E8P9;jBeO9L^{^t z>kZ}rEJSG@)}>G)u5UOO5c96mxzpMKKf!HG77L+yC*+t(xMNiA#rcnv_*&s&Adto? z?@T|M*c9fcfJM-cK*wZI^t+~nGAq}j^z~Xux|k3IlI}~niGHD#A}7NJLa&M)!;iiV z@3wuhkS@UW>v}L-Q`>MBhrefb`}%BvcxM+f;O0p{Shx(^-nrFI)}E5@n#k0#?dwF% z^UniskVUs~8*bZa zyKh?nkEfvi?jJCnG3IyECA4^?X^wQyM`)$78UXkh$%r9?w_n9+<*^dx|BL$+{<{Pv zZ^`D+(9k8+FSdVPws4R=AAGZSZA_Ztz%>~m1jsu1cWM>+XXXV}>DB1sRYTy5>*E#;5zfbZ%4E$FT o_-~!u?xFuW(^Gx1iL|tDcfD6AT%1(PQb~Y;uF0*E8%{6&KPfJ*(EtDd literal 0 HcmV?d00001 diff --git a/images/keys.PNG b/images/train_keys.PNG similarity index 100% rename from images/keys.PNG rename to images/train_keys.PNG diff --git a/main.py b/main.py index 3994b79..bc2fef5 100644 --- a/main.py +++ b/main.py @@ -2,13 +2,14 @@ import sys from network.agent import DeepAgent from environments.initial_positions import * - - +import matplotlib.pyplot as plt +import numpy as np +import seaborn as sns import psutil from os import getpid from network.Memory import Memory from aux_functions import * -from configs.read_cfg import read_cfg +from configs.read_cfg import read_cfg, read_env_cfg # TF Debug message suppressed os.environ['TF_CPP_MIN_LOG_LEVEL'] = '3' @@ -20,10 +21,10 @@ process = psutil.Process(getpid()) cfg = read_cfg(config_filename='configs/config.cfg', verbose=True) # Start the environment -env_process = start_environment(env_name=cfg.env_name) +env_process, env_folder = start_environment(env_name=cfg.env_name) # Load PyGame Screen -screen = pygame_connect() +screen = pygame_connect(phase = cfg.phase) # Load the initial positions for the environment reset_array, level_name, crash_threshold, initZ = initial_positions(cfg.env_name) @@ -35,11 +36,23 @@ cfg = save_network_path(cfg=cfg) ReplayMemory = Memory(cfg.buffer_len) # Connect to Unreal Engine and get the drone handle: client -client, old_posit = connect_drone(ip_address=cfg.ip_address) +client, old_posit = connect_drone(ip_address=cfg.ip_address, phase=cfg.phase) # Define DQN agents +fig_z=[] +fig_nav=[] agent = DeepAgent(cfg, client, name='DQN') -target_agent = DeepAgent(cfg, client, name='Target') +if cfg.phase=='train': + target_agent = DeepAgent(cfg, client, name='Target') + +elif cfg.phase == 'infer': + env_cfg = read_env_cfg(config_filename=env_folder+'config.cfg') + nav_x=[] + nav_y=[] + altitude=[] + f_z, fig_z, ax_z, line_z, fig_nav, ax_nav, nav = initialize_infer(env_cfg=env_cfg, client=client, env_folder=env_folder) + nav_text = ax_nav.text(0, 0, '') + # Initialize variables iter = 0 @@ -75,151 +88,194 @@ f = open(log_path, 'w') while active: try: - - active, automate, cfg.lr, client = check_user_input(active, automate, cfg.lr, cfg.epsilon, agent, cfg.network_path, client, old_posit, initZ) + active, automate, cfg.lr, client = check_user_input(active, automate, cfg.lr, cfg.epsilon, agent, cfg.network_path, client, old_posit, initZ, cfg.phase, fig_z, fig_nav, env_folder) if automate: - start_time = time.time() - if switch_env: - posit1_old = client.simGetVehiclePose() - times_switch=times_switch+1 - level_state[level] = current_state - level_posit[level] = posit1_old - last_crash_array[level] = last_crash - ret_array[level] = ret - distance_array[level] = distance - epi_env_array[int(level/3)] = episode - level = (level + 1) % len(reset_array) + if cfg.phase == 'train': - print('Transferring to level: ', level ,' - ', level_name[level]) + start_time = time.time() + if switch_env: + posit1_old = client.simGetVehiclePose() + times_switch=times_switch+1 + level_state[level] = current_state + level_posit[level] = posit1_old + last_crash_array[level] = last_crash + ret_array[level] = ret + distance_array[level] = distance + epi_env_array[int(level/3)] = episode - if times_switch < len(reset_array): - reset_to_initial(level, reset_array, client) - else: - current_state = level_state[level] - posit1_old = level_posit[level] + level = (level + 1) % len(reset_array) - reset_to_initial(level, reset_array, client) - client.simSetVehiclePose(posit1_old, ignore_collison=True) - time.sleep(0.1) + print('Transferring to level: ', level ,' - ', level_name[level]) - last_crash = last_crash_array[level] - ret = ret_array[level] - distance = distance_array[level] - episode = epi_env_array[int(level/3)] - xxx = client.simGetVehiclePose() - # environ = environ^True - - - action1, action_type, cfg.epsilon, qvals = policy(cfg.epsilon, current_state, iter, cfg.epsilon_saturation, cfg.epsilon_model, cfg.wait_before_train, cfg.num_actions, agent) - - action_word1 = translate_action(action1, cfg.num_actions) - # Take the action - agent.take_action(action1, cfg.num_actions) - time.sleep(0.05) - - posit = client.simGetVehiclePose() - - new_state = agent.get_state() - new_depth1, thresh = agent.get_depth() - - # Get GPS information - posit = client.simGetVehiclePose() - orientation = posit.orientation - position = posit.position - old_p = np.array([old_posit.position.x_val, old_posit.position.y_val]) - new_p = np.array([position.x_val, position.y_val]) - # calculate distance - distance = distance + np.linalg.norm(new_p - old_p) - old_posit = posit - - reward, crash = agent.reward_gen(new_depth1, action1, crash_threshold, thresh) - - ret = ret+reward - agent_state1 = agent.GetAgentState() - - if agent_state1.has_collided: - num_collisions = num_collisions + 1 - print('crash') - crash = True - reward = -1 - data_tuple=[] - data_tuple.append([current_state, action1, new_state, reward, crash]) - err = get_errors(data_tuple, choose, ReplayMemory, cfg.input_size, agent, target_agent, cfg.gamma, cfg.Q_clip) - ReplayMemory.add(err, data_tuple) - - # Train if sufficient frames have been stored - if iter > cfg.wait_before_train: - if iter%cfg.train_interval==0: - # Train the RL network - old_states, Qvals, actions, err, idx = minibatch_double(data_tuple, cfg.batch_size, choose, ReplayMemory, cfg.input_size, agent, target_agent, cfg.gamma, cfg.Q_clip) - - for i in range(cfg.batch_size): - ReplayMemory.update(idx[i], err[i]) - - if print_qval: - print(Qvals) - - if choose: - # Double-DQN - target_agent.train_n(old_states, Qvals, actions, cfg.batch_size, cfg.dropout_rate, cfg.lr, cfg.epsilon, iter) + if times_switch < len(reset_array): + reset_to_initial(level, reset_array, client) else: - agent.train_n(old_states, Qvals,actions, cfg.batch_size, cfg.dropout_rate, cfg.lr, cfg.epsilon, iter) + current_state = level_state[level] + posit1_old = level_posit[level] - if iter % cfg.update_target_interval == 0: - agent.take_action([-1], cfg.num_actions) - print('Switching Target Network') - choose = not choose - agent.save_network(cfg.network_path) + reset_to_initial(level, reset_array, client) + client.simSetVehiclePose(posit1_old, ignore_collison=True) + time.sleep(0.1) - iter += 1 - - time_exec = time.time()-start_time - - mem_percent = process.memory_info()[0]/2.**30 - - s_log = 'Level :{:>2d}: Iter: {:>6d}/{:<5d} {:<8s}-{:>5s} Eps: {:<1.4f} lr: {:>1.8f} Ret = {:<+6.4f} Last Crash = {:<5d} t={:<1.3f} Mem = {:<5.4f} Reward: {:<+1.4f} '.format( - int(level),iter, episode, - action_word1, - action_type, - cfg.epsilon, - cfg.lr, - ret, - last_crash, - time_exec, - mem_percent, - reward) - - print(s_log) - f.write(s_log+'\n') - - last_crash=last_crash+1 - - if crash: - agent.return_plot(ret, episode, int(level/3), mem_percent, iter, distance) - ret=0 - distance=0 - episode = episode + 1 - last_crash=0 - - reset_to_initial(level, reset_array, client) - time.sleep(0.2) - current_state =agent.get_state() - else: - current_state = new_state + last_crash = last_crash_array[level] + ret = ret_array[level] + distance = distance_array[level] + episode = epi_env_array[int(level/3)] + xxx = client.simGetVehiclePose() + # environ = environ^True - if iter%cfg.switch_env_steps==0: - switch_env=True - else: - switch_env=False + action, action_type, cfg.epsilon, qvals = policy(cfg.epsilon, current_state, iter, cfg.epsilon_saturation, cfg.epsilon_model, cfg.wait_before_train, cfg.num_actions, agent) - if iter%cfg.max_iters==0: - automate=False + action_word = translate_action(action, cfg.num_actions) + # Take the action + agent.take_action(action, cfg.num_actions, phase=cfg.phase) + time.sleep(0.05) - # if iter >140: - # active=False + posit = client.simGetVehiclePose() + + new_state = agent.get_state() + new_depth1, thresh = agent.get_depth() + + # Get GPS information + posit = client.simGetVehiclePose() + orientation = posit.orientation + position = posit.position + old_p = np.array([old_posit.position.x_val, old_posit.position.y_val]) + new_p = np.array([position.x_val, position.y_val]) + # calculate distance + distance = distance + np.linalg.norm(new_p - old_p) + old_posit = posit + + reward, crash = agent.reward_gen(new_depth1, action, crash_threshold, thresh) + + ret = ret+reward + agent_state1 = agent.GetAgentState() + + if agent_state1.has_collided: + num_collisions = num_collisions + 1 + print('crash') + crash = True + reward = -1 + data_tuple=[] + data_tuple.append([current_state, action, new_state, reward, crash]) + err = get_errors(data_tuple, choose, ReplayMemory, cfg.input_size, agent, target_agent, cfg.gamma, cfg.Q_clip) + ReplayMemory.add(err, data_tuple) + + # Train if sufficient frames have been stored + if iter > cfg.wait_before_train: + if iter%cfg.train_interval==0: + # Train the RL network + old_states, Qvals, actions, err, idx = minibatch_double(data_tuple, cfg.batch_size, choose, ReplayMemory, cfg.input_size, agent, target_agent, cfg.gamma, cfg.Q_clip) + + for i in range(cfg.batch_size): + ReplayMemory.update(idx[i], err[i]) + + if print_qval: + print(Qvals) + + if choose: + # Double-DQN + target_agent.train_n(old_states, Qvals, actions, cfg.batch_size, cfg.dropout_rate, cfg.lr, cfg.epsilon, iter) + else: + agent.train_n(old_states, Qvals,actions, cfg.batch_size, cfg.dropout_rate, cfg.lr, cfg.epsilon, iter) + + if iter % cfg.update_target_interval == 0: + agent.take_action([-1], cfg.num_actions, phase=cfg.phase) + print('Switching Target Network') + choose = not choose + agent.save_network(cfg.network_path) + + iter += 1 + + time_exec = time.time()-start_time + + mem_percent = process.memory_info()[0]/2.**30 + + s_log = 'Level :{:>2d}: Iter: {:>6d}/{:<5d} {:<8s}-{:>5s} Eps: {:<1.4f} lr: {:>1.8f} Ret = {:<+6.4f} Last Crash = {:<5d} t={:<1.3f} Mem = {:<5.4f} Reward: {:<+1.4f} '.format( + int(level),iter, episode, + action_word, + action_type, + cfg.epsilon, + cfg.lr, + ret, + last_crash, + time_exec, + mem_percent, + reward) + + print(s_log) + f.write(s_log+'\n') + + last_crash=last_crash+1 + + if crash: + agent.return_plot(ret, episode, int(level/3), mem_percent, iter, distance) + ret=0 + distance=0 + episode = episode + 1 + last_crash=0 + + reset_to_initial(level, reset_array, client) + time.sleep(0.2) + current_state =agent.get_state() + else: + current_state = new_state + + + if iter%cfg.switch_env_steps==0: + switch_env=True + else: + switch_env=False + + if iter%cfg.max_iters==0: + automate=False + + # if iter >140: + # active=False + elif cfg.phase == 'infer': + # Inference phase + agent_state = agent.GetAgentState() + if agent_state.has_collided: + print('Drone collided') + print("Total distance traveled: ", np.round(distance, 2)) + active = False + client.moveByVelocityAsync(vx=0, vy=0, vz=0, duration=1).join() + ax_nav.plot(nav_x.pop(), nav_y.pop(), 'r*', linewidth=20) + file_path = env_folder + 'results/' + fig_z.savefig(file_path + 'altitude_variation.png', dpi=1000) + fig_nav.savefig(file_path + 'navigation.png', dpi=1000) + + print('Figures saved') + else: + posit = agent.client.simGetVehiclePose() + distance = distance + np.linalg.norm(np.array([old_posit.position.x_val-posit.position.x_val,old_posit.position.y_val-posit.position.y_val])) + altitude.append(-posit.position.z_val-f_z) + + x_val = posit.position.x_val + y_val = posit.position.y_val + + nav_x.append(env_cfg.alpha*x_val+env_cfg.o_x) + nav_y.append(env_cfg.alpha*y_val+env_cfg.o_y) + nav.set_data(nav_x, nav_y) + nav_text.remove() + nav_text = ax_nav.text(25, 55, 'Distance: '+str(np.round(distance, 2)), style='italic', + bbox={'facecolor': 'white', 'alpha': 0.5}) + + line_z.set_data(np.arange(len(altitude)), altitude) + ax_z.set_xlim(0, len(altitude)) + fig_z.canvas.draw() + fig_z.canvas.flush_events() + + current_state = agent.get_state() + action, action_type, cfg.epsilon, qvals = policy(1, current_state, iter, + cfg.epsilon_saturation, 'inference', + cfg.wait_before_train, cfg.num_actions, agent) + # Take continuous action + agent.take_action(action, cfg.num_actions, phase=cfg.phase) + old_posit=posit diff --git a/network/agent.py b/network/agent.py index ddb89b9..225caa0 100644 --- a/network/agent.py +++ b/network/agent.py @@ -124,7 +124,7 @@ class DeepAgent(): # self.action_array[action[0].astype(int)]+=1 return action.astype(int) - def take_action(self, action, num_actions): + def take_action(self, action, num_actions, phase): # Set Paramaters fov_v = 45 * np.pi / 180 fov_h = 80 * np.pi / 180 @@ -133,7 +133,6 @@ class DeepAgent(): ignore_collision = False sqrt_num_actions = np.sqrt(num_actions) - posit = self.client.simGetVehiclePose() pos = posit.position orientation = posit.orientation @@ -148,25 +147,42 @@ class DeepAgent(): theta = fov_v/sqrt_num_actions * (theta_ind - (sqrt_num_actions - 1) / 2) psi = fov_h / sqrt_num_actions * (psi_ind - (sqrt_num_actions - 1) / 2) - noise_theta = (fov_v / sqrt_num_actions) / 6 - noise_psi = (fov_h / sqrt_num_actions) / 6 - - psi = psi + random.uniform(-1, 1)*noise_psi - theta = theta + random.uniform(-1, 1)*noise_theta # print('Theta: ', theta * 180 / np.pi, end='') # print(' Psi: ', psi * 180 / np.pi) + if phase == 'train': + noise_theta = (fov_v / sqrt_num_actions) / 6 + noise_psi = (fov_h / sqrt_num_actions) / 6 + psi = psi + random.uniform(-1, 1) * noise_psi + theta = theta + random.uniform(-1, 1) * noise_theta - x = pos.x_val + r * np.cos(alpha + psi) - y = pos.y_val + r * np.sin(alpha + psi) - z = pos.z_val + r * np.sin(theta) # -ve because Unreal has -ve z direction going upwards + x = pos.x_val + r * np.cos(alpha + psi) + y = pos.y_val + r * np.sin(alpha + psi) + z = pos.z_val + r * np.sin(theta) # -ve because Unreal has -ve z direction going upwards - self.client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(x, y, z), airsim.to_quaternion(0, 0, alpha + psi)), - ignore_collison=ignore_collision) + self.client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(x, y, z), airsim.to_quaternion(0, 0, alpha + psi)), + ignore_collison=ignore_collision) + elif phase == 'infer': + r_infer=0.5 + vx = r_infer * np.cos(alpha + psi) + vy = r_infer * np.sin(alpha + psi) + vz = r_infer * np.sin(theta) + # TODO + # Take average of previous velocities and current to smoothen out drone movement. + self.client.moveByVelocityAsync(vx=vx, vy=vy, vz=vz, duration=1, + drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=airsim.YawMode(is_rate=False, + yaw_or_rate=180 * (alpha + psi) / np.pi)) + # self.client.moveByVelocityAsync(vx=0, vy=0, vz=0, duration=0.01).join() + # print("") + # print("Throttle:", throttle) + # print('Yaw:', yaw) + # self.client.moveByAngleThrottleAsync(pitch=-0.015, roll=0, throttle=throttle, yaw_rate=yaw, duration=0.2).join() + # self.client.moveByVelocityAsync(vx=0, vy=0, vz=0, duration=0.005) def get_depth(self): responses = self.client.simGetImages([airsim.ImageRequest(2, airsim.ImageType.DepthVis, False, False)]) depth = [] diff --git a/scratch.py b/scratch.py index 73c5888..d26db4a 100644 --- a/scratch.py +++ b/scratch.py @@ -1,10 +1,169 @@ -# Author: Aqeel Anwar(ICSRL) -# Created: 11/8/2019, 4:02 PM -# Email: aqeel.anwar@gatech.edu +# # # Author: Aqeel Anwar(ICSRL) +# # # Created: 11/8/2019, 4:02 PM +# # # Email: aqeel.anwar@gatech.edu +# # +# # +# # # import airsim +# import os +# import time +# from aux_functions import * +# from util.transformations import euler_from_quaternion +# import airsim +# +# # Connect to Unreal Engine and get the drone handle: client +# client, old_posit = connect_drone(ip_address='127.0.0.1', phase='infer') +# +# +# # Async methods returns Future. Call join() to wait for task to complete. +# # client.takeoffAsync().join() +# pos = client.simGetVehiclePose() +# # print(pos) +# num_actions = 25 +# action = [20] +# fov_v = 45 * np.pi / 180 +# fov_h = 80 * np.pi / 180 +# r = 0.4 +# +# sqrt_num_actions = np.sqrt(num_actions) +# +# +# for i in range(5): +# +# client.moveByVelocityAsync(vx=0, vy=0, vz=-1, duration=1).join() +# posit = client.simGetVehiclePose() +# pos = posit.position +# orientation = posit.orientation +# print("Z", pos.z_val) +# cc=1 + +# + +# quat = (orientation.w_val, orientation.x_val, orientation.y_val, orientation.z_val) +# eulers = euler_from_quaternion(quat) +# alpha = eulers[2] +# +# theta_ind = int(action[0] / sqrt_num_actions) +# psi_ind = action[0] % sqrt_num_actions +# +# theta = fov_v/sqrt_num_actions * (theta_ind - (sqrt_num_actions - 1) / 2) +# psi = fov_h / sqrt_num_actions * (psi_ind - (sqrt_num_actions - 1) / 2) +# +# vx = r * np.cos(alpha + psi) +# vy = r * np.sin(alpha + psi) +# vz = r * np.sin(theta) +# print("ang:", 180*(alpha+psi)/np.pi) +# client.moveByVelocityAsync(vx=vx, vy=vy, vz=vz, duration=3, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=180*(alpha+psi)/np.pi)).join() +# client.moveByVelocityAsync(vx=0,vy=0,vz=0, duration=0.1).join() +# +# posit = client.simGetVehiclePose() +# pos = posit.position +# orientation = posit.orientation +# +# quat = (orientation.w_val, orientation.x_val, orientation.y_val, orientation.z_val) +# eulers = euler_from_quaternion(quat) +# alpha = eulers[2] +# print(eulers) +# +# +# # time.sleep(5) +# # client.moveToPositionAsync(-10, 10, -10, 5).join() +# print("Done") +# # +# +# +# def action_take(action): +# action = [22] +# +# fov_v = 45 * np.pi / 180 +# fov_h = 80 * np.pi / 180 +# +# sqrt_num_actions = 5 +# theta_ind = int(action[0] / sqrt_num_actions) +# psi_ind = action[0] % sqrt_num_actions +# +# theta = fov_v / sqrt_num_actions * (theta_ind - (sqrt_num_actions - 1) / 2) +# psi = fov_h / sqrt_num_actions * (psi_ind - (sqrt_num_actions - 1) / 2) +# +# yaw = psi +# throttle = 0.596 + np.sin(theta) +# +# print("") +# print("Throttle:", throttle) +# print('Yaw:', yaw) +# +# return throttle, yaw -import airsim -from aux_functions import * +import numpy as np +# import seaborn as sns +# import matplotlib.pyplot as plt +# import time, random +# # x, y, z = np.random.rand(3, 100) +# plt.style.use('seaborn-pastel') +# cmap = sns.cubehelix_palette(as_cmap=True) +# plt.ion() +# fig = plt.figure() +# ax = fig.add_subplot(111) +# x=[] +# y=[] +# points, = ax.plot(x, y) +# # ax.set_ylim([0, 5]) +# # ax.set_xlim([0, 10]) +# img = plt.imread("unreal_envs/indoor_condo/floor.png") +# ax.imshow(img) +# plt.axis('off') +# plt.title("Navigational map") +# o_x = 1318.49 +# o_y = 769.635 +# # a = 10 +# # b = [3,5] +# # plt.axis('off') +# for i in range(10): +# x.append(i) +# y.append(random.randint(1,5)) +# points.set_data(x,y) +# fig.canvas.draw() +# fig.canvas.flush_events() +# time.sleep(1) -client, old_posit = connect_drone() +import matplotlib.pyplot as plt +import time, random + +altitude=[] +plt.ion() +fig = plt.figure() +ax1 = fig.add_subplot(111) +line1, = ax1.plot(0, 0) +plt.title("Altitude variation") + +nav_x=[] +nav_y=[] +o_x = 1318.49 +o_y = 769.635 +alpha = 36.4 +fig2 = plt.figure() +ax2 = fig2.add_subplot(111) +img = plt.imread("unreal_envs/indoor_cloud/floor.png") +ax2.imshow(img) +plt.axis('off') +plt.title("Navigational map") +plt.plot(o_x, o_y, 'r*', linewidth=5) +nav, = ax2.plot(o_x, o_y) + + + +for i in range(100): + nav_text = ax2.text(25, 55, 'Distance: ' + str(np.round(1.2333, 2)), style='italic', + bbox={'facecolor': 'white', 'alpha': 0.5}) + nav_x.append(alpha * random.randint(0,5) + o_x) + nav_y.append(alpha * random.randint(0,5) + o_y) + nav.set_data(nav_x, nav_y) + # line1.set_ydata(altitude) + # print("alt: ", altitude) + altitude.append(random.randint(0,5)) + line1.set_data(altitude, altitude) + fig.canvas.draw() + fig.canvas.flush_events() + + time.sleep(0.2) \ No newline at end of file