├── README.md ├── resources ├── bg.png ├── bg-DESKTOP-5P8IJ0H.png ├── obstacles_entrance.npy ├── obstacles_sorbuoya.npy ├── obstacles_trondheim.npy ├── bg-DESKTOP-5P8IJ0H-2.png ├── speed_vals.txt └── speed_density.txt ├── logs └── play_results │ ├── path.pdf │ ├── reward.pdf │ ├── progress.pdf │ ├── timesteps.pdf │ ├── collisions.pdf │ ├── cross_track_error.pdf │ └── report.txt ├── gym_auv ├── utils │ ├── PPO_MlpPolicy_trained.pkl │ ├── RadarCNN_concept_example.png │ ├── radarCNN_example_Network.pkl │ ├── __pycache__ │ │ ├── helpers.cpython-37.pyc │ │ ├── helpers.cpython-38.pyc │ │ ├── constants.cpython-37.pyc │ │ ├── constants.cpython-38.pyc │ │ ├── geomutils.cpython-37.pyc │ │ ├── geomutils.cpython-38.pyc │ │ └── radarCNN.cpython-38.pyc │ ├── helpers.py │ ├── geomutils.py │ ├── constants.py │ └── radarCNN.py ├── __pycache__ │ ├── __init__.cpython-37.pyc │ ├── __init__.cpython-38.pyc │ ├── environment.cpython-37.pyc │ ├── environment.cpython-38.pyc │ ├── reporting.cpython-37.pyc │ └── reporting.cpython-38.pyc ├── envs │ ├── __init__.py │ ├── __pycache__ │ │ ├── __init__.cpython-37.pyc │ │ ├── __init__.cpython-38.pyc │ │ ├── realworld.cpython-37.pyc │ │ ├── realworld.cpython-38.pyc │ │ ├── testscenario.cpython-37.pyc │ │ ├── testscenario.cpython-38.pyc │ │ ├── movingobstacles.cpython-37.pyc │ │ └── movingobstacles.cpython-38.pyc │ ├── movingobstacles.py │ ├── testscenario.py │ └── realworld.py ├── objects │ ├── __pycache__ │ │ ├── path.cpython-37.pyc │ │ ├── path.cpython-38.pyc │ │ ├── rewarder.cpython-37.pyc │ │ ├── rewarder.cpython-38.pyc │ │ ├── vessel.cpython-37.pyc │ │ ├── vessel.cpython-38.pyc │ │ ├── obstacles.cpython-37.pyc │ │ └── obstacles.cpython-38.pyc │ ├── path.py │ ├── obstacles.py │ ├── rewarder.py │ └── vessel.py ├── rendering │ ├── __pycache__ │ │ ├── render2d.cpython-37.pyc │ │ ├── render2d.cpython-38.pyc │ │ ├── render3d.cpython-37.pyc │ │ └── render3d.cpython-38.pyc │ ├── render3d.py │ └── render2d_old.py ├── __init__.py └── environment.py └── environment.yml /README.md: -------------------------------------------------------------------------------- 1 | # gym-auv-SB3 2 | gym-auv repository upgraded to Stable-Baselines 3 3 | -------------------------------------------------------------------------------- /resources/bg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/resources/bg.png -------------------------------------------------------------------------------- /logs/play_results/path.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/logs/play_results/path.pdf -------------------------------------------------------------------------------- /logs/play_results/reward.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/logs/play_results/reward.pdf -------------------------------------------------------------------------------- /logs/play_results/progress.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/logs/play_results/progress.pdf -------------------------------------------------------------------------------- /logs/play_results/timesteps.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/logs/play_results/timesteps.pdf -------------------------------------------------------------------------------- /logs/play_results/collisions.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/logs/play_results/collisions.pdf -------------------------------------------------------------------------------- /resources/bg-DESKTOP-5P8IJ0H.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/resources/bg-DESKTOP-5P8IJ0H.png -------------------------------------------------------------------------------- /resources/obstacles_entrance.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/resources/obstacles_entrance.npy -------------------------------------------------------------------------------- /resources/obstacles_sorbuoya.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/resources/obstacles_sorbuoya.npy -------------------------------------------------------------------------------- /resources/obstacles_trondheim.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/resources/obstacles_trondheim.npy -------------------------------------------------------------------------------- /resources/bg-DESKTOP-5P8IJ0H-2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/resources/bg-DESKTOP-5P8IJ0H-2.png -------------------------------------------------------------------------------- /gym_auv/utils/PPO_MlpPolicy_trained.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/utils/PPO_MlpPolicy_trained.pkl -------------------------------------------------------------------------------- /logs/play_results/cross_track_error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/logs/play_results/cross_track_error.pdf -------------------------------------------------------------------------------- /gym_auv/__pycache__/__init__.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/__pycache__/__init__.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/utils/RadarCNN_concept_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/utils/RadarCNN_concept_example.png -------------------------------------------------------------------------------- /gym_auv/utils/radarCNN_example_Network.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/utils/radarCNN_example_Network.pkl -------------------------------------------------------------------------------- /gym_auv/__pycache__/environment.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/__pycache__/environment.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/__pycache__/environment.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/__pycache__/environment.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/__pycache__/reporting.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/__pycache__/reporting.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/__pycache__/reporting.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/__pycache__/reporting.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/envs/__init__.py: -------------------------------------------------------------------------------- 1 | from gym_auv.envs.realworld import * 2 | from gym_auv.envs.movingobstacles import * 3 | from gym_auv.envs.testscenario import * -------------------------------------------------------------------------------- /gym_auv/envs/__pycache__/__init__.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/envs/__pycache__/__init__.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/envs/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/envs/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/objects/__pycache__/path.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/objects/__pycache__/path.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/objects/__pycache__/path.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/objects/__pycache__/path.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/utils/__pycache__/helpers.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/utils/__pycache__/helpers.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/utils/__pycache__/helpers.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/utils/__pycache__/helpers.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/envs/__pycache__/realworld.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/envs/__pycache__/realworld.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/envs/__pycache__/realworld.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/envs/__pycache__/realworld.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/objects/__pycache__/rewarder.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/objects/__pycache__/rewarder.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/objects/__pycache__/rewarder.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/objects/__pycache__/rewarder.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/objects/__pycache__/vessel.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/objects/__pycache__/vessel.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/objects/__pycache__/vessel.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/objects/__pycache__/vessel.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/utils/__pycache__/constants.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/utils/__pycache__/constants.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/utils/__pycache__/constants.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/utils/__pycache__/constants.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/utils/__pycache__/geomutils.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/utils/__pycache__/geomutils.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/utils/__pycache__/geomutils.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/utils/__pycache__/geomutils.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/utils/__pycache__/radarCNN.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/utils/__pycache__/radarCNN.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/envs/__pycache__/testscenario.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/envs/__pycache__/testscenario.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/envs/__pycache__/testscenario.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/envs/__pycache__/testscenario.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/objects/__pycache__/obstacles.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/objects/__pycache__/obstacles.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/objects/__pycache__/obstacles.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/objects/__pycache__/obstacles.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/rendering/__pycache__/render2d.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/rendering/__pycache__/render2d.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/rendering/__pycache__/render2d.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/rendering/__pycache__/render2d.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/rendering/__pycache__/render3d.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/rendering/__pycache__/render3d.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/rendering/__pycache__/render3d.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/rendering/__pycache__/render3d.cpython-38.pyc -------------------------------------------------------------------------------- /gym_auv/envs/__pycache__/movingobstacles.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/envs/__pycache__/movingobstacles.cpython-37.pyc -------------------------------------------------------------------------------- /gym_auv/envs/__pycache__/movingobstacles.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ThomasNLarsen/gym-auv-SB3/HEAD/gym_auv/envs/__pycache__/movingobstacles.cpython-38.pyc -------------------------------------------------------------------------------- /logs/play_results/report.txt: -------------------------------------------------------------------------------- 1 | # PERFORMANCE METRICS (LAST 100 EPISODES AVG.) 2 | Episodes 1 3 | Avg. Reward -1132.51 4 | Std. Reward 0.00 5 | Avg. Progress 99.01% 6 | Avg. Collisions 0.00 7 | No Collisions 100.00% 8 | Avg. Cross-Track Error 152.87 9 | Avg. Timesteps 1742.00 10 | Avg. Duration 1742.00 11 | Avg. Pathlength 800.00 12 | Avg. Speed 0.46 13 | Max. Speed 0.46 14 | -------------------------------------------------------------------------------- /gym_auv/utils/helpers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import gym_auv.utils.geomutils as geom 3 | 4 | def generate_obstacle(rng, path, vessel, displacement_dist_std=150, obst_radius_distr=np.random.poisson, obst_radius_mean=30): 5 | min_distance = 0 6 | while min_distance <= 0: 7 | obst_displacement_dist = np.random.normal(0, displacement_dist_std) 8 | obst_arclength = (0.1 + 0.8*rng.rand())*path.length 9 | obst_position = path(obst_arclength) 10 | obst_displacement_angle = geom.princip(path.get_direction(obst_arclength) - np.pi/2) 11 | obst_position += obst_displacement_dist*np.array([ 12 | np.cos(obst_displacement_angle), 13 | np.sin(obst_displacement_angle) 14 | ]) 15 | obst_radius = max(1, obst_radius_distr(obst_radius_mean)) 16 | 17 | vessel_distance_vec = geom.Rzyx(0, 0, -vessel.heading).dot( 18 | np.hstack([obst_position - vessel.position, 0]) 19 | ) 20 | vessel_distance = np.linalg.norm(vessel_distance_vec) - vessel.width - obst_radius 21 | goal_distance = np.linalg.norm(obst_position - path(path.length)) - obst_radius 22 | min_distance = min(vessel_distance, goal_distance) 23 | 24 | return (obst_position, obst_radius) -------------------------------------------------------------------------------- /gym_auv/utils/geomutils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def princip(angle): 5 | return ((angle + np.pi) % (2*np.pi)) - np.pi 6 | 7 | def Rzyx(phi, theta, psi): 8 | cphi = np.cos(phi) 9 | sphi = np.sin(phi) 10 | cth = np.cos(theta) 11 | sth = np.sin(theta) 12 | cpsi = np.cos(psi) 13 | spsi = np.sin(psi) 14 | 15 | return np.vstack([ 16 | np.hstack([cpsi*cth, -spsi*cphi+cpsi*sth*sphi, spsi*sphi+cpsi*cphi*sth]), 17 | np.hstack([spsi*cth, cpsi*cphi+sphi*sth*spsi, -cpsi*sphi+sth*spsi*cphi]), 18 | np.hstack([-sth, cth*sphi, cth*cphi]) 19 | ]) 20 | 21 | def Rz(psi): 22 | cpsi = np.cos(psi) 23 | spsi = np.sin(psi) 24 | 25 | return np.vstack([ 26 | np.hstack([cpsi, -spsi, 0]), 27 | np.hstack([spsi, cpsi, -0]), 28 | np.hstack([0, 0, 1]) 29 | ]) 30 | 31 | def Rzyx_dpsi(phi, theta, psi): 32 | cphi = np.cos(phi) 33 | sphi = np.sin(phi) 34 | cth = np.cos(theta) 35 | sth = np.sin(theta) 36 | cpsi = np.cos(psi) 37 | spsi = np.sin(psi) 38 | 39 | return np.vstack([ 40 | np.hstack([-spsi*cth, -cpsi*cphi-spsi*sth*sphi, cpsi*sphi-spsi*cphi*sth]), 41 | np.hstack([cpsi*cth, -spsi*cphi+sphi*sth*cpsi, spsi*sphi+sth*cpsi*cphi]), 42 | np.hstack([0, 0, 0]) 43 | ]) 44 | 45 | def to_homogeneous(x): 46 | return np.array([x[0], x[1], 1]) 47 | 48 | def to_cartesian(x): 49 | return np.array([x[0], x[1]]) -------------------------------------------------------------------------------- /gym_auv/utils/constants.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | m = 23.8 5 | x_g = 0.046 6 | I_z = 1.760 7 | X_udot = -2.0 8 | Y_vdot = -10.0 9 | Y_rdot = 0.0 10 | N_rdot = -1.0 11 | N_vdot = 0.0 12 | X_u = -2.0 13 | Y_v = -7.0 14 | Y_r = -0.1 15 | N_v = -0.1 16 | N_r = -0.5 17 | X_uu = -1.32742 18 | Y_vv = -80 19 | Y_rr = 0.3 20 | N_vv = -1.5 21 | N_rr = -9.1 22 | Y_uvb = -0.5*1000*np.pi*1.24*(0.15/2)**2 23 | Y_uvf = -1000*3*0.0064 24 | Y_urf = -0.4*Y_uvf 25 | N_uvb = (-0.65*1.08 + 0.4)*Y_uvb 26 | N_uvf = -0.4*Y_uvf 27 | N_urf = -0.4*Y_urf 28 | Y_uudr = 19.2 29 | N_uudr = -0.4*Y_uudr 30 | 31 | MAX_SPEED = 2 32 | 33 | M = np.array([[m - X_udot, 0, 0], 34 | [0, m - Y_vdot, m*x_g - Y_rdot], 35 | [0, m*x_g - N_vdot, I_z - N_rdot]] 36 | ) 37 | M_inv = np.linalg.inv(M) 38 | 39 | D = np.array([ 40 | [2.0, 0, 0], 41 | [0, 7.0, -2.5425], 42 | [0, -2.5425, 1.422] 43 | ]) 44 | 45 | def B(nu): 46 | return np.array([ 47 | [1, 0], 48 | [0, -1.7244], 49 | [0, 1], 50 | ]) 51 | 52 | def C(nu): 53 | u = nu[0] 54 | v = nu[1] 55 | r = nu[2] 56 | C = np.array([ 57 | [0, 0, -33.8*v + 11.748*r], 58 | [0, 0, 25.8*u], 59 | [33.8*v - 11.748*r, -25.8*u, 0] 60 | ]) 61 | return C 62 | 63 | def N(nu): 64 | u = nu[0] 65 | v = nu[1] 66 | r = nu[2] 67 | N = np.array([ 68 | [-X_u, 0, 0], 69 | [0, -Y_v, m*u - Y_r], 70 | [0, -N_v, m*x_g*u-N_r] 71 | ]) 72 | return N 73 | -------------------------------------------------------------------------------- /resources/speed_vals.txt: -------------------------------------------------------------------------------- 1 | 5.973753401783561678e-02 2 | 7.920977979642182532e-02 3 | 9.868202557500804772e-02 4 | 1.181542713535942563e-01 5 | 1.376265171321804648e-01 6 | 1.570987629107666872e-01 7 | 1.765710086893528818e-01 8 | 1.960432544679390765e-01 9 | 2.155155002465253267e-01 10 | 2.349877460251115213e-01 11 | 2.544599918036977160e-01 12 | 2.739322375822839661e-01 13 | 2.934044833608701608e-01 14 | 3.128767291394563554e-01 15 | 3.323489749180426056e-01 16 | 3.518212206966288003e-01 17 | 3.712934664752149949e-01 18 | 3.907657122538011896e-01 19 | 4.102379580323874397e-01 20 | 4.297102038109736344e-01 21 | 4.491824495895598846e-01 22 | 4.686546953681460792e-01 23 | 4.881269411467322739e-01 24 | 5.075991869253184685e-01 25 | 5.270714327039047742e-01 26 | 5.465436784824908578e-01 27 | 5.660159242610771635e-01 28 | 5.854881700396632471e-01 29 | 6.049604158182495528e-01 30 | 6.244326615968358585e-01 31 | 6.439049073754219421e-01 32 | 6.633771531540082478e-01 33 | 6.828493989325943314e-01 34 | 7.023216447111806371e-01 35 | 7.217938904897667207e-01 36 | 7.412661362683530264e-01 37 | 7.607383820469393321e-01 38 | 7.802106278255254157e-01 39 | 7.996828736041117214e-01 40 | 8.191551193826978050e-01 41 | 8.386273651612841107e-01 42 | 8.580996109398703053e-01 43 | 8.775718567184565000e-01 44 | 8.970441024970428057e-01 45 | 9.165163482756288893e-01 46 | 9.359885940542151950e-01 47 | 9.554608398328012786e-01 48 | 9.749330856113875843e-01 49 | 9.944053313899738900e-01 50 | 1.013877577168559974e+00 51 | 1.033349822947146279e+00 52 | 1.052822068725732585e+00 53 | 1.072294314504318669e+00 54 | 1.091766560282904752e+00 55 | 1.111238806061491058e+00 56 | 1.130711051840077364e+00 57 | 1.150183297618663447e+00 58 | 1.169655543397249753e+00 59 | 1.189127789175835836e+00 60 | 1.208600034954422142e+00 61 | 1.228072280733008448e+00 62 | 1.247544526511594531e+00 63 | 1.267016772290180615e+00 64 | 1.286489018068766921e+00 65 | 1.305961263847353226e+00 66 | 1.325433509625939532e+00 67 | 1.344905755404525616e+00 68 | 1.364378001183111699e+00 69 | 1.383850246961698005e+00 70 | 1.403322492740284311e+00 71 | 1.422794738518870394e+00 72 | 1.442266984297456700e+00 73 | 1.461739230076042784e+00 74 | 1.481211475854629089e+00 75 | 1.500683721633215395e+00 76 | 1.520155967411801479e+00 77 | 1.539628213190387784e+00 78 | 1.559100458968973868e+00 79 | 1.578572704747560174e+00 80 | 1.598044950526146479e+00 81 | 1.617517196304732563e+00 82 | 1.636989442083318647e+00 83 | 1.656461687861904952e+00 84 | 1.675933933640491258e+00 85 | 1.695406179419077564e+00 86 | 1.714878425197663647e+00 87 | 1.734350670976249731e+00 88 | 1.753822916754836037e+00 89 | 1.773295162533422342e+00 90 | 1.792767408312008426e+00 91 | 1.812239654090594732e+00 92 | 1.831711899869180815e+00 93 | 1.851184145647767121e+00 94 | 1.870656391426353427e+00 95 | 1.890128637204939510e+00 96 | 1.909600882983525816e+00 97 | 1.929073128762111899e+00 98 | 1.948545374540698205e+00 99 | 1.968017620319284511e+00 100 | 1.987489866097870594e+00 101 | -------------------------------------------------------------------------------- /resources/speed_density.txt: -------------------------------------------------------------------------------- 1 | 1.206554933469061210e-02 2 | 1.064313240747473237e-02 3 | 8.748091325528974810e-03 4 | 7.702864829491747230e-03 5 | 7.421108121864320763e-03 6 | 7.262051915945612161e-03 7 | 7.734676070675489123e-03 8 | 7.857376572384206989e-03 9 | 7.666509125281756494e-03 10 | 8.970770013815167199e-03 11 | 9.343415981967570505e-03 12 | 1.056587653602850292e-02 13 | 1.082036646549843634e-02 14 | 1.208827164982185717e-02 15 | 1.391060132334763326e-02 16 | 1.582836472042463416e-02 17 | 2.081364066021958864e-02 18 | 2.681687631789427678e-02 19 | 3.473787537264597103e-02 20 | 4.281793063331636867e-02 21 | 5.079346324438304555e-02 22 | 5.776466952664872923e-02 23 | 6.304988002617610554e-02 24 | 6.288627935723115070e-02 25 | 5.821911582927360995e-02 26 | 5.194321239002399476e-02 27 | 4.778502872100632431e-02 28 | 4.459481567657965634e-02 29 | 4.031847596887951773e-02 30 | 3.600578055696938840e-02 31 | 3.023431251363338859e-02 32 | 2.396295353741001791e-02 33 | 1.735530429724423812e-02 34 | 1.446048134952374054e-02 35 | 1.312440921980658794e-02 36 | 1.228822802297680448e-02 37 | 1.062495455536973700e-02 38 | 8.180033447247873901e-03 39 | 5.457900094524830575e-03 40 | 4.276339707700137736e-03 41 | 2.908456336799243678e-03 42 | 2.144986548389442131e-03 43 | 1.617828837344579282e-03 44 | 1.213371628008434516e-03 45 | 7.589253253835527048e-04 46 | 7.543808623573038628e-04 47 | 7.725587144622991223e-04 48 | 6.544026757798298817e-04 49 | 6.998473060423180845e-04 50 | 6.453137497273321978e-04 51 | 4.908020068348724384e-04 52 | 7.362030102523086034e-04 53 | 8.770813640660219453e-04 54 | 9.407038464335054076e-04 55 | 1.090671126299716433e-03 56 | 1.376972296953391959e-03 57 | 2.013197120628226582e-03 58 | 2.231331345888169695e-03 59 | 2.181342252599432867e-03 60 | 1.831418599578273770e-03 61 | 1.636006689449574650e-03 62 | 1.372427833927143117e-03 63 | 1.231549480113429884e-03 64 | 7.998254926197920656e-04 65 | 6.816694539373227167e-04 66 | 7.225696211735621859e-04 67 | 4.953464698611211720e-04 68 | 2.999345597324219975e-04 69 | 2.635788555224314786e-04 70 | 2.317676143386897475e-04 71 | 2.499454664436850070e-04 72 | 2.590343924961826367e-04 73 | 2.772122446011779503e-04 74 | 2.908456336799243678e-04 75 | 4.226350614411401342e-04 76 | 4.635352286773794950e-04 77 | 6.498582127535810398e-04 78 | 9.497927724860030915e-04 79 | 1.090671126299716433e-03 80 | 1.304260888533410922e-03 81 | 1.581473133134588763e-03 82 | 1.785973969315785568e-03 83 | 2.108630844179451829e-03 84 | 2.040463898785719633e-03 85 | 2.490365738384352386e-03 86 | 2.490365738384352386e-03 87 | 2.331309532465643784e-03 88 | 2.031374972733221949e-03 89 | 1.717807023922053372e-03 90 | 1.395150149058387327e-03 91 | 9.452483094597542495e-04 92 | 8.589035119610266859e-04 93 | 4.726241547298771248e-04 94 | 2.953900967061732098e-04 95 | 2.408565403911873772e-04 96 | 1.408783538137133690e-04 97 | 1.090671126299716379e-04 98 | 9.543372355122519335e-05 99 | 5.453355631498581896e-05 100 | 4.090016723623936761e-05 101 | -------------------------------------------------------------------------------- /gym_auv/objects/path.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | import numpy as np 3 | import numpy.linalg as linalg 4 | import shapely.geometry 5 | from scipy.optimize import minimize 6 | 7 | from scipy import interpolate 8 | 9 | import gym_auv.utils.geomutils as geom 10 | 11 | def _arc_len(coords): 12 | diff = np.diff(coords, axis=1) 13 | delta_arc = np.sqrt(np.sum(diff ** 2, axis=0)) 14 | return np.concatenate([[0], np.cumsum(delta_arc)]) 15 | 16 | class Path(): 17 | def __init__(self, waypoints:list) -> None: 18 | """Initializes path based on specified waypoints.""" 19 | 20 | self.init_waypoints = waypoints.copy() 21 | 22 | for _ in range(3): 23 | self._arclengths = _arc_len(waypoints) 24 | path_coords = interpolate.pchip(x=self._arclengths, y=waypoints, axis=1) 25 | path_derivatives = path_coords.derivative() 26 | path_dderivatives = path_derivatives.derivative() 27 | waypoints = path_coords(np.linspace(self._arclengths[0], self._arclengths[-1], 1000)) 28 | 29 | self._waypoints = waypoints.copy() 30 | self._path_coords = path_coords 31 | self._path_derivatives = path_derivatives 32 | self._path_dderivatives = path_dderivatives 33 | 34 | #print("DEBUG: path.py, Path() class initialization, self.length =", self.length) 35 | S = np.linspace(0, round(self.length), 10*round(self.length)) 36 | self._points = np.transpose(self._path_coords(S)) 37 | self._linestring = shapely.geometry.LineString(self._points) 38 | 39 | @property 40 | def length(self) -> float: 41 | """Length of path in meters.""" 42 | return self._arclengths[-1] 43 | 44 | @property 45 | def start(self) -> np.ndarray: 46 | """Coordinates of the path's starting point.""" 47 | return self._path_coords(0) 48 | 49 | @property 50 | def end(self) -> np.ndarray: 51 | """Coordinates of the path's end point.""" 52 | return self._path_coords(self.length) 53 | 54 | def __call__(self, arclength:float) -> np.ndarray: 55 | """ 56 | Returns the (x,y) point corresponding to the 57 | specified arclength. 58 | 59 | Returns 60 | ------- 61 | point : np.array 62 | """ 63 | return self._path_coords(arclength) 64 | 65 | def get_direction(self, arclength:float) -> float: 66 | """ 67 | Returns the direction in radians with respect to the 68 | positive x-axis. 69 | 70 | Returns 71 | ------- 72 | direction : float 73 | """ 74 | derivative = self._path_derivatives(arclength) 75 | return np.arctan2(derivative[1], derivative[0]) 76 | 77 | def get_closest_arclength(self, position:np.ndarray) -> float: 78 | """ 79 | Returns the arc length value corresponding to the point 80 | on the path which is closest to the specified position. 81 | 82 | Returns 83 | ------- 84 | point : np.array 85 | """ 86 | return self._linestring.project(shapely.geometry.Point(position)) 87 | 88 | class RandomCurveThroughOrigin(Path): 89 | def __init__(self, rng, nwaypoints, length=400): 90 | angle_init = 2*np.pi*(rng.rand() - 0.5) 91 | start = np.array([0.5*length*np.cos(angle_init), 0.5*length*np.sin(angle_init)]) 92 | end = -np.array(start) 93 | waypoints = np.vstack([start, end]) 94 | for waypoint in range(nwaypoints // 2): 95 | newpoint1 = ((nwaypoints // 2 - waypoint) 96 | * start / (nwaypoints // 2 + 1) 97 | + length / (nwaypoints // 2 + 1) 98 | * (rng.rand()-0.5)) 99 | newpoint2 = ((nwaypoints // 2 - waypoint) 100 | * end / (nwaypoints // 2 + 1) 101 | + length / (nwaypoints // 2 + 1) 102 | * (rng.rand()-0.5)) 103 | waypoints = np.vstack([waypoints[:waypoint+1, :], 104 | newpoint1, 105 | np.array([0, 0]), 106 | newpoint2, 107 | waypoints[-1*waypoint-1:, :]]) 108 | super().__init__(np.transpose(waypoints)) -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | name: SB3 2 | channels: 3 | - pytorch 4 | - conda-forge 5 | - defaults 6 | dependencies: 7 | - _pytorch_select=0.1=cpu_0 8 | - absl-py=0.13.0=pyhd8ed1ab_0 9 | - aiohttp=3.7.4.post0=py38h294d835_0 10 | - async-timeout=3.0.1=py_1000 11 | - attrs=21.2.0=pyhd8ed1ab_0 12 | - blas=1.0=mkl 13 | - blinker=1.4=py_1 14 | - blosc=1.21.0=h0e60522_0 15 | - brotlipy=0.7.0=py38h294d835_1001 16 | - bzip2=1.0.8=h8ffe710_4 17 | - ca-certificates=2021.10.8=h5b45459_0 18 | - cachetools=4.2.2=pyhd8ed1ab_0 19 | - certifi=2021.10.8=py38haa244fe_1 20 | - cffi=1.14.6=py38hd8c33c5_0 21 | - chardet=4.0.0=py38haa244fe_1 22 | - click=8.0.1=py38haa244fe_0 23 | - cloudpickle=1.6.0=py_0 24 | - colorama=0.4.4=pyh9f0ad1d_0 25 | - cryptography=3.4.7=py38hd7da0ea_0 26 | - cudatoolkit=11.1.1=heb2d755_7 27 | - cycler=0.10.0=py_2 28 | - dataclasses=0.8=pyhc8e2a94_1 29 | - ffmpeg=4.3.1=ha925a31_0 30 | - freetype=2.10.4=h546665d_1 31 | - future=0.18.2=py38haa244fe_3 32 | - geos=3.9.1=h39d44d4_2 33 | - google-auth=1.32.1=pyh6c4a22f_0 34 | - google-auth-oauthlib=0.4.1=py_2 35 | - grpcio=1.38.1=py38he5377a8_0 36 | - gym=0.21.0=py38h595d716_2 37 | - hdf5=1.10.6=nompi_h5268f04_1114 38 | - idna=2.10=pyh9f0ad1d_0 39 | - importlib-metadata=4.6.1=py38haa244fe_0 40 | - intel-openmp=2019.4=245 41 | - jbig=2.1=h8d14728_2003 42 | - joblib=1.0.1=pyhd8ed1ab_0 43 | - jpeg=9d=h8ffe710_0 44 | - kiwisolver=1.3.1=py38hbd9d945_1 45 | - krb5=1.19.1=hbae68bd_0 46 | - lcms2=2.12=h2a16943_0 47 | - lerc=2.2.1=h0e60522_0 48 | - libblas=3.9.0=1_h8933c1f_netlib 49 | - libcblas=3.9.0=5_hd5c7e75_netlib 50 | - libcurl=7.77.0=h789b8ee_0 51 | - libdeflate=1.7=h8ffe710_5 52 | - liblapack=3.9.0=5_hd5c7e75_netlib 53 | - libmklml=2019.0.5=haa95532_0 54 | - libpng=1.6.37=h1d00b33_2 55 | - libprotobuf=3.17.2=h7755175_0 56 | - libssh2=1.9.0=h680486a_6 57 | - libtiff=4.3.0=h0c97f57_1 58 | - libuv=1.41.1=h8ffe710_0 59 | - lz4-c=1.9.3=h8ffe710_0 60 | - m2w64-gcc-libgfortran=5.3.0=6 61 | - m2w64-gcc-libs=5.3.0=7 62 | - m2w64-gcc-libs-core=5.3.0=7 63 | - m2w64-gmp=6.1.0=2 64 | - m2w64-libwinpthread-git=5.0.0.4634.697f757=2 65 | - markdown=3.3.4=pyhd8ed1ab_0 66 | - matplotlib-base=3.4.2=py38heae8d8c_0 67 | - mkl=2019.4=245 68 | - mkl-service=2.3.0=py38h196d8e1_0 69 | - mock=4.0.3=py38haa244fe_1 70 | - msys2-conda-epoch=20160418=1 71 | - multidict=5.1.0=py38h294d835_1 72 | - ninja=1.10.2=h5362a0b_0 73 | - numexpr=2.7.3=py38h4c96930_0 74 | - numpy=1.21.0=py38h09042cb_0 75 | - oauthlib=3.1.1=pyhd8ed1ab_0 76 | - olefile=0.46=pyh9f0ad1d_1 77 | - openjpeg=2.4.0=hb211442_1 78 | - openssl=1.1.1l=h8ffe710_0 79 | - pandas=1.3.0=py38h60cbd38_0 80 | - pillow=7.1.2=py38h7011068_0 81 | - pip=21.1.3=pyhd8ed1ab_0 82 | - progressbar=2.5=py_0 83 | - protobuf=3.17.2=py38h885f38d_0 84 | - pyasn1=0.4.8=py_0 85 | - pyasn1-modules=0.2.7=py_0 86 | - pycparser=2.20=pyh9f0ad1d_2 87 | - pyglet=1.5.16=py38haa244fe_1 88 | - pyjwt=2.1.0=pyhd8ed1ab_0 89 | - pyopenssl=20.0.1=pyhd8ed1ab_0 90 | - pyparsing=2.4.7=pyh9f0ad1d_0 91 | - pysocks=1.7.1=py38haa244fe_3 92 | - pytables=3.6.1=py38h153c448_3 93 | - python=3.8.10=h7840368_1_cpython 94 | - python-dateutil=2.8.1=py_0 95 | - python_abi=3.8=2_cp38 96 | - pytorch=1.9.0=py3.8_cuda11.1_cudnn8_0 97 | - pytz=2021.1=pyhd8ed1ab_0 98 | - pyu2f=0.1.5=pyhd8ed1ab_0 99 | - pywin32=300=py38h294d835_0 100 | - requests=2.25.1=pyhd3deb0d_0 101 | - requests-oauthlib=1.3.0=pyh9f0ad1d_0 102 | - rsa=4.7.2=pyh44b312d_0 103 | - scikit-learn=0.24.2=py38h5d5d464_0 104 | - scipy=1.7.0=py38he847743_0 105 | - setuptools=49.6.0=py38haa244fe_3 106 | - shapely=1.7.1=py38h13ff51f_5 107 | - six=1.16.0=pyh6c4a22f_0 108 | - sqlite=3.36.0=h8ffe710_0 109 | - stable-baselines3=1.1.0=pyhd8ed1ab_0 110 | - tensorboard=2.5.0=pyhd8ed1ab_0 111 | - tensorboard-data-server=0.6.0=py38haa244fe_0 112 | - tensorboard-plugin-wit=1.8.0=pyh44b312d_0 113 | - threadpoolctl=2.1.0=pyh5ca1d4c_0 114 | - tk=8.6.10=h8ffe710_1 115 | - torchaudio=0.9.0=py38 116 | - torchvision=0.2.2=py_3 117 | - tornado=6.1=py38h294d835_1 118 | - tqdm=4.61.2=pyhd8ed1ab_1 119 | - typing-extensions=3.10.0.0=hd8ed1ab_0 120 | - typing_extensions=3.10.0.0=pyha770c72_0 121 | - ucrt=10.0.20348.0=h57928b3_0 122 | - urllib3=1.26.6=pyhd8ed1ab_0 123 | - vc=14.2=hb210afc_5 124 | - vs2015_runtime=14.29.30037=h902a5da_5 125 | - werkzeug=2.0.1=pyhd8ed1ab_0 126 | - wheel=0.36.2=pyhd3deb0d_0 127 | - win10toast=0.9=py38h32f6830_2 128 | - win_inet_pton=1.1.0=py38haa244fe_2 129 | - wincertstore=0.2=py38haa244fe_1006 130 | - xz=5.2.5=h62dcd97_1 131 | - yarl=1.6.3=py38h294d835_2 132 | - zipp=3.5.0=pyhd8ed1ab_0 133 | - zlib=1.2.11=h62dcd97_1010 134 | - zstd=1.5.0=h6255e5f_0 135 | - pip: 136 | - pywavefront==1.3.3 137 | prefix: C:\Users\Thomas\anaconda3\envs\SB3 138 | -------------------------------------------------------------------------------- /gym_auv/objects/obstacles.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import shapely.geometry 3 | import shapely.affinity 4 | import gym_auv.utils.geomutils as geom 5 | from abc import ABC, abstractmethod 6 | import copy 7 | 8 | class BaseObstacle(ABC): 9 | def __init__(self, *args, **kwargs) -> None: 10 | """Initializes obstacle instance by calling private setup method implemented by 11 | subclasses of BaseObstacle and calculating obstacle boundary.""" 12 | self._prev_position = [] 13 | self._prev_heading = [] 14 | self._setup(*args, **kwargs) 15 | self._boundary = self._calculate_boundary() 16 | if not self._boundary.is_valid: 17 | self._boundary = self._boundary.buffer(0) 18 | self._init_boundary = copy.deepcopy(self._boundary) 19 | 20 | 21 | @property 22 | def boundary(self) -> shapely.geometry.Polygon: 23 | """shapely.geometry.Polygon object used for simulating the 24 | sensors' detection of the obstacle instance.""" 25 | return self._boundary 26 | 27 | @property 28 | def init_boundary(self) -> shapely.geometry.Polygon: 29 | """shapely.geometry.Polygon object used for simulating the 30 | sensors' detection of the obstacle instance.""" 31 | return self._init_boundary 32 | 33 | def update(self, dt:float) -> None: 34 | """Updates the obstacle according to its dynamic behavior, e.g. 35 | a ship model and recalculates the boundary.""" 36 | has_changed = self._update(dt) 37 | if has_changed: 38 | self._boundary = self._calculate_boundary() 39 | if not self._boundary.is_valid: 40 | self._boundary = self._boundary.buffer(0) 41 | 42 | @abstractmethod 43 | def _calculate_boundary(self) -> shapely.geometry.Polygon: 44 | """Returns a shapely.geometry.Polygon instance representing the obstacle 45 | given its current state.""" 46 | 47 | @abstractmethod 48 | def _setup(self, *args, **kwargs) -> None: 49 | """Initializes the obstacle given the constructor parameters provided to 50 | the specific BaseObstacle extension.""" 51 | 52 | def _update(self, _dt:float) -> bool: 53 | """Performs the specific update routine associated with the obstacle. 54 | Returns a boolean flag representing whether something changed or not. 55 | 56 | Returns 57 | ------- 58 | has_changed : bool 59 | """ 60 | return False 61 | 62 | @property 63 | def path_taken(self) -> list: 64 | """Returns an array holding the path of the obstacle in cartesian 65 | coordinates.""" 66 | return self._prev_position 67 | 68 | @property 69 | def heading_taken(self) -> list: 70 | """Returns an array holding the heading of the obstacle at previous timesteps.""" 71 | return self._prev_heading 72 | 73 | class CircularObstacle(BaseObstacle): 74 | def _setup(self, position, radius, color=(0.6, 0, 0)): 75 | self.color = color 76 | if not isinstance(position, np.ndarray): 77 | position = np.array(position) 78 | if radius < 0: 79 | raise ValueError 80 | self.static = True 81 | self.radius = radius 82 | self.position = position.flatten() 83 | 84 | ## Thomas 85 | self.dx = 0 86 | self.dy = 0 87 | 88 | def _calculate_boundary(self): 89 | return shapely.geometry.Point(*self.position).buffer(self.radius).boundary.simplify(0.3, preserve_topology=False) 90 | 91 | class PolygonObstacle(BaseObstacle): 92 | def _setup(self, points, color=(0.6, 0, 0)): 93 | self.static = True 94 | self.color = color 95 | self.points = points 96 | 97 | def _calculate_boundary(self): 98 | return shapely.geometry.Polygon(self.points) 99 | 100 | class LineObstacle(BaseObstacle): 101 | def _setup(self, points): 102 | self.static = True 103 | self.points = points 104 | 105 | def _calculate_boundary(self): 106 | return shapely.geometry.LineString(self.points) 107 | 108 | class VesselObstacle(BaseObstacle): 109 | def _setup(self, width, trajectory, init_position=None, init_heading=None, init_update=True, name=''): 110 | self.static = False 111 | self.width = width 112 | self.trajectory = trajectory 113 | self.trajectory_velocities = [] 114 | self.name = name 115 | i = 0 116 | while i < len(trajectory)-1: 117 | cur_t = trajectory[i][0] 118 | next_t = trajectory[i+1][0] 119 | cur_waypoint = trajectory[i][1] 120 | next_waypoint = trajectory[i+1][1] 121 | 122 | dx = (next_waypoint[0] - cur_waypoint[0])/(next_t - cur_t) 123 | dy = (next_waypoint[1] - cur_waypoint[1])/(next_t - cur_t) 124 | 125 | for _ in range(cur_t, next_t): 126 | self.trajectory_velocities.append((dx, dy)) 127 | 128 | i+= 1 129 | 130 | self.waypoint_counter = 0 131 | self.points = [ 132 | (-self.width/2, -self.width/2), 133 | (-self.width/2, self.width/2), 134 | (self.width/2, self.width/2), 135 | (3/2*self.width, 0), 136 | (self.width/2, -self.width/2), 137 | ] 138 | if init_position is not None: 139 | self.position = init_position 140 | else: 141 | self.position = np.array(self.trajectory[0][1]) 142 | self.init_position = self.position.copy() 143 | if init_heading is not None: 144 | self.heading = init_heading 145 | else: 146 | self.heading = np.pi/2 147 | 148 | if init_update: 149 | self.update(dt=0.1) 150 | 151 | def _update(self, dt): 152 | self.waypoint_counter += dt 153 | 154 | index = int(np.floor(self.waypoint_counter)) 155 | 156 | if index >= len(self.trajectory_velocities) - 1: 157 | self.waypoint_counter = 0 158 | index = 0 159 | self.position = np.array(self.trajectory[0][1]) 160 | 161 | dx = self.trajectory_velocities[index][0] 162 | dy = self.trajectory_velocities[index][1] 163 | 164 | self.dx = dt*dx 165 | self.dy = dt*dy 166 | self.heading = np.arctan2(self.dy, self.dx) 167 | self.position = self.position + np.array([self.dx, self.dy]) 168 | self._prev_position.append(self.position) 169 | self._prev_heading.append(self.heading) 170 | 171 | return True 172 | 173 | def _calculate_boundary(self): 174 | ship_angle = self.heading# float(geom.princip(self.heading)) 175 | 176 | boundary_temp = shapely.geometry.Polygon(self.points) 177 | boundary_temp = shapely.affinity.rotate(boundary_temp, ship_angle, use_radians=True, origin='centroid') 178 | boundary_temp = shapely.affinity.translate(boundary_temp, xoff=self.position[0], yoff=self.position[1]) 179 | 180 | return boundary_temp 181 | 182 | 183 | -------------------------------------------------------------------------------- /gym_auv/envs/movingobstacles.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import gym_auv.utils.geomutils as geom 4 | import gym_auv.utils.helpers as helpers 5 | from gym_auv.objects.vessel import Vessel 6 | from gym_auv.objects.path import RandomCurveThroughOrigin, Path 7 | from gym_auv.objects.obstacles import PolygonObstacle, VesselObstacle, CircularObstacle 8 | from gym_auv.environment import BaseEnvironment 9 | from gym_auv.objects.rewarder import ColregRewarder, ColavRewarder, ColavRewarder2, PathRewarder 10 | import shapely.geometry, shapely.errors 11 | 12 | import os 13 | dir_path = os.path.dirname(os.path.realpath(__file__)) 14 | 15 | vessel_speed_vals = np.loadtxt('resources/speed_vals.txt') 16 | vessel_speed_density = np.loadtxt('resources/speed_density.txt') 17 | 18 | class MovingObstacles(BaseEnvironment): 19 | 20 | def __init__(self, *args, **kwargs) -> None: 21 | """ 22 | Sets following parameters for the scenario before calling super init. method: 23 | self._n_moving_obst : Number of moving obstacles 24 | self._n_static_obst : Number of static obstacles 25 | self._rewarder_class : Rewarder used, e.g. PathRewarder, ColregRewarder 26 | """ 27 | 28 | super().__init__(*args, **kwargs) 29 | 30 | def _generate(self): 31 | # Initializing path 32 | if not hasattr(self, '_n_waypoints'): 33 | self._n_waypoints = int(np.floor(4*self.rng.rand() + 2)) 34 | 35 | self.path = RandomCurveThroughOrigin(self.rng, self._n_waypoints, length=800) 36 | 37 | # Initializing vessel 38 | init_state = self.path(0) 39 | init_angle = self.path.get_direction(0) 40 | init_state[0] += 50*(self.rng.rand()-0.5) 41 | init_state[1] += 50*(self.rng.rand()-0.5) 42 | init_angle = geom.princip(init_angle + 2*np.pi*(self.rng.rand()-0.5)) 43 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle]), width=self.config["vessel_width"]) 44 | prog = 0 45 | self.path_prog_hist = np.array([prog]) 46 | self.max_path_prog = prog 47 | 48 | self.obstacles = [] 49 | 50 | # Adding moving obstacles 51 | for _ in range(self._n_moving_obst): 52 | other_vessel_trajectory = [] 53 | 54 | obst_position, obst_radius = helpers.generate_obstacle(self.rng, self.path, self.vessel, obst_radius_mean=10, displacement_dist_std=500) 55 | obst_direction = self.rng.rand()*2*np.pi 56 | obst_speed = np.random.choice(vessel_speed_vals, p=vessel_speed_density) 57 | 58 | for i in range(10000): 59 | other_vessel_trajectory.append((i, ( 60 | obst_position[0] + i*obst_speed*np.cos(obst_direction), 61 | obst_position[1] + i*obst_speed*np.sin(obst_direction) 62 | ))) 63 | other_vessel_obstacle = VesselObstacle(width=obst_radius, trajectory=other_vessel_trajectory) 64 | 65 | self.obstacles.append(other_vessel_obstacle) 66 | 67 | # Adding static obstacles 68 | if not hasattr(self,'displacement_dist_std'): 69 | self.displacement_dist_std = 250 70 | 71 | for _ in range(self._n_static_obst): 72 | obstacle = CircularObstacle(*helpers.generate_obstacle(self.rng, self.path, self.vessel, displacement_dist_std=self.displacement_dist_std)) 73 | self.obstacles.append(obstacle) 74 | 75 | self._update() 76 | 77 | class MovingObstaclesNoRules(MovingObstacles): 78 | def __init__(self, *args, **kwargs): 79 | self._n_moving_obst = 17 80 | self._n_static_obst = 11 81 | self._rewarder_class = PathRewarder # ColavRewarder2 82 | super().__init__(*args, **kwargs) 83 | 84 | class MovingObstaclesColreg(MovingObstacles): 85 | def __init__(self, *args, **kwargs): 86 | self._n_moving_obst = 17 87 | self._n_static_obst = 11 88 | self._rewarder_class = ColregRewarder 89 | super().__init__(*args, **kwargs) 90 | 91 | 92 | ########################################### THOMAS' CUSTOM ENVS ######################################################## 93 | class Env0(MovingObstacles): 94 | ''' 95 | Complexity index: 1 (trivial case) 96 | Path is a straight line in a random direction. 97 | No static obstacles. 98 | No moving obstacles. 99 | ''' 100 | def __init__(self, *args, **kwargs): 101 | self.straight_path = True 102 | self._n_waypoints = -1 # less than 2 --> only start-/end-point --> straight path 103 | self._n_moving_obst = 0 104 | self._n_static_obst = 0 105 | self._rewarder_class = PathRewarder 106 | super().__init__(*args, **kwargs) 107 | 108 | 109 | class Env1(MovingObstacles): 110 | ''' 111 | Complexity index: 2 112 | Path is a simple random curve. 113 | No static obstacles. 114 | No moving obstacles. 115 | ''' 116 | def __init__(self, *args, **kwargs): 117 | self._n_waypoints = 2 # Curve complexity: more waypoints --> more turns. Remove to get random complexity. 118 | self._n_moving_obst = 0 119 | self._n_static_obst = 0 120 | self._rewarder_class = PathRewarder 121 | super().__init__(*args, **kwargs) 122 | 123 | 124 | class Env2(MovingObstacles): 125 | ''' 126 | Complexity index: 3 127 | Path is a straight line in a random direction. 128 | Some static obstacles. 129 | No moving obstacles. 130 | ''' 131 | def __init__(self, *args, **kwargs): 132 | self.straight_path = True 133 | self._n_waypoints = -1 # Curve complexity: more waypoints --> more turns. Remove to get random complexity. 134 | self._n_moving_obst = 0 135 | self._n_static_obst = 4 136 | self.displacement_dist_std = 0 # Object distance from path 137 | self._rewarder_class = PathRewarder # ColavRewarder 138 | super().__init__(*args, **kwargs) 139 | 140 | 141 | class Env3(MovingObstacles): 142 | ''' 143 | Complexity index: 4 144 | Path is a straight line in a random direction. 145 | Some static obstacles. 146 | Some moving obstacles. 147 | ''' 148 | def __init__(self, *args, **kwargs): 149 | self.straight_path = True 150 | self._n_waypoints = -1 # Curve complexity: more waypoints --> more turns. Remove to get random complexity. 151 | self._n_moving_obst = 17 152 | self._n_static_obst = 4 153 | self.displacement_dist_std = 0 # Object distance from path 154 | self._rewarder_class = PathRewarder 155 | super().__init__(*args, **kwargs) 156 | 157 | class Env4(MovingObstacles): 158 | ''' 159 | Complexity index: 5 (almost the same as MovingObstaclesNoRules) 160 | Path is a simple random curve. 161 | Some static obstacles. 162 | Some moving obstacles. 163 | ''' 164 | def __init__(self, *args, **kwargs): 165 | self._n_waypoints = 2 # Curve complexity: more waypoints --> more turns. Remove to get random complexity. 166 | self._n_moving_obst = 17 167 | self._n_static_obst = 4 168 | self.displacement_dist_std = 0 # Object distance from path 169 | self._rewarder_class = PathRewarder 170 | super().__init__(*args, **kwargs) -------------------------------------------------------------------------------- /gym_auv/__init__.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from gym.envs.registration import register 3 | 4 | def observe_obstacle_fun(t, dist): 5 | return t % (int(0.0025*dist**1.7) + 1) == 0 6 | 7 | def return_true_fun(t, dist): 8 | return True 9 | 10 | def sector_partition_fun(env, isensor, c=0.1): 11 | a = env.config["n_sensors_per_sector"]*env.config["n_sectors"] 12 | b = env.config["n_sectors"] 13 | sigma = lambda x: b / (1 + np.exp((-x + a / 2) / (c * a))) 14 | return int(np.floor(sigma(isensor) - sigma(0))) 15 | 16 | DEFAULT_CONFIG = { 17 | # ---- EPISODE ---- # 18 | "min_cumulative_reward": -2000, # Minimum cumulative reward received before episode ends 19 | "max_timesteps": 10000, # Maximum amount of timesteps before episode ends 20 | "min_goal_distance": 5, # Minimum aboslute distance to the goal position before episode ends 21 | "min_path_progress": 0.99, # Minimum path progress before scenario is considered successful and the episode ended 22 | 23 | # ---- SIMULATION ---- # 24 | "t_step_size": 1.0, # Length of simulation timestep [s] 25 | "sensor_frequency": 1.0, # Sensor execution frequency (0.0 = never execute, 1.0 = always execute) 26 | "observe_frequency": 1.0, # Frequency of using actual obstacles instead of virtual ones for detection 27 | 28 | # ---- VESSEL ---- # 29 | 'thrust_max_auv': 2.0, # Maximum thrust of the AUV [N] 30 | 'moment_max_auv': 0.15, # maximum moment applied to the AUV [Nm] 31 | "vessel_width": 1.255, # Width of vessel [m] 32 | "feasibility_width_multiplier": 5.0, # Multiplier for vessel width in feasibility pooling algorithm 33 | "look_ahead_distance": 150, # Path look-ahead distance for vessel [m] 34 | 'render_distance': 300, # 3D rendering render distance [m] 35 | "sensing": True, # Whether rangerfinder sensors for perception should be activated 36 | "sensor_interval_load_obstacles": 25, # Interval for loading nearby obstacles 37 | "n_sensors_per_sector": 20, # Number of rangefinder sensors within each sector 38 | "n_sectors": 9, # Number of sensor sectors 39 | "sector_partition_fun": sector_partition_fun, # Function that returns corresponding sector for a given sensor index 40 | "sensor_rotation": False, # Whether to activate the sectors in a rotating pattern (for performance reasons) 41 | "sensor_range": 150.0, # Range of rangefinder sensors [m] 42 | "sensor_log_transform": True, # Whether to use a log. transform when calculating closeness # 43 | "observe_obstacle_fun": observe_obstacle_fun, # Function that outputs whether an obstacle should be observed (True), 44 | # or if a virtual obstacle based on the latest reading should be used (False). 45 | # This represents a trade-off between sensor accuracy and computation speed. 46 | # With real-world terrain, using virtual obstacles is critical for performance. 47 | 48 | # ---- RENDERING ---- # 49 | "show_indicators": True, # Whether to show debug information on screen during 2d rendering. 50 | "autocamera3d": True # Whether to let the camera automatically rotate during 3d rendering 51 | } 52 | 53 | MOVING_CONFIG = DEFAULT_CONFIG.copy() 54 | MOVING_CONFIG["observe_obstacle_fun"] = return_true_fun 55 | 56 | DEBUG_CONFIG = DEFAULT_CONFIG.copy() 57 | DEBUG_CONFIG["t_step_size"] = 0.5 58 | DEBUG_CONFIG["min_goal_distance"] = 0.1 59 | 60 | REALWORLD_CONFIG = DEFAULT_CONFIG.copy() 61 | REALWORLD_CONFIG["t_step_size"] = 1.0 # 0.2 62 | REALWORLD_CONFIG["render_distance"] = 300#2000 63 | #REALWORLD_CONFIG["observe_frequency"] = 0.1 # HALVOR CREATED; THOMAS COMMENTED 64 | #REALWORLD_CONFIG["sensor_frequency"] = 0.5 # HALVOR CREATED; THOMAS COMMENTED 65 | 66 | SCENARIOS = { 67 | 'TestScenario0-v0': { 68 | 'entry_point': 'gym_auv.envs:TestScenario0', 69 | 'config': DEFAULT_CONFIG 70 | }, 71 | 'TestScenario1-v0': { 72 | 'entry_point': 'gym_auv.envs:TestScenario1', 73 | 'config': DEFAULT_CONFIG 74 | }, 75 | 'TestScenario2-v0': { 76 | 'entry_point': 'gym_auv.envs:TestScenario2', 77 | 'config': DEFAULT_CONFIG 78 | }, 79 | 'TestScenario3-v0': { 80 | 'entry_point': 'gym_auv.envs:TestScenario3', 81 | 'config': DEFAULT_CONFIG 82 | }, 83 | 'TestScenario4-v0': { 84 | 'entry_point': 'gym_auv.envs:TestScenario4', 85 | 'config': DEFAULT_CONFIG 86 | }, 87 | 'TestHeadOn-v0': { 88 | 'entry_point': 'gym_auv.envs:TestHeadOn', 89 | 'config': DEFAULT_CONFIG 90 | }, 91 | 'TestCrossing-v0': { 92 | 'entry_point': 'gym_auv.envs:TestCrossing', 93 | 'config': DEFAULT_CONFIG 94 | }, 95 | 'TestCrossing1-v0': { 96 | 'entry_point': 'gym_auv.envs:TestCrossing1', 97 | 'config': DEFAULT_CONFIG 98 | }, 99 | 'DebugScenario-v0': { 100 | 'entry_point': 'gym_auv.envs:DebugScenario', 101 | 'config': DEBUG_CONFIG 102 | }, 103 | 'EmptyScenario-v0': { 104 | 'entry_point': 'gym_auv.envs:EmptyScenario', 105 | 'config': DEBUG_CONFIG 106 | }, 107 | 'Sorbuoya-v0': { 108 | 'entry_point': 'gym_auv.envs:Sorbuoya', 109 | 'config': REALWORLD_CONFIG 110 | }, 111 | 'Agdenes-v0': { 112 | 'entry_point': 'gym_auv.envs:Agdenes', 113 | 'config': REALWORLD_CONFIG 114 | }, 115 | 'Trondheim-v0': { 116 | 'entry_point': 'gym_auv.envs:Trondheim', 117 | 'config': REALWORLD_CONFIG 118 | }, 119 | 'Trondheimsfjorden-v0': { 120 | 'entry_point': 'gym_auv.envs:Trondheimsfjorden', 121 | 'config': REALWORLD_CONFIG 122 | }, 123 | 'MovingObstaclesNoRules-v0': { 124 | 'entry_point': 'gym_auv.envs:MovingObstaclesNoRules', 125 | 'config': MOVING_CONFIG 126 | }, 127 | 'MovingObstaclesColreg-v0': { 128 | 'entry_point': 'gym_auv.envs:MovingObstaclesColreg', 129 | 'config': MOVING_CONFIG 130 | }, 131 | 'FilmScenario-v0': { 132 | 'entry_point': 'gym_auv.envs:FilmScenario', 133 | 'config': REALWORLD_CONFIG 134 | }, 135 | ##### THOMAS' CUSTOM ENVS ##### 136 | 'Env0-v0': { 137 | 'entry_point': 'gym_auv.envs:Env0', 138 | 'config': MOVING_CONFIG 139 | }, 140 | 'Env1-v0': { 141 | 'entry_point': 'gym_auv.envs:Env1', 142 | 'config': MOVING_CONFIG 143 | }, 144 | 'Env2-v0': { 145 | 'entry_point': 'gym_auv.envs:Env2', 146 | 'config': MOVING_CONFIG 147 | }, 148 | 'Env3-v0': { 149 | 'entry_point': 'gym_auv.envs:Env3', 150 | 'config': MOVING_CONFIG 151 | }, 152 | 'Env4-v0': { 153 | 'entry_point': 'gym_auv.envs:Env4', 154 | 'config': MOVING_CONFIG 155 | }, 156 | } 157 | 158 | for scenario in SCENARIOS: 159 | register( 160 | id=scenario, 161 | entry_point=SCENARIOS[scenario]['entry_point'], 162 | #kwargs={'env_config': SCENARIOS[scenario]['config']} 163 | ) -------------------------------------------------------------------------------- /gym_auv/envs/testscenario.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import random 3 | import math 4 | 5 | import gym_auv.utils.geomutils as geom 6 | from gym_auv.objects.vessel import Vessel 7 | from gym_auv.objects.path import RandomCurveThroughOrigin, Path 8 | from gym_auv.objects.obstacles import CircularObstacle, VesselObstacle 9 | from gym_auv.environment import BaseEnvironment 10 | 11 | import os 12 | dir_path = os.path.dirname(os.path.realpath(__file__)) 13 | 14 | TERRAIN_DATA_PATH = 'resources/terrain.npy' 15 | 16 | deg2rad = math.pi/180 17 | 18 | class TestScenario0(BaseEnvironment): 19 | def _generate(self): 20 | self.path = Path([[0, 0, 50, 50], [0, 500, 600, 1000]]) 21 | 22 | init_state = self.path(0) 23 | init_angle = self.path.get_direction(0) 24 | 25 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) 26 | prog = self.path.get_closest_arclength(self.vessel.position) 27 | self.path_prog_hist = np.array([prog]) 28 | self.max_path_prog = prog 29 | 30 | obst_arclength = 50 31 | for o in range(9): 32 | obst_radius = 20 33 | obst_arclength += obst_radius*2 + 170 34 | obst_position = self.path(obst_arclength) 35 | 36 | obst_displacement = np.array([obst_radius*(-1)**(o+1), obst_radius]) 37 | self.obstacles.append(CircularObstacle(obst_position + obst_displacement, obst_radius)) 38 | 39 | 40 | 41 | class TestScenario1(BaseEnvironment): 42 | def _generate(self): 43 | self.path = Path([[0, 1100], [0, 1100]]) 44 | 45 | init_state = self.path(0) 46 | init_angle = self.path.get_direction(0) 47 | 48 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) 49 | prog = self.path.get_closest_arclength(self.vessel.position) 50 | self.path_prog_hist = np.array([prog]) 51 | self.max_path_prog = prog 52 | 53 | ##self.obstacles = [] # Thomas bugfix maybe? If _generate is called many times, self.obstacles may become huge. 54 | 55 | obst_arclength = 30 56 | for o in range(20): 57 | obst_radius = 10 + 10*o**1.5 58 | obst_arclength += obst_radius*2 + 30 59 | obst_position = self.path(obst_arclength) 60 | self.obstacles.append(CircularObstacle(obst_position, obst_radius)) 61 | 62 | 63 | class TestScenario2(BaseEnvironment): 64 | def _generate(self): 65 | 66 | waypoint_array = [] 67 | for t in range(500): 68 | x = t*np.cos(t/100) 69 | y = 2*t 70 | waypoint_array.append([x, y]) 71 | 72 | waypoints = np.vstack(waypoint_array).T 73 | self.path = Path(waypoints) 74 | 75 | init_state = self.path(0) 76 | init_angle = self.path.get_direction(0) 77 | 78 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) 79 | prog = self.path.get_closest_arclength(self.vessel.position) 80 | self.path_prog_hist = np.array([prog]) 81 | self.max_path_prog = prog 82 | 83 | obst_arclength = 30 84 | obst_radius = 5 85 | while True: 86 | obst_arclength += 2*obst_radius 87 | if (obst_arclength >= self.path.length): 88 | break 89 | 90 | obst_displacement_dist = 140 - 120 / (1 + np.exp(-0.005*obst_arclength)) 91 | 92 | obst_position = self.path(obst_arclength) 93 | obst_displacement_angle = self.path.get_direction(obst_arclength) - np.pi/2 94 | obst_displacement = obst_displacement_dist*np.array([ 95 | np.cos(obst_displacement_angle), 96 | np.sin(obst_displacement_angle) 97 | ]) 98 | 99 | self.obstacles.append(CircularObstacle(obst_position + obst_displacement, obst_radius)) 100 | self.obstacles.append(CircularObstacle(obst_position - obst_displacement, obst_radius)) 101 | 102 | class TestScenario3(BaseEnvironment): 103 | def _generate(self): 104 | waypoints = np.vstack([[0, 0], [0, 500]]).T 105 | self.path = Path(waypoints) 106 | 107 | init_state = self.path(0) 108 | init_angle = self.path.get_direction(0) 109 | 110 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) 111 | prog = self.path.get_closest_arclength(self.vessel.position) 112 | self.path_prog_hist = np.array([prog]) 113 | self.max_path_prog = prog 114 | 115 | N_obst = 20 116 | N_dist = 100 117 | for n in range(N_obst + 1): 118 | obst_radius = 25 119 | angle = np.pi/4 + n/N_obst * np.pi/2 120 | obst_position = np.array([np.cos(angle)*N_dist, np.sin(angle)*N_dist]) 121 | self.obstacles.append(CircularObstacle(obst_position, obst_radius)) 122 | 123 | class TestScenario4(BaseEnvironment): 124 | def _generate(self): 125 | waypoints = np.vstack([[0, 0], [0, 500]]).T 126 | self.path = Path(waypoints) 127 | 128 | init_state = self.path(0) 129 | init_angle = self.path.get_direction(0) 130 | 131 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) 132 | prog = self.path.get_closest_arclength(self.vessel.position) 133 | self.path_prog_hist = np.array([prog]) 134 | self.max_path_prog = prog 135 | 136 | N_obst = 20 137 | N_dist = 100 138 | for n in range(N_obst+1): 139 | obst_radius = 25 140 | angle = n/N_obst * 2*np.pi 141 | if (abs(angle < 3/2*np.pi) < np.pi/12): 142 | continue 143 | obst_position = np.array([np.cos(angle)*N_dist, np.sin(angle)*N_dist]) 144 | self.obstacles.append(CircularObstacle(obst_position, obst_radius)) 145 | 146 | class TestHeadOn(BaseEnvironment): 147 | def _generate(self): 148 | 149 | waypoints = np.vstack([[0, 0], [0, 250]]).T 150 | self.path = Path(waypoints) 151 | 152 | init_state = self.path(0) 153 | init_angle = self.path.get_direction(0) 154 | 155 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) 156 | prog = self.path.get_closest_arclength(self.vessel.position) 157 | self.path_prog_hist = np.array([prog]) 158 | self.max_path_prog = prog 159 | vessel_pos = self.vessel.position 160 | 161 | start_angle = random.uniform(-5*deg2rad, 5*deg2rad) 162 | trajectory_shift = 5*deg2rad #random.uniform(-5*deg2rad+start_angle, 5*deg2rad+start_angle) #2*np.pi*(rng.rand() - 0.5) 163 | trajectory_radius = 150 164 | trajectory_speed = 0.5 165 | start_x = vessel_pos[0] + trajectory_radius*np.sin(start_angle) 166 | start_y = vessel_pos[1] + trajectory_radius*np.cos(start_angle) 167 | 168 | vessel_trajectory = [[0, (start_x, start_y)]] 169 | 170 | 171 | for i in range(1,5000): 172 | vessel_trajectory.append((1*i, ( 173 | start_x - trajectory_speed*np.sin(start_angle)*i, 174 | start_y - trajectory_speed*np.cos(start_angle)*i 175 | ))) 176 | 177 | self.obstacles = [VesselObstacle(width=30, trajectory=vessel_trajectory)] 178 | 179 | self._update() 180 | 181 | class TestCrossing(BaseEnvironment): 182 | def _generate(self): 183 | 184 | waypoints = np.vstack([[0, 0], [0, 500]]).T 185 | self.path = Path(waypoints) 186 | 187 | init_state = self.path(0) 188 | init_angle = self.path.get_direction(0) 189 | 190 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) 191 | prog = self.path.get_closest_arclength(self.vessel.position) 192 | self.path_prog_hist = np.array([prog]) 193 | self.max_path_prog = prog 194 | vessel_pos = self.vessel.position 195 | 196 | trajectory_shift = 90*deg2rad #random.uniform(-5*deg2rad, 5*deg2rad) #2*np.pi*(rng.rand() - 0.5) 197 | trajectory_radius = 200 198 | trajectory_speed = 0.5 199 | start_angle = -45*deg2rad 200 | start_x = vessel_pos[0] + trajectory_radius*np.sin(start_angle) 201 | start_y = vessel_pos[1] + trajectory_radius*np.cos(start_angle) 202 | 203 | # vessel_trajectory = [[0, (vessel_pos[1], trajectory_radius+vessel_pos[0])]] # in front, ahead 204 | vessel_trajectory = [[0, (start_x,start_y)]] 205 | 206 | for i in range(1,5000): 207 | vessel_trajectory.append((1*i, ( 208 | start_x + trajectory_speed*np.sin(trajectory_shift)*i, 209 | start_y + trajectory_speed*np.cos(trajectory_shift)*i 210 | ))) 211 | 212 | self.obstacles = [VesselObstacle(width=30, trajectory=vessel_trajectory)] 213 | 214 | self._update() 215 | 216 | class TestCrossing1(BaseEnvironment): 217 | def _generate(self): 218 | 219 | waypoints = np.vstack([[0, 0], [0, 500]]).T 220 | self.path = Path(waypoints) 221 | 222 | init_state = self.path(0) 223 | init_angle = self.path.get_direction(0) 224 | 225 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) 226 | prog = self.path.get_closest_arclength(self.vessel.position) 227 | self.path_prog_hist = np.array([prog]) 228 | self.max_path_prog = prog 229 | vessel_pos = self.vessel.position 230 | 231 | trajectory_shift = -50*deg2rad #random.uniform(-5*deg2rad, 5*deg2rad) #2*np.pi*(rng.rand() - 0.5) 232 | trajectory_radius = 200 233 | trajectory_speed = 0.5 234 | start_angle = 70*deg2rad 235 | start_x = vessel_pos[0] + trajectory_radius*np.sin(start_angle) 236 | start_y = vessel_pos[1] + trajectory_radius*np.cos(start_angle) 237 | 238 | # vessel_trajectory = [[0, (vessel_pos[1], trajectory_radius+vessel_pos[0])]] # in front, ahead 239 | vessel_trajectory = [[0, (start_x,start_y)]] 240 | 241 | for i in range(1,5000): 242 | vessel_trajectory.append((1*i, ( 243 | start_x + trajectory_speed*np.sin(trajectory_shift)*i, 244 | start_y + trajectory_speed*np.cos(trajectory_shift)*i 245 | ))) 246 | 247 | self.obstacles = [VesselObstacle(width=30, trajectory=vessel_trajectory)] 248 | 249 | self._update() 250 | 251 | class EmptyScenario(BaseEnvironment): 252 | 253 | def _generate(self): 254 | waypoints = np.vstack([[25, 10], [25, 200]]).T 255 | self.path = Path(waypoints) 256 | 257 | init_state = self.path(0) 258 | init_angle = self.path.get_direction(0) 259 | 260 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle]), width=self.config["vessel_width"]) 261 | prog = self.path.get_closest_arclength(self.vessel.position) 262 | self.path_prog_hist = np.array([prog]) 263 | self.max_path_prog = prog 264 | 265 | if self.render_mode == '3d': 266 | self.all_terrain = np.zeros((50, 50), dtype=float) 267 | self._viewer3d.create_world(self.all_terrain, 0, 0, 50, 50) 268 | 269 | class DebugScenario(BaseEnvironment): 270 | def _generate(self): 271 | waypoints = np.vstack([[250, 100], [250, 200]]).T 272 | self.path = Path(waypoints) 273 | 274 | init_state = self.path(0) 275 | init_angle = self.path.get_direction(0) 276 | 277 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle]), width=self.config["vessel_width"]) 278 | prog = self.path.get_closest_arclength(self.vessel.position) 279 | self.path_prog_hist = np.array([prog]) 280 | self.max_path_prog = prog 281 | 282 | self.obstacles = [] 283 | self.vessel_obstacles = [] 284 | 285 | for vessel_idx in range(5): 286 | other_vessel_trajectory = [] 287 | trajectory_shift = self.rng.rand()*2*np.pi 288 | trajectory_radius = self.rng.rand()*40 + 30 289 | trajectory_speed = self.rng.rand()*0.003 + 0.003 290 | for i in range(10000): 291 | #other_vessel_trajectory.append((10*i, (250, 400-10*i))) 292 | other_vessel_trajectory.append((1*i, ( 293 | 250 + trajectory_radius*np.cos(trajectory_speed*i + trajectory_shift), 294 | 150 + 70*vessel_idx + trajectory_radius*np.sin(trajectory_speed*i + trajectory_shift) 295 | ))) 296 | other_vessel_obstacle = VesselObstacle(width=6, trajectory=other_vessel_trajectory) 297 | 298 | self.obstacles.append(other_vessel_obstacle) 299 | self.vessel_obstacles.append(other_vessel_obstacle) 300 | 301 | for vessel_idx in range(5): 302 | other_vessel_trajectory = [] 303 | trajectory_start = self.rng.rand()*200 + 150 304 | trajectory_speed = self.rng.rand()*0.03 + 0.03 305 | trajectory_shift = 10*self.rng.rand() 306 | for i in range(10000): 307 | other_vessel_trajectory.append((i, (245 + 2.5*vessel_idx + trajectory_shift, trajectory_start-10*trajectory_speed*i))) 308 | other_vessel_obstacle = VesselObstacle(width=6, trajectory=other_vessel_trajectory) 309 | 310 | self.obstacles.append(other_vessel_obstacle) 311 | self.vessel_obstacles.append(other_vessel_obstacle) 312 | 313 | if self.render_mode == '3d': 314 | self.all_terrain = np.load(TERRAIN_DATA_PATH)[1950:2450, 5320:5820]/7.5 315 | #terrain = np.zeros((500, 500), dtype=float) 316 | 317 | # for x in range(10, 40): 318 | # for y in range(10, 40): 319 | # z = 0.5*np.sqrt(max(0, 15**2 - (25.0-x)**2 - (25.0-y)**2)) 320 | # terrain[x][y] = z 321 | self._viewer3d.create_world(self.all_terrain, 0, 0, 500, 500) -------------------------------------------------------------------------------- /gym_auv/envs/realworld.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pandas as pd 3 | 4 | import gym_auv.utils.geomutils as geom 5 | from gym_auv.objects.vessel import Vessel 6 | from gym_auv.objects.path import RandomCurveThroughOrigin, Path 7 | from gym_auv.objects.obstacles import PolygonObstacle, VesselObstacle 8 | from gym_auv.objects.rewarder import ColavRewarder, ColregRewarder, PathRewarder 9 | from gym_auv.environment import BaseEnvironment 10 | import shapely.geometry, shapely.errors 11 | 12 | import os 13 | dir_path = os.path.dirname(os.path.realpath(__file__)) 14 | 15 | UPDATE_WAIT = 100 16 | TERRAIN_DATA_PATH = 'resources/terrain.npy' 17 | INCLUDED_VESSELS = None 18 | 19 | VESSEL_SPEED_RANGE_LOWER = 0.1 20 | VESSEL_SPEED_RANGE_UPPER = 2 21 | 22 | class RealWorldEnv(BaseEnvironment): 23 | 24 | def __init__(self, *args, **kw): 25 | self.last_scenario_load_coordinates = None 26 | self.all_terrain = None 27 | 28 | super().__init__(*args, **kw) 29 | 30 | def _generate(self): 31 | 32 | vessel_trajectories = [] 33 | if self.vessel_data_path is not None: 34 | df = pd.read_csv(self.vessel_data_path) 35 | vessels = dict(tuple(df.groupby('Vessel_Name'))) 36 | vessel_names = sorted(list(vessels.keys())) 37 | 38 | #print('Preprocessing traffic...') 39 | while len(vessel_trajectories) < self.n_vessels: 40 | if len(vessel_names) == 0: 41 | break 42 | vessel_idx = self.rng.randint(0, len(vessel_names)) 43 | vessel_name = vessel_names.pop(vessel_idx) 44 | 45 | vessels[vessel_name]['AIS_Timestamp'] = pd.to_datetime(vessels[vessel_name]['AIS_Timestamp']) 46 | vessels[vessel_name]['AIS_Timestamp'] -= vessels[vessel_name].iloc[0]['AIS_Timestamp'] 47 | start_timestamp = None 48 | 49 | last_timestamp = pd.to_timedelta(0, unit='D') 50 | last_east = None 51 | last_north = None 52 | cutoff_dt = pd.to_timedelta(0.1, unit='D') 53 | path = [] 54 | for _, row in vessels[vessel_name].iterrows(): 55 | east = row['AIS_East']/10.0 56 | north = row['AIS_North']/10.0 57 | if row['AIS_Length_Overall'] < 12: 58 | continue 59 | if len(path) == 0: 60 | start_timestamp = row['AIS_Timestamp'] 61 | timedelta = row['AIS_Timestamp'] - last_timestamp 62 | if timedelta < cutoff_dt: 63 | if last_east is not None: 64 | dx = east - last_east 65 | dy = north - last_north 66 | distance = np.sqrt(dx**2 + dy**2) 67 | seconds = timedelta.seconds 68 | speed = distance/seconds 69 | if speed < VESSEL_SPEED_RANGE_LOWER or speed > VESSEL_SPEED_RANGE_UPPER: 70 | path = [] 71 | continue 72 | 73 | path.append((int((row['AIS_Timestamp']-start_timestamp).total_seconds()), (east-self.x0, north-self.y0))) 74 | else: 75 | if len(path) > 1 and not np.isnan(row['AIS_Length_Overall']) and row['AIS_Length_Overall'] > 0: 76 | start_index = self.rng.randint(0, len(path)-1) 77 | vessel_trajectories.append((row['AIS_Length_Overall']/10.0, path[start_index:], vessel_name)) 78 | path = [] 79 | last_timestamp = row['AIS_Timestamp'] 80 | last_east = east 81 | last_north = north 82 | 83 | #if self.other_vessels: 84 | # print(vessel_name, path[0], len(path)) 85 | 86 | #print('Completed traffic preprocessing') 87 | 88 | other_vessel_indeces = self.rng.choice(list(range(len(vessel_trajectories))), min(len(vessel_trajectories), self.n_vessels), replace=False) 89 | self.other_vessels = [vessel_trajectories[idx] for idx in other_vessel_indeces] 90 | 91 | init_state = self.path(0) 92 | init_angle = self.path.get_direction(0) 93 | 94 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle]), width=self.config["vessel_width"]) 95 | prog = self.path.get_closest_arclength(self.vessel.position) 96 | self.path_prog_hist = np.array([prog]) 97 | self.max_path_prog = prog 98 | 99 | self.all_obstacles = [] 100 | self.obstacles = [] 101 | if self.obstacle_perimeters is not None: 102 | for obstacle_perimeter in self.obstacle_perimeters: 103 | if len(obstacle_perimeter) > 3: 104 | obstacle = PolygonObstacle(obstacle_perimeter) 105 | assert obstacle.boundary.is_valid, 'The added obstacle is invalid!' 106 | self.all_obstacles.append(obstacle) 107 | self.obstacles.append(obstacle) 108 | 109 | if self.verbose: print('Added {} obstacles'.format(len(self.obstacles))) 110 | 111 | if self.verbose: print('Generating {} vessel trajectories'.format(len(self.other_vessels))) 112 | for vessel_width, vessel_trajectory, vessel_name in self.other_vessels: 113 | # for k in range(0, len(vessel_trajectory)-1): 114 | # vessel_obstacle = VesselObstacle(width=int(vessel_width), trajectory=vessel_trajectory[k:]) 115 | # self.all_obstacles.append(vessel_obstacle) 116 | if len(vessel_trajectory) > 2: 117 | vessel_obstacle = VesselObstacle(width=int(vessel_width), trajectory=vessel_trajectory, name=vessel_name) 118 | self.all_obstacles.append(vessel_obstacle) 119 | self.obstacles.append(vessel_obstacle) 120 | 121 | # if self.render_mode == '3d': 122 | # if self.verbose: 123 | # print('Loading nearby 3D terrain...') 124 | # xlow = 0 125 | # xhigh = self.all_terrain.shape[0] 126 | # ylow = 0 127 | # yhigh = self.all_terrain.shape[1] 128 | # self._viewer3d.create_world(self.all_terrain, xlow, ylow, xhigh, yhigh) 129 | # if self.verbose: 130 | # print('Loaded nearby 3D terrain ({}-{}, {}-{})'.format(xlow, xhigh, ylow, yhigh)) 131 | 132 | self._update() 133 | 134 | def _update(self, force=False): 135 | if self.render_mode == '3d': 136 | if self.t_step % UPDATE_WAIT == 0 or force: 137 | travelled_distance = np.linalg.norm(self.vessel.position - self.last_scenario_load_coordinates) if self.last_scenario_load_coordinates is not None else np.inf 138 | if travelled_distance > self.render_distance/10: 139 | if self.verbose: 140 | print('Update scheduled with distance travelled {:.2f}.'.format(travelled_distance)) 141 | 142 | if self.verbose: 143 | print('Loading nearby terrain...'.format(len(self.obstacles))) 144 | vessel_center = shapely.geometry.Point( 145 | self.vessel.position[0], 146 | self.vessel.position[1], 147 | ) 148 | self.obstacles = [] 149 | for obstacle in self.all_obstacles: 150 | obst_dist = float(vessel_center.distance(obstacle.boundary)) - self.vessel.width 151 | if obst_dist <= self.render_distance: 152 | self.obstacles.append(obstacle) 153 | else: 154 | if not obstacle.static: 155 | obstacle.update(UPDATE_WAIT*self.config["t_step_size"]) 156 | 157 | if self.verbose: 158 | print('Loaded nearby terrain ({} obstacles).'.format(len(self.obstacles))) 159 | 160 | if self.verbose: 161 | print('Loading nearby 3D terrain...') 162 | x = int(self.vessel.position[0] + self.x0) 163 | y = int(self.vessel.position[1] + self.y0) 164 | xlow = max(0, x-self.render_distance) 165 | xhigh = min(self.all_terrain.shape[0], x+self.render_distance) 166 | ylow = max(0, y-self.render_distance) 167 | yhigh = min(self.all_terrain.shape[1], y+self.render_distance) 168 | self._viewer3d.create_world(self.all_terrain, xlow, ylow, xhigh, yhigh, self.x0, self.y0) 169 | if self.verbose: 170 | print('Loaded nearby 3D terrain ({}-{}, {}-{})'.format(xlow, xhigh, ylow, yhigh)) 171 | 172 | self.last_scenario_load_coordinates = self.vessel.position 173 | 174 | super()._update() 175 | 176 | class Sorbuoya(RealWorldEnv): 177 | def __init__(self, *args, **kw): 178 | self.x0, self.y0 = 0, 10000 179 | self.vessel_data_path = 'resources/vessel_data_local_sorbuoya.csv' 180 | self.n_vessels = 25 181 | super().__init__(*args, **kw) 182 | 183 | def _generate(self): 184 | #self.path = Path([[-50, 1750], [250, 1200]]) 185 | #self.path = Path([[650, 1750], [450, 1200]]) 186 | self.path = Path([[1000, 830, 700, 960, 1080, 1125], [910, 800, 700, 550, 750, 810]]) 187 | self.obstacle_perimeters = np.load('resources/obstacles_sorbuoya.npy') 188 | self.all_terrain = np.load(TERRAIN_DATA_PATH)/7.5 #np.load(TERRAIN_DATA_PATH)[0000:2000, 10000:12000]/7.5 189 | super()._generate() 190 | 191 | class Agdenes(RealWorldEnv): 192 | def __init__(self, *args, **kw): 193 | self.x0, self.y0 = 3121, 5890 194 | self.vessel_data_path = 'resources/vessel_data_local_agdenes.csv' 195 | self.n_vessels = 15 196 | super().__init__(*args, **kw) 197 | 198 | def _generate(self): 199 | #self.path = Path([[520, 1070, 4080, 5473, 10170, 12220], [3330, 5740, 7110, 4560, 7360, 11390]]) #South-west -> north-east 200 | self.path = Path([[4100-self.x0, 4247-self.x0, 4137-self.x0, 3937-self.x0, 3217-self.x0], [6100-self.y0, 6100-self.y0, 6860-self.y0, 6910-self.y0, 6690-self.y0]]) 201 | self.obstacle_perimeters = np.load('resources/obstacles_entrance.npy') 202 | self.all_terrain = np.load(TERRAIN_DATA_PATH)/7.5 #[3121:4521, 5890:7390]/7.5 203 | 204 | super()._generate() 205 | 206 | class Trondheim(RealWorldEnv): 207 | def __init__(self, *args, **kw): 208 | self.x0, self.y0 = 5000,3900 209 | self.vessel_data_path = 'resources/vessel_data_local_trondheim.csv' 210 | self.n_vessels = 100 211 | super().__init__(*args, **kw) 212 | 213 | def _generate(self): 214 | self.path = Path([[6945-self.x0, 6329-self.x0], [4254-self.y0, 5614-self.y0]]) 215 | self.obstacle_perimeters = np.load('resources/obstacles_trondheim.npy') 216 | self.all_terrain = np.load(TERRAIN_DATA_PATH)[self.x0:8000, self.y0:6900]/7.5 217 | super()._generate() 218 | 219 | class Trondheimsfjorden(RealWorldEnv): 220 | def __init__(self, *args, **kw): 221 | self.x0, self.y0 = 0, 0 222 | self.vessel_data_path = 'resources/vessel_data.csv' 223 | self.n_vessels = 999999 224 | super().__init__(*args, **kw) 225 | 226 | def _generate(self): 227 | self.path = Path([[520, 1070, 4080, 5473, 10170, 12220], [3330, 5740, 7110, 4560, 7360, 11390]]) #South-west -> north-east 228 | self.obstacle_perimeters = np.load('resources/obstacles_trondheimsfjorden.npy') 229 | self.all_terrain = np.load(TERRAIN_DATA_PATH)/7.5 #[3121:4521, 5890:7390]/7.5 230 | 231 | super()._generate() 232 | 233 | class FilmScenario(RealWorldEnv): 234 | def __init__(self, *args, **kw): 235 | self.x0, self.y0 = 0, 0 236 | self.vessel_data_path = None 237 | self._rewarder_class = ColregRewarder 238 | self.n_vessels = 999999 239 | super().__init__(*args, **kw) 240 | 241 | def _generate(self): 242 | print('Generating') 243 | 244 | self.obstacle_perimeters = None 245 | self.all_terrain = np.load(TERRAIN_DATA_PATH)/7.5 246 | path_length = 1.2*(100 + self.rng.randint(400)) 247 | 248 | while 1: 249 | x0 = self.rng.randint(1000, self.all_terrain.shape[0]-1000) 250 | y0 = self.rng.randint(1000, self.all_terrain.shape[1]-1000) 251 | dir = self.rng.rand()*2*np.pi 252 | waypoints = [[x0, x0+path_length*np.cos(dir)], [y0, y0+path_length*np.sin(dir)]] 253 | close_proximity = self.all_terrain[x0-50:x0+50, y0-50:y0+50] 254 | path_center = [x0+path_length/2*np.cos(dir), y0+path_length/2*np.sin(dir)] 255 | path_end = [x0+path_length*np.cos(dir), y0+path_length*np.sin(dir)] 256 | proximity = self.all_terrain[x0-250:x0+250, y0-250:y0+250] 257 | 258 | if proximity.max() > 0 and close_proximity.max() == 0: 259 | break 260 | 261 | self.path = Path(waypoints) 262 | 263 | init_state = self.path(0) 264 | init_angle = self.path.get_direction(0) 265 | 266 | self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) 267 | self.rewarder = ColregRewarder(self.vessel, test_mode=True) 268 | self._rewarder_class = ColregRewarder 269 | prog = self.path.get_closest_arclength(self.vessel.position) 270 | self.path_prog_hist = np.array([prog]) 271 | self.max_path_prog = prog 272 | 273 | self.obstacles, self.all_obstacles = [], [] 274 | for i in range(1): 275 | trajectory_speed = 0.4 + 0.2*self.rng.rand() 276 | start_x = path_end[0] 277 | start_y = path_end[1] 278 | vessel_trajectory = [[0, (start_x,start_y)]] 279 | for t in range(1,10000): 280 | vessel_trajectory.append((1*t, ( 281 | start_x - trajectory_speed*np.cos(dir)*t, 282 | start_y - trajectory_speed*np.sin(dir)*t 283 | ))) 284 | vessel_obstacle = VesselObstacle(width=10, trajectory=vessel_trajectory) 285 | 286 | self.obstacles.append(vessel_obstacle) 287 | self.all_obstacles.append(vessel_obstacle) 288 | 289 | print('Updating') 290 | self._update(force=True) -------------------------------------------------------------------------------- /gym_auv/utils/radarCNN.py: -------------------------------------------------------------------------------- 1 | import gym 2 | import torch as th 3 | import torch.nn as nn 4 | 5 | from stable_baselines3 import PPO 6 | from stable_baselines3.common.torch_layers import BaseFeaturesExtractor 7 | 8 | 9 | class RadarCNN(BaseFeaturesExtractor): 10 | """ 11 | :param observation_space: (gym.Space) of dimension (N_sensors x 1) 12 | :param features_dim: (int) Number of features extracted. 13 | This corresponds to the number of unit for the last layer. 14 | """ 15 | 16 | #def __init__(self, observation_space: gym.spaces.Box, sensor_dim: int = 180, features_dim: int = 32, kernel_overlap: float = 0.05): 17 | def __init__(self, observation_space: gym.spaces.Box, sensor_dim : int = 180, features_dim: int = 12, kernel_overlap : float = 0.25): 18 | super(RadarCNN, self).__init__(observation_space, features_dim=features_dim) 19 | # We assume CxHxW images (channels first) 20 | # Re-ordering will be done by pre-preprocessing or wrapper 21 | 22 | # Adjust kernel size for sensor density. (Default 180 sensors with 0.05 overlap --> kernel covers 9 sensors. 23 | self.in_channels = observation_space.shape[0] # 180 24 | self.kernel_size = round(sensor_dim * kernel_overlap) # 45 25 | self.kernel_size = self.kernel_size + 1 if self.kernel_size % 2 == 0 else self.kernel_size # Make it odd sized 26 | #self.padding = (self.kernel_size - 1) // 2 # 22 27 | self.padding = self.kernel_size // 3 # 15 28 | self.stride = self.padding 29 | #print("RADAR_CNN CONFIG") 30 | #print("\tIN_CHANNELS =", self.in_channels) 31 | #print("\tKERNEL_SIZE =", self.kernel_size) 32 | #print("\tPADDING =", self.padding) 33 | #print("\tSTRIDE =", self.stride) 34 | self.cnn = nn.Sequential( 35 | # in_channels: sensor distance, obst_velocity_x, obst_velocity_y 36 | nn.Conv1d(in_channels=self.in_channels, out_channels=1, kernel_size=self.kernel_size, padding=self.padding, 37 | padding_mode='circular', stride=self.stride), 38 | nn.Flatten(), 39 | ) 40 | 41 | # Compute shape by doing one forward pass 42 | self.n_flatten = 0 43 | sample = th.as_tensor(observation_space.sample()).float() 44 | print("Observation space - sample shape:", sample.shape) 45 | sample = sample.reshape(1, sample.shape[0], sample.shape[1]) 46 | with th.no_grad(): 47 | print("RadarCNN initializing, CNN input is", sample.shape, "and", end=" ") 48 | flatten = self.cnn(sample) 49 | self.n_flatten = flatten.shape[1] 50 | print("output is", flatten.shape) 51 | 52 | self.linear = nn.Sequential(nn.Linear(self.n_flatten, features_dim), nn.ReLU()) 53 | 54 | def forward(self, observations: th.Tensor) -> th.Tensor: 55 | return self.cnn(observations) 56 | #return self.linear(self.cnn(observations)) 57 | #return self.cnn(observations) 58 | 59 | def get_features(self, observations: th.Tensor) -> list: 60 | feat = [] 61 | out = observations 62 | for layer in self.cnn: 63 | out = layer(out) 64 | if not isinstance(layer, nn.ReLU): 65 | feat.append(out.cpu().detach().numpy()) 66 | 67 | #for layer in self.linear: 68 | # out = layer(out) 69 | # if not isinstance(layer, nn.ReLU): 70 | # feat.append(out.cpu().detach().numpy()) 71 | 72 | return feat 73 | 74 | def get_activations(self, observations: th.Tensor) -> list: 75 | feat = [] 76 | out = observations 77 | for layer in self.cnn: 78 | out = layer(out) 79 | if isinstance(layer, nn.ReLU): 80 | feat.append(out) 81 | 82 | for layer in self.linear: 83 | out = layer(out) 84 | if isinstance(layer, nn.ReLU): 85 | feat.append(out.detach().numpy()) 86 | 87 | return feat 88 | 89 | class NavigatioNN(BaseFeaturesExtractor): 90 | def __init__(self, observation_space: gym.spaces.Box, features_dim: int = 6): 91 | super(NavigatioNN, self).__init__(observation_space, features_dim=features_dim) 92 | 93 | self.passthrough = nn.Identity() 94 | 95 | def forward(self, observations: th.Tensor) -> th.Tensor: 96 | shape = observations.shape 97 | observations = observations[:,0,:].reshape(shape[0], shape[-1]) 98 | return self.passthrough(observations) 99 | 100 | class PerceptionNavigationExtractor(BaseFeaturesExtractor): 101 | """ 102 | :param observation_space: (gym.Space) of dimension (1, 3, N_sensors) 103 | :param features_dim: (int) Number of features extracted. 104 | This corresponds to the number of unit for the last layer. 105 | """ 106 | 107 | #def __init__(self, observation_space: gym.spaces.Dict, sensor_dim : int = 180, features_dim: int = 32, kernel_overlap : float = 0.05): 108 | def __init__(self, observation_space: gym.spaces.Dict, sensor_dim: int = 180, features_dim: int = 11, kernel_overlap: float = 0.25): 109 | # We do not know features-dim here before going over all the items, 110 | # so put something dummy for now. PyTorch requires calling 111 | # nn.Module.__init__ before adding modules 112 | super(PerceptionNavigationExtractor, self).__init__(observation_space, features_dim=1) 113 | # We assume CxHxW images (channels first) 114 | # Re-ordering will be done by pre-preprocessing or wrapper 115 | 116 | extractors = {} 117 | 118 | total_concat_size = 0 119 | # We need to know size of the output of this extractor, 120 | # so go over all the spaces and compute output feature sizes 121 | for key, subspace in observation_space.spaces.items(): 122 | if key == "perception": 123 | # Pass sensor readings through CNN 124 | extractors[key] = RadarCNN(subspace, sensor_dim=sensor_dim, features_dim=features_dim, kernel_overlap=kernel_overlap) 125 | total_concat_size += features_dim # extractors[key].n_flatten 126 | elif key == "navigation": 127 | # Pass navigation features straight through to the MlpPolicy. 128 | extractors[key] = NavigatioNN(subspace, features_dim=subspace.shape[-1]) #nn.Identity() 129 | total_concat_size += subspace.shape[-1] 130 | 131 | self.extractors = nn.ModuleDict(extractors) 132 | 133 | # Update the features dim manually 134 | self._features_dim = total_concat_size 135 | 136 | def forward(self, observations) -> th.Tensor: 137 | encoded_tensor_list = [] 138 | 139 | # self.extractors contain nn.Modules that do all the processing. 140 | for key, extractor in self.extractors.items(): 141 | encoded_tensor_list.append(extractor(observations[key])) 142 | # Return a (B, self._features_dim) PyTorch tensor, where B is batch dimension. 143 | return th.cat(encoded_tensor_list, dim=1) 144 | 145 | 146 | if __name__ == '__main__': 147 | import numpy as np 148 | import matplotlib.pyplot as plt 149 | from stable_baselines3.common.base_class import BaseAlgorithm 150 | from stable_baselines3.ppo.policies import MlpPolicy 151 | from stable_baselines3 import PPO 152 | 153 | #### Test RadarCNN network circular 1D convolution: 154 | # Hyperparams 155 | n_sensors = 180 156 | kernel = 4 157 | padding = 4 158 | stride = 1 159 | 160 | ## Synthetic observation: (batch x channels x n_sensors) 161 | # Let obstacle be detected in the "edge" of the sensor array. 162 | # If circular padding works, should affect the outputs of the first elements 163 | obs = np.zeros((8, 3, n_sensors)) 164 | obs[:, 0, :] = 150.0 # max distance 165 | obs[:, 0, -9:-1] = 10.0 # obstacle detected close in last 9 sensors 166 | obs[:, 1, :] = 0.0 # no obstacle 167 | obs[:, 2, :] = 0.0 # no obstacle 168 | obs = th.as_tensor(obs).float() 169 | 170 | ## Load existing convnet 171 | def load_net(): 172 | from . import gym_auv 173 | algo = PPO 174 | #path = "radarCNN_example_Network.pkl" 175 | path = "../../radarCNN_example_Network150000.pkl" 176 | #path = "PPO_MlpPolicy_trained.pkl" 177 | #model = th.load(path) # RunTimeError: : [enforce fail at ..\caffe2\serialize\inline_container.cc:114] . file in archive is not in a subdirectory: data 178 | #model = MlpPolicy.load(path) 179 | model = algo.load(path) 180 | 181 | 182 | load_net() 183 | print("loaded net") 184 | exit() 185 | ## Initialize convolutional layers (circular padding in all layers or just the first?) 186 | # First layer retains spatial structure, 187 | # includes circular padding to maintain the continuous radial structure of the RADAR, 188 | # and increased the feature-space dimensionality for extrapolation 189 | # (other padding types:) 190 | net1 = nn.Conv1d(in_channels=3, out_channels=6, kernel_size=kernel, padding=padding, 191 | padding_mode='circular', stride=stride) 192 | # Second layer 193 | net2 = nn.Conv1d(in_channels=6, out_channels=3, kernel_size=kernel, padding=padding, 194 | padding_mode='circular', stride=stride) 195 | net3 = nn.Conv1d(in_channels=3, out_channels=1, kernel_size=kernel, stride=2) 196 | net4 = nn.Conv1d(in_channels=1, out_channels=1, kernel_size=kernel, stride=2) 197 | 198 | flatten = nn.Flatten() 199 | act = nn.ReLU() 200 | #conv_weights = np.zeros(net1.weight.shape) 201 | 202 | #out1 = net1(obs) 203 | #out2 = net2(out1) 204 | #out3 = net3(out2) 205 | #out4 = net4(out3) 206 | out1 = act(net1(obs)) 207 | out2 = act(net2(out1)) 208 | out3 = act(net3(out2)) 209 | out4 = act(net4(out3)) 210 | 211 | feat = flatten(out4) 212 | 213 | 214 | ## Print shapes and characteritics of intermediate layer outputs 215 | obs = obs.detach().numpy() 216 | out1 = out1.detach().numpy() 217 | out2 = out2.detach().numpy() 218 | out3 = out3.detach().numpy() 219 | out4 = out4.detach().numpy() 220 | feat = feat.detach().numpy() 221 | 222 | def th2np_info(arr): 223 | #arr = tensor.detach().numpy() 224 | return "{:15.2f}{:15.2f}{:15.2f}{:15.2f}".format(arr.mean(), arr.std(), np.min(arr), np.max(arr)) 225 | 226 | print("Observation", obs.shape, th2np_info(obs)) 227 | print("First layer", out1.shape, th2np_info(out1)) 228 | print("Second layer", out2.shape, th2np_info(out2)) 229 | print("Third layer", out3.shape, th2np_info(out3)) 230 | print("Fourth layer", out4.shape, th2np_info(out4)) 231 | print("Output features", feat.shape, th2np_info(feat)) 232 | 233 | ## PLOTTING 234 | plt.style.use('ggplot') 235 | plt.rc('font', family='serif') 236 | # plt.rc('font', family='serif', serif='Times') 237 | # plt.rc('text', usetex=True) 238 | plt.rc('xtick', labelsize=8) 239 | plt.rc('ytick', labelsize=8) 240 | plt.rc('axes', labelsize=8) 241 | plt.axis('scaled') 242 | def feat2radar(feat, avg=False): 243 | # Find length of feature vector 244 | n = feat.shape[-1] # number of activations differ between conv-layers 245 | feat = np.mean(feat, axis=0) if avg else feat[0] # average activations over batch or just select one 246 | 247 | # Find angles for each feature 248 | theta_d = 2 * np.pi / n # Spatial spread according to the number of actications 249 | theta = np.array([(i + 1)*theta_d for i in range(n)]) # Angles for each activation 250 | 251 | # Hotfix: append first element of each list to connect the ends of the lines in the plot. 252 | theta = np.append(theta, theta[0]) 253 | if len(feat.shape) > 1: 254 | _feat = [] 255 | for ch, f in enumerate(feat): 256 | ext = np.concatenate((f, [f[0]])) 257 | _feat.append(ext) 258 | else: 259 | _feat = np.append(feat, feat[0]) 260 | 261 | _feat = np.array(_feat) 262 | return theta, _feat # Return angle positions & list of features. 263 | 264 | # sensor angles : -pi -> pi, such that the first sensor is directly behind the vessel, and sensors go counter-clockwise around to the back again. 265 | _d_sensor_angle = 2 * np.pi / n_sensors 266 | sensor_angles = np.array([(i + 1)*_d_sensor_angle for i in range(n_sensors)]) 267 | sensor_distances = obs[0,0,:] 268 | 269 | # hotfix to connect lines on start and end 270 | sensor_angles = np.append(sensor_angles, sensor_angles[0]) 271 | sensor_distances = np.append(sensor_distances, sensor_distances[0]) 272 | 273 | 274 | fig, ax = plt.subplots(figsize=(11,11), subplot_kw={'projection': 'polar'}) 275 | ax.set_theta_zero_location("S") 276 | ax.plot(sensor_angles, sensor_distances) 277 | ax.set_rmax(1) 278 | ax.set_rticks([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0]) # Less radial ticks 279 | ax.set_rlabel_position(22.5) # Move radial labels away from plotted line 280 | #ax.set_rscale('symlog') 281 | ax.grid(True) 282 | 283 | ax.set_title("RadarCNN: intermediate layers visualization", va='bottom') 284 | 285 | to_plot = [obs, out1, out2, out3, out4, feat] 286 | names = ["obs", "out1", "out2", "out3", "out4", "feat"] 287 | channels = [0,1,2,3,4,5] 288 | linetypes = ["solid", 'dotted', 'dashed', 'dashdot', (0, (5, 10)), (0, (3, 5, 1, 5, 1, 5))] 289 | CB_color_cycle = ['#377eb8', '#ff7f00', '#4daf4a', 290 | '#f781bf', '#a65628', '#984ea3', 291 | '#999999', '#e41a1c', '#dede00'] 292 | 293 | layer_color = { 294 | 'obs' : '#377eb8', 295 | 'out1': '#ff7f00', 296 | 'out2': '#4daf4a', 297 | 'out3': '#f781bf', 298 | 'out4': '#a65628', 299 | 'feat': '#984ea3', 300 | } 301 | 302 | for arr, layer in zip(to_plot, names): 303 | angle, data = feat2radar(arr, avg=False) 304 | if len(data.shape) > 1: 305 | for ch, _d in enumerate(data): 306 | ax.plot(angle, _d, linestyle=linetypes[ch], color=layer_color[layer], label=layer+'_ch'+str(ch)) 307 | else: 308 | ax.plot(angle, data, linestyle=linetypes[0], color=layer_color[layer], label=layer) 309 | 310 | plt.legend(loc="upper right", bbox_to_anchor=(1.2, 1.0)) 311 | plt.tight_layout() 312 | plt.show() 313 | -------------------------------------------------------------------------------- /gym_auv/objects/rewarder.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from abc import ABC, abstractmethod 3 | from gym_auv.objects.vessel import Vessel 4 | 5 | deg2rad = np.pi/180 6 | rad2deg = 180/np.pi 7 | 8 | def _sample_lambda(scale): 9 | log = -np.random.gamma(1, scale) 10 | y = np.power(10, log) 11 | return y 12 | 13 | def _sample_eta(): 14 | y = np.random.gamma(shape=1.9, scale=0.6) 15 | return y 16 | 17 | class BaseRewarder(ABC): 18 | def __init__(self, vessel, test_mode) -> None: 19 | self._vessel = vessel 20 | self._test_mode = test_mode 21 | self.params = {} 22 | 23 | @property 24 | def vessel(self) -> Vessel: 25 | """Vessel instance that the reward is calculated with respect to.""" 26 | return self._vessel[-1] 27 | 28 | @abstractmethod 29 | def calculate(self) -> float: 30 | """ 31 | Calculates the step reward and decides whether the episode 32 | should be ended. 33 | 34 | Returns 35 | ------- 36 | reward : float 37 | The reward for performing action at this timestep. 38 | """ 39 | 40 | def insight(self) -> np.ndarray: 41 | """ 42 | Returns a numpy array with reward parameters for the agent 43 | to have an insight into its reward function 44 | 45 | Returns 46 | ------- 47 | insight : np.array 48 | The reward insight array at this timestep. 49 | """ 50 | return np.array([]) 51 | 52 | 53 | class PathRewarder(BaseRewarder): 54 | def __init__(self, vessel, test_mode): 55 | super().__init__(vessel, test_mode) 56 | self.params['gamma_theta'] = 10.0 57 | self.params['gamma_x'] = 0.1 58 | self.params['gamma_v_y'] = 1.0 59 | self.params['gamma_y_e'] = 5.0 60 | self.params['penalty_yawrate'] = 0.0 # not used 61 | self.params['penalty_torque_change'] = 0.0 62 | self.params['cruise_speed'] = 0.1 63 | self.params['neutral_speed'] = 0.05 64 | self.params['negative_multiplier'] = 2.0 65 | self.params['collision'] = -1000.0 66 | self.params['lambda'] = 0.5 # _sample_lambda(scale=0.2) 67 | self.params['eta'] = 0 # _sample_eta() 68 | 69 | N_INSIGHTS = 0 70 | 71 | def insight(self): 72 | return np.array([]) 73 | # return np.array([np.log10(self.params['lambda'])]) 74 | 75 | def calculate(self): 76 | latest_data = self._vessel.req_latest_data() 77 | nav_states = latest_data['navigation'] 78 | collision = latest_data['collision'] 79 | 80 | if collision: 81 | reward = self.params["collision"] # * (1 - self.params["lambda"]) # -5000 82 | return reward 83 | 84 | reward = 0 85 | 86 | # Extracting navigation states 87 | cross_track_error = nav_states['cross_track_error'] 88 | heading_error = nav_states['heading_error'] 89 | 90 | # Calculating path following reward component 91 | #cross_track_performance = np.exp(-self.params['gamma_y_e'] * np.abs(cross_track_error)) 92 | #path_reward = (1 + np.cos(heading_error) * self._vessel.speed / self._vessel.max_speed) * ( 93 | # 1 + cross_track_performance) - 1 94 | 95 | # Calculate path reward 96 | speed_term = self._vessel.speed / self._vessel.max_speed 97 | cte_term = 1 / (np.abs(cross_track_error) + 1) # 1 if on path, smaller otherwise 98 | heading_term = 1 + np.cos(heading_error) # 1 if heading in right dir, 0 when heading in opposite dir. 99 | path_reward = heading_term * cte_term * speed_term # 1 if on path heading in right dir at max speed 100 | # 0 if heading in opposite dir, small if right dir but far from path. 101 | 102 | # Calculating living penalty: 0.5*(2*0.05 + 1) = 0.05+0.5 = 0.55 103 | living_penalty = 1.0 # 0.55 104 | 105 | # Calculating total reward 106 | reward = path_reward - living_penalty 107 | 108 | 109 | return reward 110 | 111 | 112 | class ColavRewarder2(BaseRewarder): 113 | def __init__(self, vessel, test_mode): 114 | super().__init__(vessel, test_mode) 115 | self.params['gamma_theta'] = 10.0 116 | self.params['gamma_x'] = 0.1 117 | self.params['gamma_v_y'] = 1.0 118 | self.params['gamma_y_e'] = 5.0 119 | self.params['penalty_yawrate'] = 0.0 120 | self.params['penalty_torque_change'] = 0.0 121 | self.params['cruise_speed'] = 0.1 122 | self.params['neutral_speed'] = 0.05 123 | self.params['negative_multiplier'] = 2.0 124 | self.params['collision'] = -1000.0 125 | self.params['lambda'] = 0.5 # _sample_lambda(scale=0.2) 126 | self.params['eta'] = 0 # _sample_eta() 127 | 128 | N_INSIGHTS = 0 129 | 130 | def insight(self): 131 | return np.array([]) 132 | # return np.array([np.log10(self.params['lambda'])]) 133 | 134 | def calculate(self): 135 | latest_data = self._vessel.req_latest_data() 136 | nav_states = latest_data['navigation'] 137 | measured_distances = latest_data['distance_measurements'] 138 | measured_speeds = latest_data['speed_measurements'] 139 | collision = latest_data['collision'] 140 | 141 | if collision: 142 | reward = self.params["collision"] 143 | return reward 144 | 145 | reward = 0 146 | 147 | # Extracting navigation states 148 | cross_track_error = nav_states['cross_track_error'] 149 | heading_error = nav_states['heading_error'] 150 | 151 | # Calculating path following reward component 152 | cross_track_performance = np.exp(-self.params['gamma_y_e'] * np.abs(cross_track_error)) 153 | path_reward = (1 + np.cos(heading_error) * self._vessel.speed / self._vessel.max_speed) * ( 154 | 1 + cross_track_performance) - 1 155 | 156 | # Calculating obstacle avoidance reward component 157 | closeness_penalty_num = 0 158 | closeness_penalty_den = 0 159 | if self._vessel.n_sensors > 0: 160 | for isensor in range(self._vessel.n_sensors): 161 | angle = self._vessel.sensor_angles[isensor] 162 | x = measured_distances[isensor] 163 | speed_vec = measured_speeds[isensor] 164 | weight = 1 / (1 + np.abs(self.params['gamma_theta'] * angle)) 165 | raw_penalty = self._vessel.config["sensor_range"] * np.exp( 166 | -self.params['gamma_x'] * x + self.params['gamma_v_y'] * max(0, speed_vec[1])) 167 | weighted_penalty = weight * raw_penalty 168 | closeness_penalty_num += weighted_penalty 169 | closeness_penalty_den += weight 170 | 171 | closeness_reward = -closeness_penalty_num / closeness_penalty_den 172 | else: 173 | closeness_reward = 0 174 | 175 | # Calculating living penalty 176 | living_penalty = 2.0 #self.params['lambda'] * (2 * self.params["neutral_speed"] + 1) + self.params["eta"] * self.params["neutral_speed"] 177 | 178 | 179 | # Calculating total reward 180 | reward = path_reward + \ 181 | 0.05 * closeness_reward - \ 182 | living_penalty 183 | 184 | return reward 185 | 186 | 187 | class ColavRewarder(BaseRewarder): 188 | def __init__(self, vessel, test_mode): 189 | super().__init__(vessel, test_mode) 190 | self.params['gamma_theta'] = 10.0 191 | self.params['gamma_x'] = 0.1 192 | self.params['gamma_v_y'] = 1.0 193 | self.params['gamma_y_e'] = 5.0 194 | self.params['penalty_yawrate'] = 0.0 195 | self.params['penalty_torque_change'] = 0.0 196 | self.params['cruise_speed'] = 0.1 197 | self.params['neutral_speed'] = 0.05 198 | self.params['negative_multiplier'] = 2.0 199 | self.params['collision'] = -1000.0 200 | self.params['lambda'] = 0.5 #_sample_lambda(scale=0.2) 201 | self.params['eta'] = 0#_sample_eta() 202 | 203 | N_INSIGHTS = 0 204 | def insight(self): 205 | return np.array([]) 206 | #return np.array([np.log10(self.params['lambda'])]) 207 | 208 | def calculate(self): 209 | latest_data = self._vessel.req_latest_data() 210 | nav_states = latest_data['navigation'] 211 | measured_distances = latest_data['distance_measurements'] 212 | measured_speeds = latest_data['speed_measurements'] 213 | collision = latest_data['collision'] 214 | 215 | if collision: 216 | reward = self.params["collision"]*(1-self.params["lambda"]) 217 | return reward 218 | 219 | reward = 0 220 | 221 | # Extracting navigation states 222 | cross_track_error = nav_states['cross_track_error'] 223 | heading_error = nav_states['heading_error'] 224 | 225 | # Calculating path following reward component 226 | cross_track_performance = np.exp(-self.params['gamma_y_e']*np.abs(cross_track_error)) 227 | path_reward = (1 + np.cos(heading_error)*self._vessel.speed/self._vessel.max_speed)*(1 + cross_track_performance) - 1 228 | 229 | # Calculating obstacle avoidance reward component 230 | closeness_penalty_num = 0 231 | closeness_penalty_den = 0 232 | if self._vessel.n_sensors > 0: 233 | for isensor in range(self._vessel.n_sensors): 234 | angle = self._vessel.sensor_angles[isensor] 235 | x = measured_distances[isensor] 236 | speed_vec = measured_speeds[isensor] 237 | weight = 1 / (1 + np.abs(self.params['gamma_theta']*angle)) 238 | raw_penalty = self._vessel.config["sensor_range"]*np.exp(-self.params['gamma_x']*x +self.params['gamma_v_y']*max(0, speed_vec[1])) 239 | weighted_penalty = weight*raw_penalty 240 | closeness_penalty_num += weighted_penalty 241 | closeness_penalty_den += weight 242 | 243 | closeness_reward = -closeness_penalty_num/closeness_penalty_den 244 | else: 245 | closeness_reward = 0 246 | 247 | # Calculating living penalty 248 | living_penalty = self.params['lambda']*(2*self.params["neutral_speed"]+1) + self.params["eta"]*self.params["neutral_speed"] 249 | 250 | # Calculating total reward 251 | reward = self.params['lambda']*path_reward + \ 252 | (1-self.params['lambda'])*closeness_reward - \ 253 | living_penalty + \ 254 | self.params["eta"]*self._vessel.speed/self._vessel.max_speed - \ 255 | self.params["penalty_yawrate"]*abs(self._vessel.yaw_rate) 256 | 257 | #if reward < 0: 258 | # reward *= self.params['negative_multiplier'] 259 | 260 | return reward 261 | 262 | class ColregRewarder(BaseRewarder): 263 | def __init__(self, vessel, test_mode): 264 | super().__init__(vessel, test_mode) 265 | self.params['gamma_theta'] = 10.0 266 | self.params['gamma_x_stat'] = 0.09 267 | self.params['gamma_x_starboard'] = 0.07 268 | self.params['gamma_x_port'] = 0.09 269 | self.params['gamma_v_y'] = 2.0 270 | self.params['gamma_y_e'] = 5.0 271 | self.params['penalty_yawrate'] = 0.0 272 | self.params['penalty_torque_change'] = 0.01 273 | self.params['cruise_speed'] = 0.1 274 | self.params['neutral_speed'] = 0.1 275 | self.params['negative_multiplier'] = 2.0 276 | self.params['collision'] = -10000.0 277 | self.params['lambda'] = 0.5#_sample_lambda(scale=0.2) 278 | self.params['eta'] = 0.2#_sample_eta() 279 | self.params['alpha_lambda'] = 3.5 280 | self.params['gamma_min_x'] = 0.04 281 | self.params['gamma_weight'] = 2 282 | 283 | 284 | N_INSIGHTS = 1 285 | def insight(self): 286 | return np.array ([self.params['lambda']]) 287 | 288 | def calculate(self): 289 | latest_data = self._vessel.req_latest_data() 290 | nav_states = latest_data['navigation'] 291 | measured_distances = latest_data['distance_measurements'] 292 | measured_speeds = latest_data['speed_measurements'] 293 | collision = latest_data['collision'] 294 | #print([x[1] for x in measured_speeds]) 295 | if collision: 296 | reward = self.params['collision']#*(1-self.params['lambda']) 297 | return reward 298 | 299 | reward = 0 300 | 301 | # Extracting navigation states 302 | cross_track_error = nav_states['cross_track_error'] 303 | heading_error = nav_states['heading_error'] 304 | 305 | # Calculating path following reward component 306 | cross_track_performance = np.exp(-self.params['gamma_y_e']*np.abs(cross_track_error)) 307 | path_reward = (1 + np.cos(heading_error)*self._vessel.speed/self._vessel.max_speed)*(1 + cross_track_performance) - 1 308 | 309 | # Calculating obstacle avoidance reward component 310 | closeness_penalty_num = 0 311 | closeness_penalty_den = 0 312 | static_closeness_penalty_num = 0 313 | static_closeness_penalty_den = 0 314 | closeness_reward = 0 315 | static_closeness_reward = 0 316 | moving_distances = [] 317 | lambdas = [] 318 | 319 | speed_weight = 2 320 | 321 | if self._vessel.n_sensors > 0: 322 | for isensor in range(self._vessel.n_sensors): 323 | angle = self._vessel.sensor_angles[isensor] 324 | x = measured_distances[isensor] 325 | speed_vec = measured_speeds[isensor] 326 | 327 | 328 | if speed_vec.any(): 329 | 330 | if speed_vec[1] > 0: 331 | self.params['lambda'] = 1/(1+np.exp(-0.04*x+4)) 332 | if speed_vec[1] < 0: 333 | self.params['lambda'] = 1/(1+np.exp(-0.06*x+3)) 334 | lambdas.append(self.params['lambda']) 335 | 336 | weight = 2 / (1 + np.exp(self.params['gamma_weight']*np.abs(angle))) 337 | moving_distances.append(x) 338 | 339 | if angle < 0*deg2rad and angle > -112.5*deg2rad: #straffer høyre side 340 | 341 | raw_penalty = 100*np.exp(-self.params['gamma_x_starboard']*x + speed_weight*speed_vec[1]) 342 | else: 343 | raw_penalty = 100*np.exp(-self.params['gamma_x_port']*x + speed_weight*speed_vec[1]) 344 | #else: 345 | # raw_penalty = 100*np.exp(-self.params['gamma_x_stat']*x) 346 | 347 | weighted_penalty = (1-self.params['lambda'])*weight*raw_penalty 348 | closeness_penalty_num += weighted_penalty 349 | closeness_penalty_den += weight 350 | 351 | else: 352 | 353 | weight = 1 / (1 + np.abs(self.params['gamma_theta']*angle)) 354 | raw_penalty = 100*np.exp(-self.params['gamma_x_stat']*x) 355 | weighted_penalty = weight*raw_penalty 356 | static_closeness_penalty_num += weighted_penalty 357 | static_closeness_penalty_den += weight 358 | 359 | 360 | 361 | if closeness_penalty_num: 362 | closeness_reward = -closeness_penalty_num/closeness_penalty_den 363 | 364 | 365 | if static_closeness_penalty_num: 366 | static_closeness_reward = -static_closeness_penalty_num/static_closeness_penalty_den 367 | 368 | 369 | #if len(moving_distances) != 0: 370 | # min_dist = np.amin(moving_distances) 371 | # self.params['lambda'] = 1/(1+np.exp(-0.04*min_dist+4)) 372 | #self.params['lambda'] = 1/(1+np.exp(-(self.params['gamma_min_x']*min_dist-self.params['alpha_lambda']))) 373 | #else: 374 | # self.params['lambda'] = 1 375 | 376 | if len(lambdas): 377 | path_lambda = np.amin(lambdas) 378 | else: 379 | path_lambda = 1 380 | 381 | #if path_reward > 0: 382 | # path_reward = path_lambda*path_reward 383 | # Calculating living penalty 384 | living_penalty = 1#.2*(self.params['neutral_speed']+1) + self.params['eta']*self.params['neutral_speed'] 385 | 386 | # Calculating total reward 387 | reward = path_lambda*path_reward + \ 388 | static_closeness_reward + \ 389 | closeness_reward - \ 390 | living_penalty + \ 391 | self.params['eta']*self._vessel.speed/self._vessel.max_speed# - \ 392 | #self.params['penalty_yawrate']*abs(self._vessel.yaw_rate) 393 | 394 | if reward < 0: 395 | reward *= self.params['negative_multiplier'] 396 | 397 | return reward -------------------------------------------------------------------------------- /gym_auv/environment.py: -------------------------------------------------------------------------------- 1 | import gym 2 | import numpy as np 3 | from gym.utils import seeding 4 | 5 | from gym_auv.objects.vessel import Vessel 6 | from gym_auv.objects.rewarder import ColavRewarder, PathRewarder 7 | import gym_auv.rendering.render2d as render2d 8 | import gym_auv.rendering.render3d as render3d 9 | from abc import ABC, abstractmethod 10 | 11 | import tables 12 | import os 13 | 14 | class BaseEnvironment(gym.Env, ABC): 15 | """Creates an environment with a vessel and a path.""" 16 | 17 | metadata = { 18 | 'render.modes': ['human', 'rgb_array', 'state_pixels'], 19 | 'video.frames_per_second': render2d.FPS 20 | } 21 | 22 | def __init__(self, env_config, test_mode=False, render_mode='2d', verbose=False): 23 | """The __init__ method declares all class atributes and calls 24 | the self.reset() to intialize them properly. 25 | 26 | Parameters 27 | ---------- 28 | env_config : dict 29 | Configuration parameters for the environment. 30 | The default values are set in __init__.py 31 | test_mode : bool 32 | If test_mode is True, the environment will not be autonatically reset 33 | due to too low cumulative reward or too large distance from the path. 34 | render_mode : {'2d', '3d', 'both'} 35 | Whether to use 2d or 3d rendering. 'both' is currently broken. 36 | verbose 37 | Whether to print debugging information. 38 | """ 39 | 40 | if not hasattr(self, '_rewarder_class'): 41 | self._rewarder_class = PathRewarder # ColavRewarder 42 | self._n_moving_obst = 10 43 | self._n_moving_stat = 10 44 | 45 | self.test_mode = test_mode 46 | self.render_mode = render_mode 47 | self.verbose = verbose 48 | self.config = env_config 49 | 50 | # Setting dimension of observation vector 51 | self._n_sensors = self.config["n_sensors_per_sector"] * self.config["n_sectors"] 52 | self.n_navigation_obs = len(Vessel.NAVIGATION_FEATURES) 53 | self.n_perception_obs = self._n_sensors # *self.config["n_sectors"] 54 | self.n_observations = len(Vessel.NAVIGATION_FEATURES) + self.config["n_sectors"] 55 | 56 | self.episode = 0 57 | self.total_t_steps = 0 58 | self.t_step = 0 59 | self.cumulative_reward = 0 60 | self.rewarder = None 61 | 62 | self.history = dict.fromkeys(['cross_track_error', 63 | 'reached_goal', 64 | 'collision', 65 | 'reward', 66 | 'timesteps', 67 | 'duration', 68 | 'progress', 69 | 'pathlength' 70 | ], np.array([])) 71 | #self.history = [] 72 | 73 | # Declaring attributes 74 | self.obstacles = [] 75 | self.vessel = None 76 | self.path = None 77 | 78 | self.reached_goal = None 79 | self.collision = None 80 | self.progress = None 81 | self.last_reward = None 82 | self.last_episode = None 83 | self.rng = None 84 | self.seed() 85 | self._tmp_storage = None 86 | self._last_image_frame = None 87 | 88 | self._action_space = gym.spaces.Box( 89 | low=np.array([-1, -1]), 90 | high=np.array([1, 1]), 91 | dtype=np.float32 92 | ) 93 | self._perception_space = gym.spaces.Box( 94 | low=0, 95 | high=1, 96 | shape=(1, self._n_sensors), 97 | dtype=np.float32 98 | ) 99 | self._navigation_space = gym.spaces.Box( 100 | low=-np.inf, # Try -1 101 | high=np.inf, # Try +1 102 | shape=(1, self.n_navigation_obs), 103 | dtype=np.float32 104 | ) 105 | self._observation_space = gym.spaces.Dict({ 106 | 'perception': self._perception_space, 107 | 'navigation': self._navigation_space 108 | }) 109 | 110 | # Initializing rendering 111 | self._viewer2d = None 112 | self._viewer3d = None 113 | if self.render_mode == '2d' or self.render_mode == 'both': 114 | render2d.init_env_viewer(self) 115 | if self.render_mode == '3d' or self.render_mode == 'both': 116 | if self.config['render_distance'] == 'random': 117 | self.render_distance = self.rng.randint(300, 2000) 118 | else: 119 | self.render_distance = self.config['render_distance'] 120 | render3d.init_env_viewer(self, autocamera=self.config["autocamera3d"], render_dist=self.render_distance) 121 | 122 | self.reset() 123 | print("BaseEnvironment init complete") 124 | 125 | @property 126 | def action_space(self) -> gym.spaces.Box: 127 | """Array defining the shape and bounds of the agent's action.""" 128 | return self._action_space 129 | 130 | @property 131 | def observation_space(self) -> gym.spaces.Dict: 132 | """Array defining the shape and bounds of the agent's observations.""" 133 | return self._observation_space 134 | 135 | def reset(self, save_history=True): 136 | """Reset the environment's state. Returns observation. 137 | 138 | Returns 139 | ------- 140 | obs : np.ndarray 141 | The initial observation of the environment. 142 | """ 143 | 144 | if self.verbose: print('Resetting environment... Last reward was {:.2f}'.format(self.cumulative_reward)) 145 | 146 | # Seeding 147 | if self.rng is None: 148 | self.seed() 149 | 150 | # Saving information about episode 151 | if self.t_step: 152 | self.save_latest_episode(save_history=save_history) 153 | 154 | # Incrementing counters 155 | self.episode += 1 156 | self.total_t_steps += self.t_step 157 | 158 | # Resetting all internal variables 159 | self.cumulative_reward = 0 160 | self.t_step = 0 161 | self.last_reward = 0 162 | self.reached_goal = False 163 | self.collision = False 164 | self.progress = 0 165 | self._last_image_frame = None 166 | 167 | # Generating a new environment 168 | if self.verbose: print('Generating scenario...') 169 | self._generate() 170 | self.rewarder = self._rewarder_class(self.vessel, self.test_mode) # Resetting rewarder instance 171 | if self.verbose: print('Generated scenario') 172 | 173 | # Initializing 3d viewer 174 | if self.render_mode == '3d': 175 | render3d.init_boat_model(self) 176 | #self._viewer3d.create_path(self.path) 177 | 178 | # Getting initial observation vector 179 | obs = self.observe() 180 | if self.verbose: print('Calculated initial observation') 181 | 182 | # Resetting temporary data storage 183 | self._tmp_storage = { 184 | 'cross_track_error': [], 185 | } 186 | 187 | return obs 188 | 189 | def observe(self): # -> np.ndarray: 190 | """Returns the array of observations at the current time-step. 191 | 192 | Returns 193 | ------- 194 | obs : np.ndarray 195 | The observation of the environment. 196 | """ 197 | navigation_states = self.vessel.navigate(self.path) 198 | if bool(self.config["sensing"]): 199 | perception_states = self.vessel.perceive(self.obstacles) 200 | else: 201 | perception_states = [] 202 | 203 | obs = {'perception' : perception_states, 'navigation' : navigation_states } 204 | return obs 205 | 206 | def step(self, action:list) -> (np.ndarray, float, bool, dict): 207 | """ 208 | Steps the environment by one timestep. Returns observation, reward, done, info. 209 | 210 | Parameters 211 | ---------- 212 | action : np.ndarray 213 | [thrust_input, torque_input]. 214 | 215 | Returns 216 | ------- 217 | obs : np.ndarray 218 | Observation of the environment after action is performed. 219 | reward : double 220 | The reward for performing action at his timestep. 221 | done : bool 222 | If True the episode is ended, due to either a collision or having reached the goal position. 223 | info : dict 224 | Dictionary with data used for reporting or debugging 225 | """ 226 | 227 | action[0] = (action[0] + 1)/2 # Done to be compatible with RL algorithms that require symmetric action spaces 228 | if np.isnan(action).any(): action = np.zeros(action.shape) 229 | 230 | # If the environment is dynamic, calling self.update will change it. 231 | self._update() 232 | 233 | # Updating vessel state from its dynamics model 234 | self.vessel.step(action) 235 | 236 | # Getting observation vector 237 | obs = self.observe() 238 | vessel_data = self.vessel.req_latest_data() 239 | self.collision = vessel_data['collision'] 240 | self.reached_goal = vessel_data['reached_goal'] 241 | self.goal_distance = vessel_data['navigation']['goal_distance'] 242 | self.progress = vessel_data['progress'] 243 | 244 | # Receiving agent's reward 245 | reward = self.rewarder.calculate() 246 | self.last_reward = reward 247 | self.cumulative_reward += reward 248 | 249 | info = {} 250 | info['collision'] = self.collision 251 | info['reached_goal'] = self.reached_goal 252 | info['goal_distance'] = self.goal_distance 253 | info['progress'] = self.progress 254 | 255 | # Testing criteria for ending the episode 256 | done = self._isdone() 257 | 258 | self._save_latest_step() 259 | 260 | self.t_step += 1 261 | 262 | return (obs, reward, done, info) 263 | 264 | def _isdone(self) -> bool: 265 | return any([ 266 | self.collision, 267 | self.reached_goal, 268 | self.t_step > self.config["max_timesteps"], # and not self.test_mode, 269 | self.cumulative_reward < self.config["min_cumulative_reward"] and not self.test_mode 270 | ]) 271 | 272 | def _update(self) -> None: 273 | """Updates the environment at each time-step. Can be customized in sub-classes.""" 274 | [obst.update(dt=self.config["t_step_size"]) for obst in self.obstacles if not obst.static] 275 | 276 | @abstractmethod 277 | def _generate(self) -> None: 278 | """Create new, stochastically genereated scenario. 279 | To be implemented in extensions of BaseEnvironment. Must set the 280 | 'vessel', 'path' and 'obstacles' attributes. 281 | """ 282 | 283 | def close(self): 284 | """Closes the environment. To be called after usage.""" 285 | if self._viewer2d is not None: 286 | self._viewer2d.close() 287 | if self._viewer3d is not None: 288 | self._viewer3d.close() 289 | 290 | def render(self, mode='human'): 291 | """Render one frame of the environment. 292 | The default mode will do something human friendly, such as pop up a window.""" 293 | image_arr = None 294 | try: 295 | if self.render_mode == '2d' or self.render_mode == 'both': 296 | image_arr = render2d.render_env(self, mode) 297 | if self.render_mode == '3d' or self.render_mode == 'both': 298 | image_arr = render3d.render_env(self, mode, self.config["t_step_size"]) 299 | except OSError: 300 | image_arr = self._last_image_frame 301 | 302 | if image_arr is None: 303 | image_arr = self._last_image_frame 304 | else: 305 | self._last_image_frame = image_arr 306 | 307 | if image_arr is None and mode == 'rgb_array': 308 | print('Warning: image_arr is None -> video is likely broken' ) 309 | 310 | return image_arr 311 | 312 | def seed(self, seed=None): 313 | """Reseeds the random number generator used in the environment""" 314 | self.rng, seed = seeding.np_random(seed) 315 | return [seed] 316 | 317 | def _save_latest_step(self): 318 | latest_data = self.vessel.req_latest_data() 319 | self._tmp_storage['cross_track_error'].append(abs(latest_data['navigation']['cross_track_error'])*100) 320 | 321 | def save_latest_episode(self, save_history=True): 322 | #print('Saving latest episode with save_history = ' + str(save_history)) 323 | self.last_episode = { 324 | 'path': self.path(np.linspace(0, self.path.length, 1000)) if self.path is not None else None, 325 | 'path_taken': np.array(self.vessel.path_taken), 326 | 'obstacles': np.array(self.obstacles) 327 | } 328 | if save_history: 329 | stats = { 330 | 'cross_track_error': np.array(self._tmp_storage['cross_track_error']).mean(), 331 | 'reached_goal': int(self.reached_goal), 332 | 'collision': int(self.collision), 333 | 'reward': self.cumulative_reward, 334 | 'timesteps': self.t_step, 335 | 'duration': self.t_step*self.config["t_step_size"], 336 | 'progress': self.progress, 337 | 'pathlength': self.path.length 338 | } 339 | for key in self.history.keys(): 340 | self.history[key] = np.append(self.history[key], stats[key]) 341 | 342 | 343 | def store_statistics_to_file(self, path): 344 | path_history = os.path.join(path, 'history.h5') 345 | 346 | if not self.history: 347 | print("DEBUG: environment.py: store_statistics_to_file(): self.history is empty, skipping...") 348 | return 349 | 350 | if not os.path.exists(path_history): 351 | f = tables.open_file(path_history, mode='w', title="Training Statistics") 352 | 353 | group = f.create_group("/", "RL_agent", "DRL Agent Training statistics") 354 | history_table = f.create_table(group, "history", Log, "History") 355 | trajectory_table = f.create_table(group, "trajectory", TrajectoryLog, "Trajectories") 356 | 357 | table_row = history_table.row # Points to first row if table instance 358 | table_row["episode"] = self.episode 359 | table_row["timesteps"] = self.history["timesteps"] 360 | table_row["duration"] = self.history["duration"] 361 | table_row["reached_goal"] = self.history["reached_goal"] 362 | table_row["collision"] = self.history["collision"] 363 | table_row["cross_track_error"] = self.history["cross_track_error"] 364 | table_row["reward"] = self.history["reward"] 365 | table_row["progress"] = self.history["progress"] 366 | table_row["pathlength"] = self.history["pathlength"] 367 | table_row.append() 368 | history_table.flush() 369 | 370 | #path_array = f.create_array(group, "path", np.array(self.last_episode["path"]), "Path") 371 | #path_taken_array = f.create_array(group, "path_taken", np.array(self.last_episode["path_taken"]), "Path taken") 372 | #obstacles_array = f.create_array(group, "obstacles", np.array(self.last_episode["obstacles"]), "Obstacles") 373 | table_row = trajectory_table.row 374 | table_row["episode"] = self.episode 375 | table_row["path"] = self.last_episode["path"] 376 | table_row["path_taken"][:self.last_episode["path_taken"].shape[0], :] = self.last_episode["path_taken"] 377 | table_row["obstacles"][:self.last_episode["obstacles"].shape[0], :] = self.last_episode["obstacles"] \ 378 | if self.last_episode["obstacles"] else None 379 | table_row.append() 380 | trajectory_table.flush() 381 | 382 | f.close() 383 | else: 384 | f = tables.open_file(path_history, mode='a') 385 | history_table = f.root.RL_agent.history 386 | trajectory_table = f.root.RL_agent.trajectory 387 | 388 | table_row = history_table.row 389 | 390 | table_row["episode"] = self.episode 391 | table_row["timesteps"] = self.history["timesteps"] 392 | table_row["duration"] = self.history["duration"] 393 | table_row["reached_goal"] = self.history["reached_goal"] 394 | table_row["collision"] = self.history["collision"] 395 | table_row["cross_track_error"] = self.history["cross_track_error"] 396 | table_row["reward"] = self.history["reward"] 397 | table_row["progress"] = self.history["progress"] 398 | table_row["pathlength"] = self.history["pathlength"] 399 | table_row.append() 400 | history_table.flush() 401 | 402 | #path_array_row = f.RL_agent.path.row 403 | #path_array_row = self.last_episode["path"] 404 | #path_taken_array = f.RL_agent.path_taken 405 | #obstacles_array = f.RL_agent.obstacles 406 | table_row = trajectory_table.row 407 | table_row["episode"] = self.episode 408 | table_row["path"] = self.last_episode["path"] 409 | table_row["path_taken"][:self.last_episode["path_taken"].shape[0], :] = self.last_episode["path_taken"] 410 | table_row["obstacles"][:self.last_episode["obstacles"].shape[0], :] = self.last_episode["obstacles"] \ 411 | if self.last_episode["obstacles"] else None 412 | table_row.append() 413 | trajectory_table.flush() 414 | 415 | f.close() 416 | 417 | 418 | class Log(tables.IsDescription): 419 | episode = tables.Int32Col() 420 | timesteps = tables.Int32Col() 421 | duration = tables.Float32Col() 422 | reached_goal = tables.Int32Col() 423 | collision = tables.Int32Col() 424 | cross_track_error = tables.Int32Col() 425 | reward = tables.Float32Col() 426 | progress = tables.Float32Col() 427 | pathlength = tables.Float32Col() 428 | 429 | 430 | class TrajectoryLog(tables.IsDescription): 431 | episode = tables.Int32Col() 432 | path = tables.Float32Col(shape=(2,1000)) 433 | path_taken = tables.Float32Col(shape=(10000, 2)) 434 | obstacles = tables.Float32Col(shape=(500, 2)) -------------------------------------------------------------------------------- /gym_auv/objects/vessel.py: -------------------------------------------------------------------------------- 1 | """ 2 | This module implements an AUV that is simulated in the horizontal plane. 3 | """ 4 | import numpy as np 5 | import numpy.linalg as linalg 6 | from itertools import islice, chain, repeat 7 | import shapely.geometry, shapely.errors, shapely.strtree, shapely.ops, shapely.prepared 8 | 9 | import gym_auv.utils.constants as const 10 | import gym_auv.utils.geomutils as geom 11 | from gym_auv.objects.obstacles import LineObstacle 12 | from gym_auv.objects.path import Path 13 | 14 | def _odesolver45(f, y, h): 15 | """Calculate the next step of an IVP of a time-invariant ODE with a RHS 16 | described by f, with an order 4 approx. and an order 5 approx. 17 | Parameters: 18 | f: function. RHS of ODE. 19 | y: float. Current position. 20 | h: float. Step length. 21 | Returns: 22 | q: float. Order 2 approx. 23 | w: float. Order 3 approx. 24 | """ 25 | s1 = f(y) 26 | s2 = f(y+h*s1/4.0) 27 | s3 = f(y+3.0*h*s1/32.0+9.0*h*s2/32.0) 28 | s4 = f(y+1932.0*h*s1/2197.0-7200.0*h*s2/2197.0+7296.0*h*s3/2197.0) 29 | s5 = f(y+439.0*h*s1/216.0-8.0*h*s2+3680.0*h*s3/513.0-845.0*h*s4/4104.0) 30 | s6 = f(y-8.0*h*s1/27.0+2*h*s2-3544.0*h*s3/2565+1859.0*h*s4/4104.0-11.0*h*s5/40.0) 31 | w = y + h*(25.0*s1/216.0+1408.0*s3/2565.0+2197.0*s4/4104.0-s5/5.0) 32 | q = y + h*(16.0*s1/135.0+6656.0*s3/12825.0+28561.0*s4/56430.0-9.0*s5/50.0+2.0*s6/55.0) 33 | return w, q 34 | 35 | def _standardize_intersect(intersect): 36 | if intersect.is_empty: 37 | return [] 38 | elif isinstance(intersect, shapely.geometry.LineString): 39 | return [shapely.geometry.Point(intersect.coords[0])] 40 | elif isinstance(intersect, shapely.geometry.Point): 41 | return [intersect] 42 | else: 43 | return list(intersect.geoms) 44 | 45 | def _feasibility_pooling(x, width, theta): 46 | N_sensors = x.shape[0] 47 | sort_idx = np.argsort(x) 48 | for idx in sort_idx: 49 | surviving = x > x[idx] + width 50 | d = x[idx]*theta 51 | opening_width = 0 52 | opening_span = 0 53 | opening_start = -theta*(N_sensors-1)/2 54 | found_opening = False 55 | for isensor, sensor_survives in enumerate(surviving): 56 | if sensor_survives: 57 | opening_width += d 58 | opening_span += theta 59 | if opening_width > width: 60 | opening_center = opening_start + opening_span/2 61 | if abs(opening_center) < theta*(N_sensors-1)/4: 62 | found_opening = True 63 | else: 64 | opening_width += 0.5*d 65 | opening_span += 0.5*theta 66 | if opening_width > width: 67 | opening_center = opening_start + opening_span/2 68 | if abs(opening_center) < theta*(N_sensors-1)/4: 69 | found_opening = True 70 | opening_width = 0 71 | opening_span = 0 72 | opening_start = -theta*(N_sensors-1)/2 + isensor*theta 73 | 74 | if not found_opening: 75 | return max(0, x[idx]) 76 | 77 | return max(0, np.max(x)) 78 | 79 | def _simulate_sensor(sensor_angle, p0_point, sensor_range, obstacles): 80 | sensor_endpoint = ( 81 | p0_point.x + np.cos(sensor_angle)*sensor_range, 82 | p0_point.y + np.sin(sensor_angle)*sensor_range 83 | ) 84 | sector_ray = shapely.geometry.LineString([p0_point, sensor_endpoint]) 85 | 86 | obst_intersections = [sector_ray.intersection(elm.boundary) for elm in obstacles] 87 | obst_intersections = list(map(_standardize_intersect, obst_intersections)) 88 | obst_references = list(chain.from_iterable(repeat(obstacles[i], len(obst_intersections[i])) for i in range(len(obst_intersections)))) 89 | obst_intersections = list(chain(*obst_intersections)) 90 | 91 | if obst_intersections: 92 | measured_distance, intercept_idx = min((float(p0_point.distance(elm)), i) for i, elm in enumerate(obst_intersections)) 93 | obstacle = obst_references[intercept_idx] 94 | if not obstacle.static: 95 | obst_speed_homogenous = geom.to_homogeneous([obstacle.dx, obstacle.dy]) 96 | obst_speed_rel_homogenous = geom.Rz(-sensor_angle - np.pi/2).dot(obst_speed_homogenous) 97 | obst_speed_vec_rel = geom.to_cartesian(obst_speed_rel_homogenous) 98 | else: 99 | obst_speed_vec_rel = (0, 0) 100 | ### Thomas 101 | #obst_speed_homogenous = geom.to_homogeneous([0.0, 0.0]) 102 | #obst_speed_rel_homogenous = geom.Rz(-sensor_angle - np.pi / 2).dot(obst_speed_homogenous) 103 | #obst_speed_vec_rel = geom.to_cartesian(obst_speed_rel_homogenous) 104 | ray_blocked = True 105 | else: 106 | measured_distance = sensor_range 107 | obst_speed_vec_rel = (0, 0) 108 | ray_blocked = False 109 | 110 | return (measured_distance, obst_speed_vec_rel, ray_blocked) 111 | 112 | class Vessel(): 113 | 114 | NAVIGATION_FEATURES = [ 115 | 'surge_velocity', 116 | 'sway_velocity', 117 | 'yaw_rate', 118 | 'look_ahead_heading_error', 119 | 'heading_error', 120 | 'cross_track_error' 121 | ] 122 | 123 | def __init__(self, config:dict, init_state:np.ndarray, width:float=4) -> None: 124 | """ 125 | Initializes and resets the vessel. 126 | 127 | Parameters 128 | ---------- 129 | config : dict 130 | Dictionary containing the configuration parameters for 131 | the vessel 132 | init_state : np.ndarray 133 | The initial attitude of the veHssel [x, y, psi], where 134 | psi is the initial heading of the AUV. 135 | width : float 136 | The distance from the center of the AUV to its edge 137 | in meters. 138 | """ 139 | 140 | self.config = config 141 | 142 | # Initializing private attributes 143 | self._width = width 144 | self._feasibility_width = width*self.config["feasibility_width_multiplier"] 145 | self._n_sectors = self.config["n_sectors"] 146 | self._n_sensors = self.config["n_sensors_per_sector"]*self.config["n_sectors"] 147 | self._d_sensor_angle = 2*np.pi/(self._n_sensors) 148 | self._sensor_angles = np.array([-np.pi + (i + 1)*self._d_sensor_angle for i in range(self._n_sensors)]) 149 | self._sector_angles = [] 150 | self._n_sensors_per_sector = [0]*self._n_sectors 151 | self._sector_start_indeces = [0]*self._n_sectors 152 | self._sector_end_indeces = [0]*self._n_sectors 153 | self._sensor_internal_indeces = [] 154 | self._sensor_interval = max(1, int(1/self.config["sensor_frequency"])) 155 | self._observe_interval = max(1, int(1/self.config["observe_frequency"])) 156 | self._virtual_environment = None 157 | 158 | # Calculating sensor partitioning 159 | last_isector = -1 160 | tmp_sector_angle_sum = 0 161 | tmp_sector_sensor_count = 0 162 | for isensor in range(self._n_sensors): 163 | isector = self.config["sector_partition_fun"](self, isensor) 164 | angle = self._sensor_angles[isensor] 165 | if isector == last_isector: 166 | tmp_sector_angle_sum += angle 167 | tmp_sector_sensor_count += 1 168 | else: 169 | if last_isector > -1: 170 | self._sector_angles.append(tmp_sector_angle_sum/tmp_sector_sensor_count) 171 | last_isector = isector 172 | self._sector_start_indeces[isector] = isensor 173 | tmp_sector_angle_sum = angle 174 | tmp_sector_sensor_count = 1 175 | self._n_sensors_per_sector[isector] += 1 176 | self._sector_angles.append(tmp_sector_angle_sum/tmp_sector_sensor_count) 177 | self._sector_angles = np.array(self._sector_angles) 178 | 179 | for isensor in range(self._n_sensors): 180 | isector = self.config["sector_partition_fun"](self, isensor) 181 | isensor_internal = isensor - self._sector_start_indeces[isector] 182 | self._sensor_internal_indeces.append(isensor_internal) 183 | 184 | for isector in range(self._n_sectors): 185 | self._sector_end_indeces[isector] = self._sector_start_indeces[isector] + self._n_sensors_per_sector[isector] 186 | 187 | # Calculating feasible closeness 188 | if self.config["sensor_log_transform"]: 189 | self._get_closeness = lambda x: 1 - np.clip(np.log(1 + x)/np.log(1 + self.config["sensor_range"]), 0, 1) 190 | else: 191 | self._get_closeness = lambda x: 1 - np.clip(x/self.config["sensor_range"], 0, 1) 192 | 193 | # Initializing vessel to initial position 194 | self.reset(init_state) 195 | 196 | @property 197 | def n_sensors(self) -> int: 198 | """Number of sensors.""" 199 | return self._n_sensors 200 | 201 | @property 202 | def width(self) -> float: 203 | """Width of vessel in meters.""" 204 | return self._width 205 | 206 | @property 207 | def position(self) -> np.ndarray: 208 | """Returns an array holding the position of the AUV in cartesian 209 | coordinates.""" 210 | return self._state[0:2] 211 | 212 | @property 213 | def path_taken(self) -> np.ndarray: 214 | """Returns an array holding the path of the AUV in cartesian 215 | coordinates.""" 216 | return self._prev_states[:, 0:2] 217 | 218 | @property 219 | def heading_taken(self) -> np.ndarray: 220 | """Returns an array holding the heading of the AUV for all timesteps.""" 221 | return self._prev_states[:, 2] 222 | 223 | @property 224 | def heading(self) -> float: 225 | """Returns the heading of the AUV with respect to true north.""" 226 | return self._state[2] 227 | 228 | @property 229 | def velocity(self) -> np.ndarray: 230 | """Returns the surge and sway velocity of the AUV.""" 231 | return self._state[3:5] 232 | 233 | @property 234 | def speed(self) -> float: 235 | """Returns the speed of the AUV.""" 236 | return linalg.norm(self.velocity) 237 | 238 | @property 239 | def yaw_rate(self) -> float: 240 | """Returns the rate of rotation about the z-axis.""" 241 | return self._state[5] 242 | 243 | @property 244 | def max_speed(self) -> float: 245 | """Returns the maximum speed of the AUV.""" 246 | return 2 247 | 248 | @property 249 | def course(self) -> float: 250 | """Returns the course angle of the AUV with respect to true north.""" 251 | crab_angle = np.arctan2(self.velocity[1], self.velocity[0]) 252 | return self.heading + crab_angle 253 | 254 | @property 255 | def sensor_angles(self) -> np.ndarray: 256 | """Array containg the angles each sensor ray relative to the vessel heading.""" 257 | return self._sensor_angles 258 | 259 | @property 260 | def sector_angles(self) -> np.ndarray: 261 | """Array containg the angles of the center line of each sensor sector relative to the vessel heading.""" 262 | return self._sector_angles 263 | 264 | def reset(self, init_state:np.ndarray) -> None: 265 | """ 266 | Resets the vessel to the specified initial state. 267 | 268 | Parameters 269 | ---------- 270 | init_state : np.ndarray 271 | The initial attitude of the veHssel [x, y, psi], where 272 | psi is the initial heading of the AUV. 273 | """ 274 | init_speed = [0, 0, 0] 275 | init_state = np.array(init_state, dtype=np.float64) 276 | init_speed = np.array(init_speed, dtype=np.float64) 277 | self._state = np.hstack([init_state, init_speed]) 278 | self._prev_states = np.vstack([self._state]) 279 | self._input = [0, 0] 280 | self._prev_inputs =np.vstack([self._input]) 281 | self._last_sensor_dist_measurements = np.ones((self._n_sensors,))*self.config["sensor_range"] 282 | self._last_sensor_speed_measurements = np.zeros((self._n_sensors,2)) 283 | self._last_sector_dist_measurements = np.zeros((self._n_sectors,)) 284 | self._last_sector_feasible_dists = np.zeros((self._n_sectors,)) 285 | self._last_navi_state_dict = dict((state, 0) for state in Vessel.NAVIGATION_FEATURES) 286 | self._virtual_environment = None 287 | self._collision = False 288 | self._progress = 0 289 | self._reached_goal = False 290 | 291 | self._step_counter = 0 292 | self._perceive_counter = 0 293 | self._nearby_obstacles = [] 294 | 295 | def step(self, action:list) -> None: 296 | """ 297 | Simulates the vessel one step forward after applying the given action. 298 | 299 | Parameters 300 | ---------- 301 | action : np.ndarray[thrust_input, torque_input] 302 | """ 303 | self._input = np.array([self._thrust_surge(action[0]), self._moment_steer(action[1])]) 304 | w, q = _odesolver45(self._state_dot, self._state, self.config["t_step_size"]) 305 | 306 | self._state = q 307 | self._state[2] = geom.princip(self._state[2]) 308 | 309 | self._prev_states = np.vstack([self._prev_states,self._state]) 310 | self._prev_inputs = np.vstack([self._prev_inputs,self._input]) 311 | 312 | self._step_counter += 1 313 | 314 | def perceive(self, obstacles:list) -> (np.ndarray, np.ndarray): 315 | """ 316 | Simulates the sensor suite and returns observation arrays of the environment. 317 | 318 | Returns 319 | ------- 320 | sector_closenesses : np.ndarray 321 | sector_velocities : np.ndarray 322 | """ 323 | 324 | # Initializing variables 325 | sensor_range = self.config["sensor_range"] 326 | p0_point = shapely.geometry.Point(*self.position) 327 | 328 | # Loading nearby obstacles, i.e. obstacles within the vessel's detection range 329 | if self._step_counter % self.config["sensor_interval_load_obstacles"] == 0: 330 | self._nearby_obstacles = list(filter( 331 | lambda obst: float(p0_point.distance(obst.boundary)) - self._width < sensor_range, obstacles 332 | )) 333 | 334 | if not self._nearby_obstacles: 335 | self._last_sensor_dist_measurements = np.ones((self._n_sensors,))*sensor_range 336 | sector_feasible_distances = np.ones((self._n_sectors,))*sensor_range 337 | sector_closenesses = np.zeros((self._n_sectors,)) 338 | sector_velocities = np.zeros((2*self._n_sectors,)) 339 | collision = False 340 | 341 | else: 342 | should_observe = (self._perceive_counter % self._observe_interval == 0) or self._virtual_environment is None 343 | if should_observe: 344 | geom_targets = self._nearby_obstacles 345 | else: 346 | geom_targets = self._virtual_environment 347 | 348 | # Simulating all sensors using _simulate_sensor subroutine 349 | sensor_angles_ned = self._sensor_angles + self.heading 350 | activate_sensor = lambda i: (i % self._sensor_interval) == (self._perceive_counter % self._sensor_interval) 351 | sensor_sim_args = (p0_point, sensor_range, geom_targets) 352 | sensor_output_arrs = list(map( 353 | lambda i: _simulate_sensor(sensor_angles_ned[i], *sensor_sim_args) if activate_sensor(i) else ( 354 | self._last_sensor_dist_measurements[i], 355 | self._last_sensor_speed_measurements[i], 356 | True 357 | ), 358 | range(self._n_sensors) 359 | )) 360 | sensor_dist_measurements, sensor_speed_measurements, sensor_blocked_arr = zip(*sensor_output_arrs) 361 | sensor_dist_measurements = np.array(sensor_dist_measurements) 362 | sensor_speed_measurements = np.array(sensor_speed_measurements) 363 | self._last_sensor_dist_measurements = sensor_dist_measurements 364 | self._last_sensor_speed_measurements = sensor_speed_measurements 365 | 366 | # Setting virtual obstacle 367 | if should_observe: 368 | line_segments = [] 369 | tmp = [] 370 | for i in range(self.n_sensors): 371 | if sensor_blocked_arr[i]: 372 | point = ( 373 | self.position[0] + np.cos(sensor_angles_ned[i])*sensor_dist_measurements[i], 374 | self.position[1] + np.sin(sensor_angles_ned[i])*sensor_dist_measurements[i] 375 | ) 376 | tmp.append(point) 377 | elif len(tmp) > 1: 378 | line_segments.append(tuple(tmp)) 379 | tmp = [] 380 | 381 | self._virtual_environment = list(map(LineObstacle, line_segments)) 382 | 383 | # Partitioning sensor readings into sectors 384 | sector_dist_measurements = np.split(sensor_dist_measurements, self._sector_start_indeces[1:]) 385 | sector_speed_measurements = np.split(sensor_speed_measurements, self._sector_start_indeces[1:], axis=0) 386 | 387 | # Performing feasibility pooling 388 | sector_feasible_distances = np.array(list( 389 | map(lambda x: _feasibility_pooling(x, self._feasibility_width, self._d_sensor_angle), sector_dist_measurements) 390 | )) 391 | 392 | # Calculating feasible closeness 393 | sector_closenesses = self._get_closeness(sector_feasible_distances) 394 | 395 | # Retrieving obstacle speed for closest obstacle within each sector 396 | closest_obst_sensor_indeces = list(map(np.argmin, sector_dist_measurements)) 397 | sector_velocities = np.concatenate( 398 | [sector_speed_measurements[i][closest_obst_sensor_indeces[i]] for i in range(self._n_sectors)] 399 | ) 400 | 401 | # Testing if vessel has collided 402 | collision = np.any(sensor_dist_measurements < self.width) 403 | 404 | self._last_sector_dist_measurements = sector_closenesses 405 | self._last_sector_feasible_dists = sector_feasible_distances 406 | self._collision = collision 407 | self._perceive_counter += 1 408 | 409 | return self._get_closeness(self._last_sensor_dist_measurements.reshape(1,self.n_sensors)) 410 | #sensor_speed_x = self._last_sensor_speed_measurements[:, 0] 411 | #sensor_speed_y = self._last_sensor_speed_measurements[:, 1] 412 | #return np.vstack((self._last_sensor_dist_measurements, 413 | # sensor_speed_x, 414 | # sensor_speed_y) 415 | # ).reshape(3, self.n_sensors) 416 | 417 | def navigate(self, path:Path) -> np.ndarray: 418 | """ 419 | Calculates and returns navigation states representing the vessel's attitude 420 | with respect to the desired path. 421 | 422 | Returns 423 | ------- 424 | navigation_states : np.ndarray 425 | """ 426 | 427 | # Calculating path arclength at reference point, i.e. the point closest to the vessel 428 | vessel_arclength = path.get_closest_arclength(self.position) 429 | 430 | # Calculating tangential path direction at reference point 431 | path_direction = path.get_direction(vessel_arclength) 432 | cross_track_error = geom.Rzyx(0, 0, -path_direction).dot( 433 | np.hstack([path(vessel_arclength) - self.position, 0]) 434 | )[1] 435 | 436 | # Calculating tangential path direction at look-ahead point 437 | target_arclength = min(path.length, vessel_arclength + self.config["look_ahead_distance"]) 438 | look_ahead_path_direction = path.get_direction(target_arclength) 439 | look_ahead_heading_error = float(geom.princip(look_ahead_path_direction - self.heading)) 440 | 441 | # Calculating vector difference between look-ahead point and vessel position 442 | target_vector = path(target_arclength) - self.position 443 | 444 | # Calculating heading error 445 | target_heading = np.arctan2(target_vector[1], target_vector[0]) 446 | heading_error = float(geom.princip(target_heading - self.heading)) 447 | 448 | # Calculating path progress 449 | progress = vessel_arclength/path.length 450 | self._progress = progress 451 | 452 | # Deciding if vessel has reached the goal 453 | goal_distance = linalg.norm(path.end - self.position) 454 | reached_goal = goal_distance <= self.config["min_goal_distance"] or progress >= self.config["min_path_progress"] 455 | self._reached_goal = reached_goal 456 | 457 | # Concatenating states 458 | self._last_navi_state_dict = { 459 | 'surge_velocity': self.velocity[0], 460 | 'sway_velocity': self.velocity[1], 461 | 'yaw_rate': self.yaw_rate, 462 | 'look_ahead_heading_error': look_ahead_heading_error, 463 | 'heading_error': heading_error, 464 | 'cross_track_error': cross_track_error/100, 465 | 'target_heading': target_heading, 466 | 'look_ahead_path_direction': look_ahead_path_direction, 467 | 'path_direction': path_direction, 468 | 'vessel_arclength': vessel_arclength, 469 | 'target_arclength': target_arclength, 470 | 'goal_distance': goal_distance 471 | } 472 | navigation_states = np.array([self._last_navi_state_dict[state] for state in Vessel.NAVIGATION_FEATURES]) 473 | 474 | return navigation_states[np.newaxis, :] 475 | 476 | def req_latest_data(self) -> dict: 477 | """Returns dictionary containing the most recent perception and navigation 478 | states.""" 479 | return { 480 | 'distance_measurements': self._last_sensor_dist_measurements, 481 | 'speed_measurements': self._last_sensor_speed_measurements, 482 | 'feasible_distances': self._last_sector_feasible_dists, 483 | 'navigation': self._last_navi_state_dict, 484 | 'collision' : self._collision, 485 | 'progress': self._progress, 486 | 'reached_goal': self._reached_goal 487 | } 488 | 489 | def _state_dot(self, state): 490 | psi = state[2] 491 | nu = state[3:] 492 | 493 | tau = np.array([self._input[0], 0, self._input[1]]) 494 | 495 | eta_dot = geom.Rzyx(0, 0, geom.princip(psi)).dot(nu) 496 | nu_dot = const.M_inv.dot( 497 | tau 498 | #- const.D.dot(nu) 499 | - const.N(nu).dot(nu) 500 | ) 501 | state_dot = np.concatenate([eta_dot, nu_dot]) 502 | return state_dot 503 | 504 | def _thrust_surge(self, surge): 505 | surge = np.clip(surge, 0, 1) 506 | return surge*self.config['thrust_max_auv'] 507 | 508 | def _moment_steer(self, steer): 509 | steer = np.clip(steer, -1, 1) 510 | return steer*self.config['moment_max_auv'] -------------------------------------------------------------------------------- /gym_auv/rendering/render3d.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | 3 | import sys 4 | import math 5 | import random 6 | import time 7 | import numpy as np 8 | import logging 9 | import pywavefront 10 | from pywavefront import visualization 11 | 12 | pywavefront.configure_logging( 13 | logging.FATAL, 14 | formatter=logging.Formatter('%(name)s-%(levelname)s: %(message)s') 15 | ) 16 | 17 | from collections import deque 18 | from pyglet import image 19 | from pyglet.gl import * 20 | from pyglet.graphics import TextureGroup 21 | from pyglet.window import key, mouse 22 | import gym_auv.utils.geomutils as geom 23 | import gym_auv.envs.realworld 24 | from gym_auv.objects.obstacles import * 25 | 26 | MAX_RENDER_DISTANCE = None 27 | FOG_DISTANCE = None 28 | PAD = 50 29 | SECTOR_SIZE = 25 30 | TICKS_PER_SEC = 1 31 | CAMERA_ROTATION_SPEED = 0.002 32 | SKY_COLOR = (109/255, 173/255, 255/255, 1) 33 | ENABLE_LIGHT = True 34 | X_SHIFT = -170 35 | 36 | # OLD CODE (deprecated) 37 | # platform = pyglet.window.get_platform() 38 | # display = platform.get_default_display() 39 | 40 | display = pyglet.canvas.Display() 41 | screen = display.get_default_screen() 42 | screen_width = screen.width 43 | screen_height = screen.height 44 | 45 | WINDOW_W = screen_width 46 | WINDOW_H = screen_height 47 | 48 | counter_3d = 0 49 | 50 | VESSEL_MODEL_PATH = 'resources/shipmodels/vessel/boat.obj' 51 | TUGBOAT_MODEL_PATH = 'resources/shipmodels/tugboat/12218_tugboat_v1_L2.obj' 52 | TEXTURE_PATH = 'resources/textures.png' 53 | 54 | class Element: 55 | BLOCK = 1 56 | PLANE = 2 57 | 58 | def cube_vertices(x, z, y_00, y_01, y_10, y_11, n): 59 | """ Return the vertices of the cube at position x, y, z with size 2*n. 60 | 61 | """ 62 | 63 | return [ 64 | x-n,y_00,z-n, x-n,y_01,z+n, x+n,y_11,z+n, x+n,y_10,z-n, # top 65 | x-n,0,z-n, x+n,0,z-n, x+n,0,z+n, x-n,0,z+n, # bottom 66 | x-n,0,z-n, x-n,0,z+n, x-n,y_01,z+n, x-n,y_00,z-n, # left 67 | x+n,0,z+n, x+n,0,z-n, x+n,y_10,z-n, x+n,y_11,z+n, # right 68 | x-n,0,z+n, x+n,0,z+n, x+n,y_11,z+n, x-n,y_01,z+n, # front 69 | x+n,0,z-n, x-n,0,z-n, x-n,y_00,z-n, x+n,y_10,z-n, # back 70 | ] 71 | 72 | def plane_vertices(x, z, y_00, y_01, y_10, y_11, n):return [ 73 | x-n,y_00,z-n, x-n,y_01,z+n, x+n,y_11,z+n, x+n,y_10,z-n 74 | ] 75 | 76 | 77 | def tex_coord(x, y, n=4): 78 | """ Return the bounding vertices of the texture square. 79 | 80 | """ 81 | m = 1.0 / n 82 | dx = x * m 83 | dy = y * m 84 | return dx, dy, dx + m, dy, dx + m, dy + m, dx, dy + m 85 | 86 | 87 | def tex_coords(top, bottom, side): 88 | """ Return a list of the texture squares for the top, bottom and side. 89 | 90 | """ 91 | top = tex_coord(*top) 92 | bottom = tex_coord(*bottom) 93 | side = tex_coord(*side) 94 | result = [] 95 | result.extend(top) 96 | result.extend(bottom) 97 | result.extend(side * 4) 98 | return result 99 | 100 | DIRT_GRASS = tex_coords((1, 0), (0, 1), (0, 0)) 101 | DIRT = tex_coords((0, 1), (0, 1), (0, 1)) 102 | GRASS = tex_coords((1, 0), (1, 0), (1, 0)) 103 | SAND = tex_coords((1, 1), (1, 1), (1, 1)) 104 | BRICK = tex_coords((2, 0), (2, 0), (2, 0)) 105 | STONE = tex_coords((2, 1), (2, 1), (2, 1)) 106 | ICE = tex_coords((3, 0), (3, 0), (3, 0)) 107 | WATER = tex_coords((0.5, 2.5), (0.5, 2.5), (0.5, 2.5)) 108 | 109 | FACES = [ 110 | ( 0, 1, 0), 111 | ( 0,-1, 0), 112 | (-1, 0, 0), 113 | ( 1, 0, 0), 114 | ( 0, 0, 1), 115 | ( 0, 0,-1), 116 | ] 117 | 118 | def get_neighbours(x, y, matrix, include_diag=False): 119 | ans = set() 120 | for dx in (-1, 0, 1): 121 | for dy in (-1, 0, 1): 122 | if dx == 0 and dy == 0: 123 | continue 124 | 125 | if not include_diag: 126 | if abs(dx) == 1 and abs(dy) == 1: 127 | continue 128 | 129 | p = (x + dx, y + dy) 130 | 131 | if p[0] >= 0 and p[0] < matrix.shape[0] and p[1] >= 0 and p[1] < matrix.shape[1]: 132 | ans.add(p) 133 | 134 | return ans 135 | 136 | def normalize(position): 137 | """ Accepts `position` of arbitrary precision and returns the block 138 | containing that position. 139 | 140 | Parameters 141 | ---------- 142 | position : tuple of len 3 143 | 144 | Returns 145 | ------- 146 | block_position : tuple of ints of len 3 147 | 148 | """ 149 | x, y, z = position 150 | x, y, z = (int(round(x)), int(round(y)), int(round(z))) 151 | return (x, y, z) 152 | 153 | 154 | def sectorize(position): 155 | """ Returns a tuple representing the sector for the given `position`. 156 | 157 | Parameters 158 | ---------- 159 | position : tuple of len 3 160 | 161 | Returns 162 | ------- 163 | sector : tuple of len 3 164 | 165 | """ 166 | x, y, z = normalize(position) 167 | x, y, z = x // SECTOR_SIZE, y // SECTOR_SIZE, z // SECTOR_SIZE 168 | return (x, 0, z) 169 | 170 | 171 | class Viewer3D(object): 172 | def __init__(self, rng, width : int = WINDOW_W, height : int = WINDOW_H, autocamera=False): 173 | self.rng = rng 174 | self.autocamera = autocamera 175 | 176 | self.overlay_batch = pyglet.graphics.Batch() 177 | self.group = TextureGroup(image.load(TEXTURE_PATH).get_texture()) 178 | self.path = {} 179 | 180 | self.width = width 181 | self.height = height 182 | self.window = pyglet.window.Window(width=width, height=height) 183 | #self.window.maximize() 184 | 185 | self.xoffset = 0 186 | self.yoffset = 0 187 | self.camera_distance = max(15, self.rng.gamma(shape=1, scale=100)) 188 | self.camera_height = self.camera_distance*self.rng.rand()*0.3 189 | self.camera_angle = (-180 + 360*self.rng.rand()) 190 | 191 | if self.autocamera: 192 | self._reset_moving_camera() 193 | 194 | self.rotation = (0, 0) 195 | self.boat_models = {} 196 | 197 | self.main_batch = pyglet.graphics.Batch() 198 | self.world = {} 199 | self.terrain_shape = {} 200 | self.element_type = {} 201 | self.shown = {} 202 | self._shown = {} 203 | self.position = (0, self.camera_height, 0) 204 | self.queue = deque() 205 | 206 | self.reset_world() 207 | 208 | self.label = pyglet.text.Label('', font_size=18, 209 | x=210, y=self.height - 10, anchor_x='left', anchor_y='top', 210 | color=(0, 0, 0, 255)) 211 | 212 | glEnable(GL_BLEND) 213 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) 214 | 215 | def _reset_moving_camera(self, init_angle=None): 216 | self.camera_distance_goal = max(15, self.rng.gamma(shape=1, scale=40.0)) 217 | self.camera_height_goal = self.camera_distance_goal*self.rng.rand()*0.3 218 | self.camera_angle_goal = (-180 + 360*self.rng.rand()) 219 | self.camera_follow_vessel = bool(self.rng.rand() > 0.5) 220 | 221 | def reset_world(self): 222 | #self.queue = deque() 223 | self.sectors = {} 224 | self.sector = None 225 | 226 | def create_path(self, path): 227 | for s in np.arange(0, path.length, path.length/5000): 228 | p = path(s) 229 | self.add_element((p[1], 0, p[0]), 2*[1.0, 1.0, 1.0, 1.0], 4*[0.1,], Element.PLANE) 230 | 231 | def create_world(self, terrain, xlow, ylow, xhigh, yhigh, xoffset=0, yoffset=0): 232 | self.xoffset = xoffset 233 | self.yoffset = yoffset 234 | self.reset_world() 235 | N = (xhigh-xlow)*(yhigh-ylow) 236 | for x_terrain in range(xlow, xhigh): 237 | for y_terrain in range(ylow, yhigh): 238 | x_3dworld = x_terrain-xoffset 239 | y_3dworld = y_terrain-yoffset 240 | 241 | z = terrain[x_terrain][y_terrain] 242 | posarr = (y_3dworld, z, x_3dworld) 243 | if posarr in self.world: 244 | continue 245 | 246 | if x_terrain == xlow or x_terrain == (xhigh-1) or y_terrain == ylow or y_terrain == (yhigh-1): 247 | h_00, h_01, h_10, h_11 = (z, z, z, z) 248 | 249 | else: 250 | x, y = x_terrain, y_terrain 251 | h_00 = np.mean([z, terrain[x-1][y], terrain[x-1][y-1], terrain[x][y-1]]) 252 | h_01 = np.mean([z, terrain[x-1][y], terrain[x-1][y+1], terrain[x][y+1]]) 253 | h_10 = np.mean([z, terrain[x+1][y], terrain[x+1][y-1], terrain[x][y-1]]) 254 | h_11 = np.mean([z, terrain[x+1][y], terrain[x+1][y+1], terrain[x][y+1]]) 255 | 256 | 257 | if z<=0: # h_00 == 0 and h_10 == 0 and h_01 == 0 and h_11 == 0: 258 | t = WATER 259 | else: 260 | neighbours = get_neighbours(x_terrain, y_terrain, matrix=terrain) 261 | values = np.array([terrain[p[0]][p[1]] for p in neighbours]) 262 | 263 | if np.std(values) >= 0.4: 264 | t = DIRT 265 | 266 | else: 267 | t = ICE 268 | 269 | self.add_element(posarr, t, (h_00, h_10, h_01, h_11), Element.BLOCK) 270 | 271 | #i = (x-xlow)*(yhigh-ylow) + (y-ylow) 272 | # if i % 100 == 0: 273 | # sys.stdout.write('Creating {}x{} world ({:.2%})\r'.format((xhigh-xlow), (yhigh-ylow), i/N)) 274 | # sys.stdout.flush() 275 | 276 | def close(self): 277 | self.window.close() 278 | 279 | def update(self): 280 | 281 | """ This method is scheduled to be called repeatedly by the pyglet 282 | clock. 283 | """ 284 | self.process_queue() 285 | sector = sectorize(self.position) 286 | if sector != self.sector: 287 | self.change_sectors(self.sector, sector) 288 | if self.sector is None: 289 | self.process_entire_queue() 290 | self.sector = sector 291 | 292 | def exposed(self, position): 293 | """ Returns False is given `position` is surrounded on all 6 sides by 294 | blocks, True otherwise. 295 | 296 | """ 297 | x, y, z = position 298 | for dx, dy, dz in FACES: 299 | if (x + dx, y + dy, z + dz) not in self.world: 300 | return True 301 | return False 302 | 303 | def add_element(self, position, texture, shape, element_type): 304 | """ Add a block with the given `texture` and `position` to the world. 305 | 306 | Parameters 307 | ---------- 308 | position : tuple of len 3 309 | The (x, y, z) position of the block to add. 310 | texture : list of len 3 311 | The coordinates of the texture squares. Use `tex_coords()` to 312 | generate. 313 | 314 | """ 315 | self.terrain_shape[position] = shape 316 | self.world[position] = texture 317 | self.element_type[position] = element_type 318 | self.sectors.setdefault(sectorize(position), []).append(position) 319 | 320 | def remove_element(self, position): 321 | """ Remove the block at the given `position`. 322 | 323 | Parameters 324 | ---------- 325 | position : tuple of len 3 326 | The (x, y, z) position of the block to remove. 327 | 328 | """ 329 | del self.world[position] 330 | del self.terrain_shape[position] 331 | del self.element_type[position] 332 | self.sectors[sectorize(position)].remove(position) 333 | 334 | def show_element(self, position): 335 | """ Show the block at the given `position`. This method assumes the 336 | block has already been added with add_element() 337 | 338 | Parameters 339 | ---------- 340 | position : tuple of len 3 341 | The (x, y, z) position of the block to show. 342 | 343 | """ 344 | texture = self.world[position] 345 | shape = self.terrain_shape[position] 346 | element_type = self.element_type[position] 347 | self.shown[position] = texture 348 | self._enqueue(self._show_element, position, texture, shape, element_type) 349 | 350 | def _show_element(self, position, texture, shape, element_type): 351 | """ Private implementation of the `show_element()` method. 352 | 353 | Parameters 354 | ---------- 355 | position : tuple of len 3 356 | The (x, y, z) position of the block to show. 357 | texture : list of len 3 358 | The coordinates of the texture squares. Use `tex_coords()` to 359 | generate. 360 | 361 | """ 362 | x, y, z = position 363 | texture_data = list(texture) 364 | 365 | if element_type == Element.BLOCK: 366 | vertex_data = cube_vertices(x, z, shape[0], shape[1], shape[2], shape[3], 0.5) 367 | self._shown[position] = self.main_batch.add(24, GL_QUADS, self.group, 368 | ('v3f/static', vertex_data), 369 | ('t2f/static', texture_data)) 370 | elif element_type == Element.PLANE: 371 | vertex_data = plane_vertices(x, z, shape[0], shape[1], shape[2], shape[3], 0.1) 372 | self._shown[position] = self.overlay_batch.add(4, GL_QUADS, self.group, ('v3f/static', vertex_data), ('t2f/static', texture_data)) 373 | 374 | def hide_element(self, position): 375 | """ Hide the block at the given `position`. Hiding does not remove the 376 | block from the world. 377 | 378 | Parameters 379 | ---------- 380 | position : tuple of len 3 381 | The (x, y, z) position of the block to hide. 382 | 383 | """ 384 | self.shown.pop(position) 385 | self._enqueue(self._hide_element, position) 386 | 387 | def _hide_element(self, position): 388 | """ Private implementation of the 'hide_element()` method. 389 | 390 | """ 391 | self._shown.pop(position).delete() 392 | 393 | def show_sector(self, sector): 394 | """ Ensure all blocks in the given sector that should be shown are 395 | drawn to the canvas. 396 | 397 | """ 398 | for position in self.sectors.get(sector, []): 399 | if position not in self.shown and self.exposed(position): 400 | self.show_element(position) 401 | 402 | def hide_sector(self, sector): 403 | """ Ensure all blocks in the given sector that should be hidden are 404 | removed from the canvas. 405 | 406 | """ 407 | for position in self.sectors.get(sector, []): 408 | if position in self.shown: 409 | self.hide_element(position) 410 | 411 | def change_sectors(self, before, after): 412 | """ Move from sector `before` to sector `after`. A sector is a 413 | contiguous x, y sub-region of world. Sectors are used to speed up 414 | world rendering. 415 | 416 | """ 417 | #print('Moving from sector ', before, 'to', after) 418 | before_set = set() 419 | after_set = set() 420 | for dx in range(-PAD, PAD + 1): 421 | for dy in [0]: # range(-pad, pad + 1): 422 | for dz in range(-PAD, PAD + 1): 423 | if dx ** 2 + dy ** 2 + dz ** 2 > (PAD + 1) ** 2: 424 | continue 425 | if before: 426 | x, y, z = before 427 | before_set.add((x + dx, y + dy, z + dz)) 428 | if after: 429 | x, y, z = after 430 | after_set.add((x + dx, y + dy, z + dz)) 431 | show = after_set - before_set 432 | hide = before_set - after_set 433 | for sector in show: 434 | self.show_sector(sector) 435 | for sector in hide: 436 | self.hide_sector(sector) 437 | 438 | def _enqueue(self, func, *args): 439 | """ Add `func` to the internal queue. 440 | 441 | """ 442 | self.queue.append((func, args)) 443 | 444 | def _dequeue(self): 445 | """ Pop the top function from the internal queue and call it. 446 | 447 | """ 448 | func, args = self.queue.popleft() 449 | func(*args) 450 | 451 | def process_queue(self): 452 | """ Process the entire queue while taking periodic breaks. This allows 453 | the game loop to run smoothly. The queue contains calls to 454 | _show_element() and _hide_element() so this method should be called if 455 | add_element() or remove_element() was called with immediate=False 456 | 457 | """ 458 | start = time.clock() 459 | while self.queue and time.clock() - start < 1.0 / TICKS_PER_SEC: 460 | self._dequeue() 461 | 462 | def process_entire_queue(self): 463 | """ Process the entire queue with no breaks. 464 | 465 | """ 466 | while self.queue: 467 | self._dequeue() 468 | 469 | def __del__(self): 470 | self.close() 471 | 472 | def draw_label(self, env): 473 | global counter_3d 474 | """ Draw the label in the top left of the screen. 475 | 476 | """ 477 | # x, y, z = self.position 478 | # xrot, yrot = self.rotation 479 | self.label.text = 'Lg. λ: {:.2f}'.format(np.log10(env.rewarder.params["lambda"])) 480 | # self.label.text = '(%.2f, %.2f, %.2f) (%.2f, %.2f) %d / %d : %d' % ( 481 | # z, x, y, xrot, yrot, 482 | # len(self._shown), len(self.world), counter_3d) 483 | self.label.draw() 484 | 485 | def set_2d(self): 486 | """ Configure OpenGL to draw in 2d. 487 | 488 | """ 489 | width, height = self.window.get_size() 490 | glDisable(GL_DEPTH_TEST) 491 | #viewport = self.window.get_viewport_size() 492 | #glViewport(0, 0, max(1, viewport[0]), max(1, viewport[1])) 493 | glMatrixMode(GL_PROJECTION) 494 | glLoadIdentity() 495 | glOrtho(0, max(1, width), 0, max(1, height), -1, 1) 496 | glMatrixMode(GL_MODELVIEW) 497 | glLoadIdentity() 498 | 499 | def set_3d(self): 500 | global counter_3d 501 | """ Configure OpenGL to draw in 3d. 502 | 503 | """ 504 | width, height = self.window.get_size() 505 | glEnable(GL_DEPTH_TEST) 506 | 507 | # glLightfv(GL_LIGHT0, GL_AMBIENT, (GLfloat*4)(0,0,0,1)) 508 | # glLightfv(GL_LIGHT0, GL_DIFFUSE, (GLfloat*4)(0,0,0,1)) 509 | # glLightfv(GL_LIGHT0, GL_SPECULAR, (GLfloat*4)(1,1,1,1)) 510 | 511 | # [...] 512 | 513 | 514 | #glHint(GL_LINE_SMOOTH_HINT, GL_NICEST) 515 | #viewport = self.window.get_viewport_size() 516 | #glViewport(0, 0, max(1, viewport[0]), max(1, viewport[1])) 517 | glMatrixMode(GL_PROJECTION) 518 | glLoadIdentity() 519 | 520 | if ENABLE_LIGHT: 521 | glEnable(GL_LIGHTING) 522 | glEnable(GL_LIGHT0) 523 | lightpos = (-1.0,1.0,1.0,0) 524 | glLightfv(GL_LIGHT0, GL_DIFFUSE, (GLfloat * 4)(0.35, 0.35, 0.5, 1.0)) 525 | glLightfv(GL_LIGHT0, GL_AMBIENT, (GLfloat * 4)(0.35, 0.35, 0.5, 1.0)) 526 | #glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION, GLfloat(0.1)) 527 | #glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, GLfloat(0.1)) 528 | # glLightf(GL_LIGHT0, GL_QUADRATIC_ATTENUATION, GLfloat(0.5)) 529 | # glLightfv(GL_LIGHT0, GL_SPOT_CUTOFF, GLfloat(180.0)) 530 | 531 | # glLightfv(GL_LIGHT0, GL_SPOT_DIRECTION, (GLfloat*3)(np.sin(0.01*counter_3d), 0, np.cos(0.01*counter_3d))) 532 | # #glLightfv(GL_LIGHT0, GL_POSITION, (GLfloat*4)(*lightpos)) 533 | # glLightfv(GL_LIGHT0, GL_DIFFUSE, (GLfloat*4)(1.0,1.0,1.0,1)) 534 | # glLightfv(GL_LIGHT0, GL_SPECULAR, (GLfloat*4)(1.0,1.0,1.0,1)) 535 | # glLightfv(GL_LIGHT0, GL_SPOT_EXPONENT, GLfloat(0.01)) #*counter_3d)) 536 | # glLightfv(GL_LIGHT0, GL_CONSTANT_ATTENUATION, GLfloat(0.001*counter_3d)) 537 | glEnable(GL_COLOR_MATERIAL) 538 | glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE) 539 | glMaterialfv(GL_FRONT, GL_SHININESS, GLfloat(0.01)) 540 | 541 | #glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, (GLfloat*1)(1)) 542 | # glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, (GLfloat*4)(1,1,1,1)) 543 | 544 | gluPerspective(65.0, width / float(height), 0.1, MAX_RENDER_DISTANCE) 545 | glMatrixMode(GL_MODELVIEW) 546 | glLoadIdentity() 547 | x, y = self.rotation 548 | glRotatef(x, 0, 1, 0) 549 | 550 | 551 | glRotatef(-y, math.cos(math.radians(x)), 0, math.sin(math.radians(x))) 552 | 553 | x, y, z = self.position 554 | glTranslatef(-x, -y, -z) 555 | 556 | if ENABLE_LIGHT: 557 | glLightfv(GL_LIGHT0, GL_POSITION, (GLfloat*4)(*lightpos)) 558 | 559 | counter_3d += 1 560 | 561 | 562 | 563 | 564 | def render_env(env, mode, dt): 565 | x, y, z = (env.vessel.position[1], env._viewer3d.camera_height, env.vessel.position[0]) 566 | 567 | # env._viewer3d.add_element((x, 0, z), 2*[1.0, 1.0, 1.0, 1.0], 4*[0.1,], Element.PLANE) 568 | # env._viewer3d.show_element((x, 0, z)) 569 | 570 | camera_direction = (-env._viewer3d.camera_angle + 180)*np.pi/180 571 | camera_x = x-np.sin(camera_direction)*env._viewer3d.camera_distance 572 | camera_z = z-np.cos(camera_direction)*env._viewer3d.camera_distance 573 | 574 | try: 575 | camera_terrain_height = env.all_terrain[int(camera_z+env._viewer3d.xoffset)][int(camera_x+env._viewer3d.yoffset)] 576 | except IndexError: 577 | camera_terrain_height = 0 578 | 579 | if env._viewer3d.autocamera: 580 | if env._viewer3d.camera_follow_vessel: 581 | env._viewer3d.camera_angle_goal = -env.vessel.heading*180/np.pi + 180 582 | height_goal = max(env._viewer3d.camera_height_goal, camera_terrain_height+3) 583 | d_height = height_goal - env._viewer3d.camera_height 584 | d_distance = env._viewer3d.camera_distance_goal - env._viewer3d.camera_distance 585 | d_angle = env._viewer3d.camera_angle_goal - env._viewer3d.camera_angle 586 | 587 | if abs(d_height) < 1 or abs(d_distance) < 1: 588 | env._viewer3d._reset_moving_camera(init_angle=-env.vessel.heading*180/np.pi + 180) 589 | 590 | env._viewer3d.camera_height += dt*CAMERA_ROTATION_SPEED*d_height 591 | env._viewer3d.camera_distance += dt*CAMERA_ROTATION_SPEED*d_distance 592 | env._viewer3d.camera_angle += dt*CAMERA_ROTATION_SPEED*180/np.pi*geom.princip(d_angle*np.pi/180) 593 | camera_y = env._viewer3d.camera_height 594 | 595 | else: 596 | camera_y = max(camera_terrain_height+1, y) 597 | 598 | if env._viewer3d.camera_angle is None: 599 | env._viewer3d.camera_angle = -env.vessel.heading*180/np.pi + 180 600 | else: 601 | env._viewer3d.camera_angle += dt*CAMERA_ROTATION_SPEED*180/np.pi*geom.princip(-env.vessel.heading + np.pi - env._viewer3d.camera_angle*np.pi/180) 602 | 603 | env._viewer3d.position = (camera_x, camera_y, camera_z) 604 | env._viewer3d.rotation = (env._viewer3d.camera_angle, -180/np.pi*np.arctan2(y, env._viewer3d.camera_distance)) 605 | env._viewer3d.update() 606 | env._viewer3d.window.switch_to() 607 | env._viewer3d.window.dispatch_events() 608 | env._viewer3d.window.clear() 609 | 610 | env._viewer3d.set_3d() 611 | glColor3d(1, 1, 1) 612 | 613 | # render other vessels 614 | vessel_obstacles = (obst for obst in env.obstacles if not obst.static) 615 | for obsvessel in vessel_obstacles: 616 | glTranslatef(obsvessel.position[1], -1, obsvessel.position[0]) 617 | glRotatef(-90, 1, 0, 0) 618 | glRotatef(90 + obsvessel.heading*180/np.pi, 0, 0, 1) 619 | if (VESSEL_MODEL_PATH, obsvessel.width) not in env._viewer3d.boat_models: 620 | save_boatmodel(VESSEL_MODEL_PATH, obsvessel.width, env) 621 | visualization.draw(env._viewer3d.boat_models[(VESSEL_MODEL_PATH, obsvessel.width)]) 622 | glRotatef(-90 - obsvessel.heading*180/np.pi, 0, 0, 1) 623 | glRotatef(90, 1, 0, 0) 624 | glTranslatef(-obsvessel.position[1], 1, -obsvessel.position[0]) 625 | 626 | # render agent vessel 627 | glTranslatef(x, 0.3, z) 628 | glRotatef(-90, 1, 0, 0) 629 | glRotatef(90 + env.vessel.heading*180/np.pi, 0, 0, 1) 630 | visualization.draw(env._viewer3d.boat_models[(TUGBOAT_MODEL_PATH, env.vessel.width)]) 631 | glRotatef(-90 - env.vessel.heading*180/np.pi, 0, 0, 1) 632 | glRotatef(90, 1, 0, 0) 633 | glTranslatef(-x, -0.3, -z) 634 | 635 | env._viewer3d.main_batch.draw() 636 | 637 | if ENABLE_LIGHT: 638 | glDisable(GL_LIGHTING) 639 | env._viewer3d.overlay_batch.draw() 640 | if ENABLE_LIGHT: 641 | glEnable(GL_LIGHTING) 642 | env._viewer3d.set_2d() 643 | #env._viewer3d.draw_label(env) 644 | 645 | arr = None 646 | if mode == 'rgb_array': 647 | #glViewport(X_SHIFT, 0, WINDOW_W, WINDOW_H) 648 | image_data = pyglet.image.get_buffer_manager().get_color_buffer().get_image_data() 649 | arr = np.fromstring(image_data.get_data(), dtype=np.uint8, sep='') 650 | height = int(len(arr)/(WINDOW_W*4)) 651 | calc_size = int(height*WINDOW_W*4) 652 | if calc_size > len(arr): 653 | arr = np.pad(arr, calc_size-len(arr)) 654 | elif calc_size < len(arr): 655 | arr = arr[:calc_size] 656 | arr = arr.reshape(height, WINDOW_W, 4) 657 | arr = arr[::-1, :, 0:3] 658 | 659 | env._viewer3d.window.flip() 660 | 661 | return arr 662 | 663 | 664 | def setup_fog(): 665 | glEnable(GL_FOG) 666 | glFogfv(GL_FOG_COLOR, (GLfloat * 4)(*SKY_COLOR)) 667 | glHint(GL_FOG_HINT, GL_DONT_CARE) 668 | glFogi(GL_FOG_MODE, GL_LINEAR) 669 | glFogf(GL_FOG_START, 20.0) 670 | glFogf(GL_FOG_END, FOG_DISTANCE) 671 | 672 | def setup(): 673 | glClearColor(*SKY_COLOR) 674 | glEnable(GL_CULL_FACE) 675 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST) 676 | 677 | setup_fog() 678 | 679 | def init_env_viewer(env, autocamera=False, render_dist=1000): 680 | global MAX_RENDER_DISTANCE 681 | global FOG_DISTANCE 682 | MAX_RENDER_DISTANCE = max(1000, render_dist*2) 683 | FOG_DISTANCE = render_dist*0.8 684 | env._viewer3d = Viewer3D(env.rng, WINDOW_W, WINDOW_H, autocamera=autocamera) 685 | setup() 686 | 687 | def save_boatmodel(path, width, env): 688 | dictkey = (path, width) 689 | env._viewer3d.boat_models[dictkey] = pywavefront.Wavefront(path) 690 | vertices = [] 691 | npvertices = np.array(env._viewer3d.boat_models[dictkey].vertices) 692 | MODEL_BOAT_LENGTH = npvertices[:, 0].max() - npvertices[:, 0].min() 693 | boat_scale = 2*width/MODEL_BOAT_LENGTH 694 | for v in env._viewer3d.boat_models[dictkey].vertices: 695 | w = tuple((x*boat_scale for x in v)) 696 | vertices.append(w) 697 | env._viewer3d.boat_models[dictkey].vertices = vertices 698 | for name, material in env._viewer3d.boat_models[dictkey].materials.items(): 699 | material.vertices = tuple((x*boat_scale for x in material.vertices)) 700 | return boat_scale 701 | 702 | def init_boat_model(env): 703 | if (TUGBOAT_MODEL_PATH, env.vessel.width) not in env._viewer3d.boat_models: 704 | boat_scale = save_boatmodel(TUGBOAT_MODEL_PATH, env.vessel.width, env) 705 | #print('Initialized 3D vessel model, scale factor is {:.4f}'.format(boat_scale)) -------------------------------------------------------------------------------- /gym_auv/rendering/render2d_old.py: -------------------------------------------------------------------------------- 1 | """ 2 | 2D rendering framework. 3 | Modified version of the classical control module in OpenAI's gym. 4 | 5 | Changes: 6 | - Added an 'origin' argument to the draw_circle() and make_circle() functions to allow drawing of circles anywhere. 7 | - Added an 'outline' argument to the draw_circle() function, allows a more stylised render 8 | 9 | Created by Haakon Robinson, based on OpenAI's gym.base_env.classical.rendering.py 10 | """ 11 | 12 | import os 13 | import six 14 | import sys 15 | import pyglet 16 | from pyglet import gl 17 | import numpy as np 18 | import math 19 | from numpy import sin, cos, arctan2 20 | from gym import error 21 | import gym_auv.utils.geomutils as geom 22 | from gym_auv.objects.obstacles import CircularObstacle, PolygonObstacle, VesselObstacle 23 | 24 | if "Apple" in sys.version: 25 | if 'DYLD_FALLBACK_LIBRARY_PATH' in os.environ: 26 | os.environ['DYLD_FALLBACK_LIBRARY_PATH'] += ':/usr/lib' 27 | # (JDS 2016/04/15): avoid bug on Anaconda 2.3.0 / Yosemite 28 | 29 | STATE_W = 96 30 | STATE_H = 96 31 | VIDEO_W = 720 32 | VIDEO_H = 600 33 | WINDOW_W = VIDEO_W 34 | WINDOW_H = VIDEO_H 35 | 36 | SCALE = 5.0 # Track scale 37 | PLAYFIELD = 5000 # Game over boundary 38 | FPS = 50 39 | ZOOM = 2 # Camera ZOOM 40 | DYNAMIC_ZOOM = False 41 | CAMERA_ROTATION_SPEED = 0.02 42 | env_bg_h = int(2*PLAYFIELD) 43 | env_bg_w = int(2*PLAYFIELD) 44 | 45 | RAD2DEG = 57.29577951308232 46 | 47 | env_bg = None 48 | bg = None 49 | rot_angle = None 50 | 51 | def get_display(spec): 52 | """Convert a display specification (such as :0) into an actual Display 53 | object. 54 | 55 | Pyglet only supports multiple Displays on Linux. 56 | """ 57 | if spec is None: 58 | return None 59 | elif isinstance(spec, six.string_types): 60 | return pyglet.canvas.Display(spec) 61 | else: 62 | raise error.Error('Invalid display specification: {}. (Must be a string like :0 or None.)'.format(spec)) 63 | 64 | 65 | class Viewer2D(object): 66 | def __init__(self, width : int = VIDEO_W, height : int = VIDEO_H, display=None): 67 | display = get_display(display) 68 | 69 | self.width = width 70 | self.height = height 71 | self.window = pyglet.window.Window(width=width, height=height, display=display) 72 | self.window.on_close = self.window_closed_by_user 73 | self.isopen = True 74 | self.geoms = [] 75 | self.onetime_geoms = [] 76 | self.fixed_geoms = [] 77 | self.transform = Transform() 78 | self.camera_zoom = 1.5 79 | 80 | gl.glEnable(gl.GL_BLEND) 81 | gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA) 82 | 83 | def close(self): 84 | self.window.close() 85 | 86 | def window_closed_by_user(self): 87 | self.isopen = False 88 | 89 | def set_bounds(self, left, right, bottom, top): 90 | assert right > left and top > bottom 91 | scalex = self.width/(right-left) 92 | scaley = self.height/(top-bottom) 93 | self.transform = Transform( 94 | translation=(-left*scalex, -bottom*scaley), 95 | scale=(scalex, scaley)) 96 | 97 | def add_geom(self, geom): 98 | self.geoms.append(geom) 99 | 100 | def add_onetime(self, geom): 101 | self.onetime_geoms.append(geom) 102 | 103 | def add_fixed(self, geom): 104 | self.fixed_geoms.append(geom) 105 | 106 | def render(self, return_rgb_array=False): 107 | gl.glClearColor(1, 1, 1, 1) 108 | self.window.clear() 109 | self.window.switch_to() 110 | self.window.dispatch_events() 111 | self.transform.enable() 112 | for geom in self.geoms: 113 | geom.render() 114 | for geom in self.onetime_geoms: 115 | geom.render() 116 | self.transform.disable() 117 | for geom in self.fixed_geoms: 118 | geom.render() 119 | arr = None 120 | if return_rgb_array: 121 | buffer = pyglet.image.get_buffer_manager().get_color_buffer() 122 | image_data = buffer.get_image_data() 123 | arr = np.fromstring(image_data.data, dtype=np.uint8, sep='') 124 | arr = arr.reshape(buffer.height, buffer.width, 4) 125 | arr = arr[::-1,:,0:3] 126 | self.window.flip() 127 | self.onetime_geoms = [] 128 | return arr if return_rgb_array else self.isopen 129 | 130 | def draw_circle(self, origin=(0,0), radius=10, res=30, filled=True, outline=True, start_angle=0, end_angle=2*np.pi, **attrs): 131 | geom = make_circle(origin=origin, radius=radius, res=res, filled=filled, start_angle=start_angle, end_angle=end_angle) 132 | _add_attrs(geom, attrs) 133 | self.add_onetime(geom) 134 | if filled and outline: 135 | outl = make_circle(origin=origin, radius=radius, res=res, filled=False) 136 | _add_attrs(outl, {'color': (0,0,0), 'linewidth': 1}) 137 | self.add_onetime(outl) 138 | return geom 139 | 140 | def draw_polygon(self, v, filled=True, **attrs): 141 | geom = make_polygon(v=v, filled=filled) 142 | _add_attrs(geom, attrs) 143 | self.add_onetime(geom) 144 | return geom 145 | 146 | def draw_polyline(self, v, **attrs): 147 | geom = make_polyline(v=v) 148 | _add_attrs(geom, attrs) 149 | 150 | self.add_onetime(geom) 151 | return geom 152 | 153 | def draw_line(self, start, end, **attrs): 154 | geom = Line(start, end) 155 | _add_attrs(geom, attrs) 156 | self.add_onetime(geom) 157 | return geom 158 | 159 | def get_array(self): 160 | self.window.flip() 161 | image_data = pyglet.image.get_buffer_manager().get_color_buffer().get_image_data() 162 | self.window.flip() 163 | arr = np.fromstring(image_data.data, dtype=np.uint8, sep='') 164 | arr = arr.reshape(self.height, self.width, 4) 165 | return arr[::-1,:,0:3] 166 | 167 | def transform_vertices(self, points, translation, rotation, scale=1): 168 | res = [] 169 | for p in points: 170 | res.append(( 171 | cos(rotation) * p[0] * scale - sin(rotation) * p[1] * scale + translation[0], 172 | sin(rotation) * p[0] * scale + cos(rotation) * p[1] * scale + translation[1])) 173 | return res 174 | 175 | def draw_arrow(self, base, angle, length, **attrs): 176 | TRIANGLE_POLY = ((-1, -1), (1, -1), (0, 1)) 177 | head = (base[0] + length * cos(angle), base[1] + length * sin(angle)) 178 | tri = self.transform_vertices(TRIANGLE_POLY, head, angle - np.pi / 2, scale=0.7) 179 | self.draw_polyline([base, head], linewidth=2, **attrs) 180 | self.draw_polygon(tri, **attrs) 181 | 182 | def draw_shape(self, vertices, position=None, angle=None, color=(1, 1, 1), filled=True, border=True): 183 | if (position is not None): 184 | poly_path = self.transform_vertices(vertices, position, angle) 185 | else: 186 | poly_path = vertices 187 | if (filled): 188 | self.draw_polygon(poly_path + [poly_path[0]], color=color) 189 | if (border): 190 | border_color = (0, 0, 0) if type(border) == bool else border 191 | self.draw_polyline(poly_path + [poly_path[0]], linewidth=1, color=border_color if filled else color) 192 | 193 | def __del__(self): 194 | self.close() 195 | 196 | 197 | def _add_attrs(geom, attrs): 198 | if "color" in attrs: 199 | geom.set_color(*attrs["color"]) 200 | if "linewidth" in attrs: 201 | geom.set_linewidth(attrs["linewidth"]) 202 | 203 | 204 | class Geom(object): 205 | def __init__(self): 206 | self._color=Color((0, 0, 0, 1.0)) 207 | self.attrs = [self._color] 208 | 209 | def render(self): 210 | for attr in reversed(self.attrs): 211 | attr.enable() 212 | self.render1() 213 | for attr in self.attrs: 214 | attr.disable() 215 | 216 | def render1(self): 217 | raise NotImplementedError 218 | 219 | def add_attr(self, attr): 220 | self.attrs.append(attr) 221 | 222 | def set_color(self, r, g, b, alpha=1): 223 | self._color.vec4 = (r, g, b, alpha) 224 | 225 | 226 | class Attr(object): 227 | def enable(self): 228 | raise NotImplementedError 229 | 230 | def disable(self): 231 | pass 232 | 233 | 234 | class Transform(Attr): 235 | def __init__(self, translation=(0.0, 0.0), rotation=0.0, scale=(1,1)): 236 | self.set_translation(*translation) 237 | self.set_rotation(rotation) 238 | self.set_scale(*scale) 239 | 240 | def enable(self): 241 | gl.glPushMatrix() 242 | gl.glTranslatef(self.translation[0], self.translation[1], 0) # translate to GL loc ppint 243 | gl.glRotatef(RAD2DEG * self.rotation, 0, 0, 1.0) 244 | gl.glScalef(self.scale[0], self.scale[1], 1) 245 | 246 | def disable(self): 247 | gl.glPopMatrix() 248 | 249 | def set_translation(self, newx, newy): 250 | self.translation = (float(newx), float(newy)) 251 | 252 | def set_rotation(self, new): 253 | self.rotation = float(new) 254 | 255 | def set_scale(self, newx, newy): 256 | self.scale = (float(newx), float(newy)) 257 | 258 | 259 | class Color(Attr): 260 | def __init__(self, vec4): 261 | self.vec4 = vec4 262 | 263 | def enable(self): 264 | gl.glColor4f(*self.vec4) 265 | 266 | 267 | class LineStyle(Attr): 268 | def __init__(self, style): 269 | self.style = style 270 | 271 | def enable(self): 272 | gl.glEnable(gl.GL_LINE_STIPPLE) 273 | gl.glLineStipple(1, self.style) 274 | 275 | def disable(self): 276 | gl.glDisable(gl.GL_LINE_STIPPLE) 277 | 278 | 279 | class LineWidth(Attr): 280 | def __init__(self, stroke): 281 | self.stroke = stroke 282 | 283 | def enable(self): 284 | gl.glLineWidth(self.stroke) 285 | 286 | 287 | class Point(Geom): 288 | def __init__(self): 289 | Geom.__init__(self) 290 | 291 | def render1(self): 292 | gl.glBegin(gl.GL_POINTS) # draw point 293 | gl.glVertex3f(0.0, 0.0, 0.0) 294 | gl.glEnd() 295 | 296 | 297 | class FilledPolygon(Geom): 298 | def __init__(self, v): 299 | Geom.__init__(self) 300 | self.v = v 301 | 302 | def render1(self): 303 | if len(self.v) == 4 : gl.glBegin(gl.GL_QUADS) 304 | elif len(self.v) > 4 : gl.glBegin(gl.GL_POLYGON) 305 | else: gl.glBegin(gl.GL_TRIANGLES) 306 | for p in self.v: 307 | gl.glVertex3f(p[0], p[1],0) # draw each vertex 308 | gl.glEnd() 309 | 310 | 311 | def make_circle(origin=(0,0), radius=10, res=30, filled=True, start_angle=0, end_angle=2*np.pi, return_points=False): 312 | points = [] 313 | for i in range(res+1): 314 | ang = start_angle + i*(end_angle - start_angle)/res 315 | points.append((math.cos(ang)*radius + origin[0], math.sin(ang)*radius + origin[1])) 316 | if (return_points): 317 | return points 318 | else: 319 | if filled: 320 | return FilledPolygon(points) 321 | else: 322 | return PolyLine(points, True) 323 | 324 | 325 | def make_polygon(v, filled=True): 326 | if filled: return FilledPolygon(v) 327 | else: return PolyLine(v, True) 328 | 329 | 330 | def make_polyline(v): 331 | return PolyLine(v, False) 332 | 333 | 334 | def make_capsule(length, width): 335 | l, r, t, b = 0, length, width/2, -width/2 336 | box = make_polygon([(l,b), (l,t), (r,t), (r,b)]) 337 | circ0 = make_circle(width/2) 338 | circ1 = make_circle(width/2) 339 | circ1.add_attr(Transform(translation=(length, 0))) 340 | geom = Compound([box, circ0, circ1]) 341 | return geom 342 | 343 | 344 | class Compound(Geom): 345 | def __init__(self, gs): 346 | Geom.__init__(self) 347 | self.gs = gs 348 | for g in self.gs: 349 | g.attrs = [a for a in g.attrs if not isinstance(a, Color)] 350 | 351 | def render1(self): 352 | for g in self.gs: 353 | g.render() 354 | 355 | 356 | class PolyLine(Geom): 357 | def __init__(self, v, close): 358 | Geom.__init__(self) 359 | self.v = v 360 | self.close = close 361 | self.linewidth = LineWidth(1) 362 | self.add_attr(self.linewidth) 363 | 364 | def render1(self): 365 | gl.glBegin(gl.GL_LINE_LOOP if self.close else gl.GL_LINE_STRIP) 366 | for p in self.v: 367 | gl.glVertex3f(p[0], p[1],0) # draw each vertex 368 | gl.glEnd() 369 | 370 | def set_linewidth(self, x): 371 | self.linewidth.stroke = x 372 | 373 | 374 | class Line(Geom): 375 | def __init__(self, start=(0.0, 0.0), end=(0.0, 0.0), linewidth=1): 376 | Geom.__init__(self) 377 | self.start = start 378 | self.end = end 379 | self.linewidth = LineWidth(linewidth) 380 | self.add_attr(self.linewidth) 381 | 382 | def render1(self): 383 | gl.glBegin(gl.GL_LINES) 384 | gl.glVertex2f(*self.start) 385 | gl.glVertex2f(*self.end) 386 | gl.glEnd() 387 | 388 | 389 | class Image(Geom): 390 | def __init__(self, fname, width, height): 391 | Geom.__init__(self) 392 | self.width = width 393 | self.height = height 394 | img = pyglet.image.load(fname) 395 | self.img = img 396 | self.flip = False 397 | 398 | def render1(self): 399 | self.img.blit(-self.width/2, -self.height/2, width=self.width, height=self.height) 400 | 401 | 402 | def _render_path(env): 403 | env._viewer2d.draw_polyline(env.path._points, linewidth=1, color=(0.3, 1.0, 0.3)) 404 | 405 | def _render_vessel(env): 406 | env._viewer2d.draw_polyline(env.vessel.path_taken, linewidth=1, color=(0.8, 0, 0)) # previous positions 407 | vertices = [ 408 | (-env.vessel.width/2, -env.vessel.width/2), 409 | (-env.vessel.width/2, env.vessel.width/2), 410 | (env.vessel.width/2, env.vessel.width/2), 411 | (3/2*env.vessel.width, 0), 412 | (env.vessel.width/2, -env.vessel.width/2), 413 | ] 414 | 415 | env._viewer2d.draw_shape(vertices, env.vessel.position, env.vessel.heading, color=(0, 0, 0.8)) 416 | 417 | def _render_interceptions(env): 418 | for t, obst_intercept_array in enumerate(env.sensor_obst_intercepts_transformed_hist): 419 | for obst_intercept in obst_intercept_array: 420 | env._viewer2d.draw_circle(origin=obst_intercept, radius=1.0 - t/len(env.sensor_obst_intercepts_transformed_hist), res=30, color=(0.3, 1.0 - t/len(env.sensor_obst_intercepts_transformed_hist), 0.3)) 421 | 422 | def _render_sensors(env): 423 | for isensor, sensor_angle in enumerate(env.vessel._sensor_angles): 424 | isector = env.config["sector_partition_fun"](env, isensor) # isensor // env.config["n_sensors_per_sector"] 425 | distance = env.vessel._last_sensor_dist_measurements[isensor] 426 | p0 = env.vessel.position 427 | p1 = ( 428 | p0[0] + np.cos(sensor_angle+env.vessel.heading)*distance, 429 | p0[1] + np.sin(sensor_angle+env.vessel.heading)*distance 430 | ) 431 | 432 | closeness = env.vessel._last_sector_dist_measurements[isector] 433 | redness = 0.5 + 0.5*max(0, closeness) 434 | greenness = 1 - max(0, closeness) 435 | blueness = 0.5 if abs(isector - int(np.floor(env.config["n_sectors"]/2) + 1)) % 2 == 0 and not env.config["sensor_rotation"] else 1 436 | alpha = 0.5 437 | env._viewer2d.draw_line(p0, p1, color=(redness, greenness, blueness, alpha)) 438 | 439 | def _render_progress(env): 440 | ref_point = env.path(env.vessel._last_navi_state_dict['vessel_arclength']).flatten() 441 | env._viewer2d.draw_circle(origin=ref_point, radius=1, res=30, color=(0.8, 0.3, 0.3)) 442 | 443 | target_point = env.path(env.vessel._last_navi_state_dict['target_arclength']).flatten() 444 | env._viewer2d.draw_circle(origin=target_point, radius=1, res=30, color=(0.3, 0.8, 0.3)) 445 | 446 | def _render_obstacles(env): 447 | for i, obst in enumerate(env.obstacles): 448 | c = (0.8, 0.8, 0.8) 449 | 450 | if isinstance(obst, CircularObstacle): 451 | env._viewer2d.draw_circle(obst.position, obst.radius, color=c) 452 | 453 | elif isinstance(obst, PolygonObstacle): 454 | env._viewer2d.draw_shape(obst.points, color=c) 455 | 456 | elif isinstance(obst, VesselObstacle): 457 | env._viewer2d.draw_shape(list(obst.boundary.exterior.coords), color=c) 458 | 459 | 460 | def _render_tiles(env, win): 461 | global env_bg 462 | global bg 463 | 464 | if env_bg is None: 465 | # Initialise background 466 | from pyglet.gl.gl import GLubyte 467 | data = np.zeros((env_bg_h, env_bg_w, 3)) 468 | k = env_bg_h//100 469 | for x in range(0, data.shape[0], k): 470 | for y in range(0, data.shape[1], k): 471 | data[x:x+k, y:y+k, :] = np.array(( 472 | int(255*min(1.0, 0.3 + 0.025 * (np.random.random() - 0.5))), 473 | int(255*min(1.0, 0.7 + 0.025 * (np.random.random() - 0.5))), 474 | int(255*min(1.0, 0.8 + 0.025 * (np.random.random() - 0.5))) 475 | )) 476 | 477 | pixels = data.flatten().astype('int').tolist() 478 | raw_data = (GLubyte * len(pixels))(*pixels) 479 | bg = pyglet.image.ImageData(width=env_bg_w, height=env_bg_h, format='RGB', data=raw_data) 480 | if not os.path.exists('./resources'): 481 | os.mkdir('./resources') 482 | bg.save('./resources/bg.png') 483 | env_bg = pyglet.sprite.Sprite(bg, x=env.vessel.position[0] - env_bg_w/2, y=env.vessel.position[1] - env_bg_h/2) 484 | env_bg.scale = 1 485 | 486 | if env.t_step % 250 == 0: 487 | env_bg = pyglet.sprite.Sprite(bg, x=env.vessel.position[0] - env_bg_w/2, y=env.vessel.position[1] - env_bg_h/2) 488 | env_bg.scale = 1 489 | 490 | env_bg.draw() 491 | 492 | def _render_indicators(env, W, H): 493 | 494 | prog = W/40.0 495 | h = H/40.0 496 | gl.glBegin(gl.GL_QUADS) 497 | gl.glColor4f(0,0,0,1) 498 | gl.glVertex3f(W, 0, 0) 499 | gl.glVertex3f(W, 5*h, 0) 500 | gl.glVertex3f(0, 5*h, 0) 501 | gl.glVertex3f(0, 0, 0) 502 | gl.glEnd() 503 | 504 | env._viewer2d.reward_text_field.text = "Current Reward:" 505 | env._viewer2d.reward_text_field.draw() 506 | # env._viewer2d.reward_value_field.text = "{:2.3f} * {:2.3f} * {:2.3f} - {:2.1f} = {:2.3f}".format(env.rewarder.speed_term, env.rewarder.heading_term, env.rewarder.cte_term, env.rewarder.living_penalty, env.last_reward) 507 | env._viewer2d.reward_value_field.text = "{:2.3f}".format(env.last_reward) 508 | env._viewer2d.reward_value_field.draw() 509 | 510 | env._viewer2d.cum_reward_text_field.text = "Cumulative Reward:" 511 | env._viewer2d.cum_reward_text_field.draw() 512 | env._viewer2d.cum_reward_value_field.text = "{:2.3f}".format(env.cumulative_reward) 513 | env._viewer2d.cum_reward_value_field.draw() 514 | 515 | env._viewer2d.time_step_text_field.text = "Time Step:" 516 | env._viewer2d.time_step_text_field.draw() 517 | env._viewer2d.time_step_value_field.text = str(env.t_step) 518 | env._viewer2d.time_step_value_field.draw() 519 | 520 | env._viewer2d.episode_text_field.text = "Episode:" 521 | env._viewer2d.episode_text_field.draw() 522 | env._viewer2d.episode_value_field.text = str(env.episode) 523 | env._viewer2d.episode_value_field.draw() 524 | 525 | env._viewer2d.lambda_text_field.text = "Speed:" 526 | env._viewer2d.lambda_text_field.draw() 527 | env._viewer2d.lambda_value_field.text = "{:2.2f} (max. {:2.2f})".format(env.rewarder._vessel.speed, 528 | env.rewarder._vessel.max_speed) 529 | env._viewer2d.lambda_value_field.draw() 530 | 531 | env._viewer2d.eta_text_field.text = "CTE:" 532 | env._viewer2d.eta_text_field.draw() 533 | env._viewer2d.eta_value_field.text = "{:2.2f}".format( 534 | env.rewarder._vessel.req_latest_data()['navigation']['cross_track_error']) 535 | env._viewer2d.eta_value_field.draw() 536 | 537 | env._viewer2d.input_text_field.text = "Input:" 538 | env._viewer2d.input_text_field.draw() 539 | env._viewer2d.input_value_field.text = "Thrust: {:2.2f} [N], Moment: {:2.2f} [Nm]".format(env.vessel._input[0], 540 | env.vessel._input[1]) 541 | env._viewer2d.input_value_field.draw() 542 | 543 | def render_env(env, mode): 544 | global rot_angle 545 | 546 | def render_objects(): 547 | t = env._viewer2d.transform 548 | t.enable() 549 | _render_sensors(env) 550 | #_render_interceptions(env) 551 | if env.path is not None: 552 | _render_path(env) 553 | _render_vessel(env) 554 | _render_tiles(env, win) 555 | _render_obstacles(env) 556 | if env.path is not None: 557 | _render_progress(env) 558 | #_render_interceptions(env) 559 | 560 | # Visualise path error (DEBUGGING) 561 | # p = np.array(env.vessel.position) 562 | # dir = rotate(env.past_obs[-1][0:2], env.vessel.heading) 563 | # env._viewer2d.draw_line(p, p + 10*np.array(dir), color=(0.8, 0.3, 0.3)) 564 | 565 | for geom in env._viewer2d.onetime_geoms: 566 | geom.render() 567 | 568 | t.disable() 569 | 570 | if env.config["show_indicators"]: 571 | _render_indicators(env, WINDOW_W, WINDOW_H) 572 | 573 | scroll_x = env.vessel.position[0] 574 | scroll_y = env.vessel.position[1] 575 | ship_angle = -env.vessel.heading + np.pi/2 576 | if (rot_angle is None): 577 | rot_angle = ship_angle 578 | else: 579 | rot_angle += CAMERA_ROTATION_SPEED * geom.princip(ship_angle - rot_angle) 580 | 581 | if DYNAMIC_ZOOM: 582 | if (int(env.t_step/1000) % 2 == 0): 583 | env._viewer2d.camera_zoom = 0.999*env._viewer2d.camera_zoom + 0.001*(ZOOM - env._viewer2d.camera_zoom) 584 | else: 585 | env._viewer2d.camera_zoom = 0.999*env._viewer2d.camera_zoom + 0.001*(1 - env._viewer2d.camera_zoom) 586 | 587 | env._viewer2d.transform.set_scale(env._viewer2d.camera_zoom, env._viewer2d.camera_zoom) 588 | env._viewer2d.transform.set_translation( 589 | WINDOW_W/2 - (scroll_x*env._viewer2d.camera_zoom*cos(rot_angle) - scroll_y*env._viewer2d.camera_zoom*sin(rot_angle)), 590 | WINDOW_H/2 - (scroll_x*env._viewer2d.camera_zoom*sin(rot_angle) + scroll_y*env._viewer2d.camera_zoom*cos(rot_angle)) 591 | ) 592 | env._viewer2d.transform.set_rotation(rot_angle) 593 | 594 | win = env._viewer2d.window 595 | win.switch_to() 596 | x = win.dispatch_events() 597 | win.clear() 598 | gl.glViewport(0, 0, WINDOW_W, WINDOW_H) 599 | render_objects() 600 | arr = None 601 | 602 | if mode == 'rgb_array': 603 | image_data = pyglet.image.get_buffer_manager().get_color_buffer().get_image_data() 604 | arr = np.fromstring(image_data.get_data(), dtype=np.uint8, sep='') 605 | arr = arr.reshape(WINDOW_H, WINDOW_W, 4) 606 | arr = arr[::-1, :, 0:3] 607 | 608 | win.flip() 609 | 610 | env._viewer2d.onetime_geoms = [] 611 | 612 | return arr 613 | 614 | def init_env_viewer(env): 615 | env._viewer2d = Viewer2D(int(VIDEO_W), int(VIDEO_H)) 616 | 617 | env._viewer2d.reward_text_field = pyglet.text.Label('0000', font_size=10, 618 | x=20, y=WINDOW_H - 30.00, anchor_x='left', anchor_y='center', 619 | color=(0, 0, 0, 255)) 620 | env._viewer2d.reward_value_field = pyglet.text.Label('0000', font_size=10, 621 | x=260, y=WINDOW_H - 30.00, anchor_x='right', anchor_y='center', 622 | color=(0, 0, 0, 255)) 623 | 624 | env._viewer2d.cum_reward_text_field = pyglet.text.Label('0000', font_size=10, 625 | x=20, y=WINDOW_H - 50.00, anchor_x='left', 626 | anchor_y='center', 627 | color=(0, 0, 0, 255)) 628 | env._viewer2d.cum_reward_value_field = pyglet.text.Label('0000', font_size=10, 629 | x=260, y=WINDOW_H - 50.00, anchor_x='right', 630 | anchor_y='center', 631 | color=(0, 0, 0, 255)) 632 | 633 | env._viewer2d.time_step_text_field = pyglet.text.Label('0000', font_size=10, 634 | x=20, y=WINDOW_H - 70.00, anchor_x='left', anchor_y='center', 635 | color=(0, 0, 0, 255)) 636 | env._viewer2d.time_step_value_field = pyglet.text.Label('0000', font_size=10, 637 | x=260, y=WINDOW_H - 70.00, anchor_x='right', 638 | anchor_y='center', 639 | color=(0, 0, 0, 255)) 640 | 641 | env._viewer2d.episode_text_field = pyglet.text.Label('0000', font_size=10, 642 | x=20, y=WINDOW_H - 90.00, anchor_x='left', anchor_y='center', 643 | color=(0, 0, 0, 255)) 644 | env._viewer2d.episode_value_field = pyglet.text.Label('0000', font_size=10, 645 | x=260, y=WINDOW_H - 90.00, anchor_x='right', 646 | anchor_y='center', 647 | color=(0, 0, 0, 255)) 648 | 649 | env._viewer2d.lambda_text_field = pyglet.text.Label('0000', font_size=10, 650 | x=20, y=WINDOW_H - 110.00, anchor_x='left', anchor_y='center', 651 | color=(0, 0, 0, 255)) 652 | env._viewer2d.lambda_value_field = pyglet.text.Label('0000', font_size=10, 653 | x=260, y=WINDOW_H - 110.00, anchor_x='right', 654 | anchor_y='center', 655 | color=(0, 0, 0, 255)) 656 | 657 | env._viewer2d.eta_text_field = pyglet.text.Label('0000', font_size=10, 658 | x=20, y=WINDOW_H - 130.00, anchor_x='left', anchor_y='center', 659 | color=(0, 0, 0, 255)) 660 | env._viewer2d.eta_value_field = pyglet.text.Label('0000', font_size=10, 661 | x=260, y=WINDOW_H - 130.00, anchor_x='right', anchor_y='center', 662 | color=(0, 0, 0, 255)) 663 | 664 | env._viewer2d.input_text_field = pyglet.text.Label('0000', font_size=10, 665 | x=20, y=WINDOW_H - 150.00, anchor_x='left', anchor_y='center', 666 | color=(0, 0, 0, 255)) 667 | env._viewer2d.input_value_field = pyglet.text.Label('0000', font_size=10, 668 | x=360, y=WINDOW_H - 150.00, anchor_x='right', anchor_y='center', 669 | color=(0, 0, 0, 255)) 670 | 671 | print('Initialized 2D viewer') 672 | --------------------------------------------------------------------------------