From eeb60445a48e83659fd8b000aa22e242af18308c Mon Sep 17 00:00:00 2001 From: YuechengLiu Date: Tue, 30 Mar 2021 20:53:06 +0800 Subject: [PATCH 1/4] quadrotor gym compatibility --- rlschool/quadrotor/env.py | 75 +++++------ rlschool/quadrotor/generate_default_map.py | 4 +- rlschool/quadrotor/quadrotorsim.py | 25 ++-- rlschool/quadrotor/render.py | 150 +++++++++++---------- rlschool/quadrotor/utils.py | 78 ++++++++++- 5 files changed, 202 insertions(+), 130 deletions(-) diff --git a/rlschool/quadrotor/env.py b/rlschool/quadrotor/env.py index 14f4a79..6957ceb 100644 --- a/rlschool/quadrotor/env.py +++ b/rlschool/quadrotor/env.py @@ -16,6 +16,7 @@ import numpy as np from math import floor, ceil from collections import namedtuple +import gym from rlschool.quadrotor.quadrotorsim import QuadrotorSim @@ -26,7 +27,7 @@ NO_DISPLAY = True -class Quadrotor(object): +class Quadrotor(gym.Env): """ Quadrotor environment. @@ -41,6 +42,7 @@ class Quadrotor(object): map is a 100x100 flatten floor. simulator_conf (None|str): path to simulator config xml file. """ + def __init__(self, dt=0.01, nt=1000, @@ -51,11 +53,12 @@ def __init__(self, healthy_reward=1.0, **kwargs): # TODO: other possible tasks: precision_landing - assert task in ['velocity_control', 'no_collision', - 'hovering_control'], 'Invalid task setting' + assert task in [ + 'velocity_control', 'no_collision', 'hovering_control' + ], 'Invalid task setting' if simulator_conf is None: - simulator_conf = os.path.join(os.path.dirname(__file__), - 'config.json') + simulator_conf = os.path.join( + os.path.dirname(__file__), 'config.json') assert os.path.exists(simulator_conf), \ 'Simulator config file does not exist' @@ -68,13 +71,11 @@ def __init__(self, cfg_dict = self.simulator.get_config(simulator_conf) self.valid_range = cfg_dict['range'] - self.action_space = namedtuple( - 'action_space', ['shape', 'high', 'low', 'sample']) - self.action_space.shape = [4] - self.action_space.high = [cfg_dict['action_space_high']] * 4 - self.action_space.low = [cfg_dict['action_space_low']] * 4 - self.action_space.sample = Quadrotor.random_action( - cfg_dict['action_space_low'], cfg_dict['action_space_high'], 4) + self.action_space = gym.spaces.Box( + low=np.array([cfg_dict['action_space_low']] * 4, dtype='float32'), + high=np.array( + [cfg_dict['action_space_high']] * 4, dtype='float32'), + shape=[4]) self.body_velocity_keys = ['b_v_x', 'b_v_y', 'b_v_z'] self.body_position_keys = ['b_x', 'b_y', 'b_z'] @@ -91,8 +92,7 @@ def __init__(self, len(self.flight_pose_keys) + len(self.barometer_keys) if self.task == 'velocity_control': obs_dim += len(self.task_velocity_control_keys) - self.observation_space = namedtuple('observation_space', ['shape']) - self.observation_space.shape = [obs_dim] + self.observation_space = gym.Space(shape=[obs_dim], dtype='float32') self.state = {} self.viewer = None @@ -132,13 +132,17 @@ def step(self, action): sensor_dict = self.simulator.get_sensor() state_dict = self.simulator.get_state() - old_pos = [self.simulator.global_position[0] + self.x_offset, - self.simulator.global_position[1] + self.y_offset, - self.simulator.global_position[2] + self.z_offset] + old_pos = [ + self.simulator.global_position[0] + self.x_offset, + self.simulator.global_position[1] + self.y_offset, + self.simulator.global_position[2] + self.z_offset + ] self._update_state(sensor_dict, state_dict) - new_pos = [self.simulator.global_position[0] + self.x_offset, - self.simulator.global_position[1] + self.y_offset, - self.simulator.global_position[2] + self.z_offset] + new_pos = [ + self.simulator.global_position[0] + self.x_offset, + self.simulator.global_position[1] + self.y_offset, + self.simulator.global_position[2] + self.z_offset + ] if self.task in ['no_collision', 'hovering_control']: is_collision = self._check_collision(old_pos, new_pos) reward = self._get_reward(collision=is_collision) @@ -166,10 +170,11 @@ def render(self): if self.viewer is None: if NO_DISPLAY: raise RuntimeError('[Error] Cannot connect to display screen.') - self.viewer = RenderWindow(task=self.task, - x_offset=self.x_offset, - y_offset=self.y_offset, - z_offset=self.z_offset) + self.viewer = RenderWindow( + task=self.task, + x_offset=self.x_offset, + y_offset=self.y_offset, + z_offset=self.z_offset) if 'z' not in self.state: # It's null state @@ -178,8 +183,9 @@ def render(self): state = self._get_state_for_viewer() if self.task == 'velocity_control': self.viewer.view( - state, self.dt, - expected_velocity=self.velocity_targets[self.ct-1]) + state, + self.dt, + expected_velocity=self.velocity_targets[self.ct - 1]) else: self.viewer.view(state, self.dt) @@ -210,7 +216,7 @@ def _get_reward(self, collision=False, velocity_target=(0.0, 0.0, 0.0)): """ # Make sure energy cost always smaller than healthy reward, # to encourage longer running - reward = - min(self.dt * self.simulator.power, self.healthy_reward) + reward = -min(self.dt * self.simulator.power, self.healthy_reward) if self.task == 'no_collision': task_reward = 0.0 if collision else self.healthy_reward reward += task_reward @@ -220,8 +226,7 @@ def _get_reward(self, collision=False, velocity_target=(0.0, 0.0, 0.0)): elif self.task == 'hovering_control': task_reward = 0.0 if collision else self.healthy_reward - velocity_norm = np.linalg.norm( - self.simulator.global_velocity) + velocity_norm = np.linalg.norm(self.simulator.global_velocity) angular_velocity_norm = np.linalg.norm( self.simulator.body_angular_velocity) @@ -249,7 +254,7 @@ def _check_collision(self, old_pos, new_pos): y_min, y_max = min_max(old_pos, new_pos, 1) z_min, z_max = min_max(old_pos, new_pos, 2) - taken_pos = self.map_matrix[y_min:y_max+1, x_min:x_max+1] + taken_pos = self.map_matrix[y_min:y_max + 1, x_min:x_max + 1] if z_min < np.any(taken_pos) or z_max < np.any(taken_pos): return True else: @@ -263,7 +268,7 @@ def _update_state(self, sensor, state): self.state[k] = v if self.task == 'velocity_control': - t = min(self.ct, self.nt-1) + t = min(self.ct, self.nt - 1) next_velocity_target = self.velocity_targets[t] self.state['next_target_g_v_x'] = next_velocity_target[0] self.state['next_target_g_v_y'] = next_velocity_target[1] @@ -300,14 +305,6 @@ def load_map(map_file): return np.array(map_lists) - @staticmethod - def random_action(low, high, dim): - @staticmethod - def sample(): - act = np.random.random_sample((dim,)) - return (high - low) * act + low - return sample - if __name__ == '__main__': import sys diff --git a/rlschool/quadrotor/generate_default_map.py b/rlschool/quadrotor/generate_default_map.py index ca74c18..c43b886 100644 --- a/rlschool/quadrotor/generate_default_map.py +++ b/rlschool/quadrotor/generate_default_map.py @@ -26,8 +26,8 @@ def generate_map(length, width, height, map_file): pos_len = len(str(height)) with open(map_file, 'w') as f: for i in range(width): - line = ' '.join([str(pos).zfill(pos_len) for pos in - list(map_matrix[i, :])]) + line = ' '.join( + [str(pos).zfill(pos_len) for pos in list(map_matrix[i, :])]) f.write(line + '\n') diff --git a/rlschool/quadrotor/quadrotorsim.py b/rlschool/quadrotor/quadrotorsim.py index 5839cb1..919a9d4 100644 --- a/rlschool/quadrotor/quadrotorsim.py +++ b/rlschool/quadrotor/quadrotorsim.py @@ -36,8 +36,10 @@ def _save_state(self): current_rot_mat = np.copy(self.rotation_matrix) current_power = self.power - return [current_position, current_velocity, current_body_acc, - current_body_w, current_prop_w, current_rot_mat, current_power] + return [ + current_position, current_velocity, current_body_acc, + current_body_w, current_prop_w, current_rot_mat, current_power + ] def _restore_state(self, state): self.global_position, self.global_velocity, self.body_acceleration, \ @@ -113,8 +115,8 @@ def _get_pitch_roll_yaw(self): self.rotation_matrix[2, 2]) pitch = np.arctan2( -self.rotation_matrix[2, 0], - np.sqrt(self.rotation_matrix[2, 1] ** 2 + - self.rotation_matrix[2, 2] ** 2)) + np.sqrt(self.rotation_matrix[2, 1]**2 + + self.rotation_matrix[2, 2]**2)) yaw = np.arctan2(self.rotation_matrix[1, 0], self.rotation_matrix[0, 0]) return pitch, roll, yaw @@ -146,8 +148,8 @@ def _run_internal(self, act): l_m = np.linalg.norm(self._propeller_coord[i]) body_velocity = np.matmul(self._coordination_converter_to_body, self.global_velocity) - bw_to_v = np.cross( - self.body_angular_velocity, self._propeller_coord[i]) * l_m + bw_to_v = np.cross(self.body_angular_velocity, + self._propeller_coord[i]) * l_m v_1 = body_velocity[2] + bw_to_v[2] sign = 1.0 if v_1 > 0 else -1.0 @@ -243,15 +245,18 @@ def reset(self): sign = ((np.random.random(3) > 0.5).astype(np.int) * 2) - 1.0 noisy = float(init_velocity['noisy']) * np.random.random(3) self.global_velocity = np.array( - [init_velocity['x'], init_velocity['y'], - init_velocity['z']], dtype=np.float32) + noisy * sign + [init_velocity['x'], init_velocity['y'], init_velocity['z']], + dtype=np.float32) + noisy * sign if hasattr(self, 'cfg') and 'init_velocity' in self.cfg: init_angular_velocity = self.cfg['init_angular_velocity'] sign = ((np.random.random(3) > 0.5).astype(np.int) * 2) - 1.0 noisy = float(init_angular_velocity['noisy']) * np.random.random(3) self.body_angular_velocity = np.array( - [init_angular_velocity['x'], init_angular_velocity['y'], - init_angular_velocity['z']], dtype=np.float32) + noisy * sign + [ + init_angular_velocity['x'], init_angular_velocity['y'], + init_angular_velocity['z'] + ], + dtype=np.float32) + noisy * sign self._coordination_converter_to_world = self.rotation_matrix self._coordination_converter_to_body = np.linalg.inv( diff --git a/rlschool/quadrotor/render.py b/rlschool/quadrotor/render.py index 49dcbad..c848d9d 100644 --- a/rlschool/quadrotor/render.py +++ b/rlschool/quadrotor/render.py @@ -48,6 +48,7 @@ class Map(object): task (str): name of the task setting. Currently, support `no_collision` and `velocity_control`. """ + def __init__(self, drone_3d_model, horizon_view_size=8, @@ -138,7 +139,7 @@ def _initialize(self, init_drone_z): def _is_exposed(self, position): x, y, z = position for dx, dy, dz in FACES: - if (x+dx, y+dy, z+dz) not in self.whole_map: + if (x + dx, y + dy, z + dz) not in self.whole_map: # At least one face is not covered by another cube block. return True return False @@ -168,7 +169,9 @@ def _add_drone(self): self.drone_texture = rendering.material_to_texture( geom.visual.material) - def _add_drone_velocity(self, init_velocity_vector, radius=0.008, + def _add_drone_velocity(self, + init_velocity_vector, + radius=0.008, color=[255, 0, 0]): """ Add the drone velocity vector as a cylinder into drone drawer batch. @@ -186,7 +189,7 @@ def _add_drone_velocity(self, init_velocity_vector, radius=0.008, radius=radius, height=height, transform=transform) velocity_axis.visual.face_colors = color axis_origin = trimesh.creation.uv_sphere( - radius=radius*5, count=[10, 10]) + radius=radius * 5, count=[10, 10]) axis_origin.visual.face_colors = color merge = trimesh.util.concatenate([axis_origin, velocity_axis]) @@ -237,7 +240,7 @@ def _remove_block(self, position, immediate=True): def _check_neighbors(self, position): x, y, z = position for dx, dy, dz in FACES: - pos = (x+dx, y+dy, z+dz) + pos = (x + dx, y + dy, z + dz) if pos not in self.whole_map: continue if self._is_exposed(pos): @@ -250,13 +253,11 @@ def _check_neighbors(self, position): def _show_block(self, position, texture): vertex_data = cube_vertices(position, 0.5) # 12x6=72 texture_data = list(texture) # 8x6=48 - vertex_count = len(vertex_data) // 3 # 24 - attributes = [ - ('v3f/static', vertex_data), - ('t2f/static', texture_data) - ] - self._partial_map[position] = self.batch.add( - vertex_count, gl.GL_QUADS, self.group, *attributes) + vertex_count = len(vertex_data) // 3 # 24 + attributes = [('v3f/static', vertex_data), ('t2f/static', + texture_data)] + self._partial_map[position] = self.batch.add(vertex_count, gl.GL_QUADS, + self.group, *attributes) def _hide_block(self, position): self._partial_map.pop(position).delete() @@ -348,8 +349,8 @@ def show_velocity(self, position, velocity, expected_velocity=None): if expected_velocity is not None and \ hasattr(self, 'drone_expected_velocity_drawer'): - transform = self._get_velocity_transform( - expected_velocity, position) + transform = self._get_velocity_transform(expected_velocity, + position) gl.glPushMatrix() gl.glMultMatrixf(rendering.matrix_to_gl(transform)) @@ -392,17 +393,17 @@ def change_sectors(self, before, after): # disappear before_set, after_set = set(), set() pad = self.horizon_view_size // 2 - for dx in range(-pad, pad+1): - for dy in range(-pad, pad+1): + for dx in range(-pad, pad + 1): + for dy in range(-pad, pad + 1): dz = 0 - if dx ** 2 + dy ** 2 + dz ** 2 > (pad + 1) ** 2: + if dx**2 + dy**2 + dz**2 > (pad + 1)**2: continue if before: x, y, z = before - before_set.add((x+dx, y+dy, z+dz)) + before_set.add((x + dx, y + dy, z + dz)) if after: x, y, z = after - after_set.add((x+dx, y+dy, z+dz)) + after_set.add((x + dx, y + dy, z + dz)) show = after_set - before_set hide = before_set - after_set @@ -455,25 +456,27 @@ class RenderWindow(pyglet.window.Window): task (str): name of the task setting. Currently, support `no_collision` and `velocity_control`. """ - def __init__(self, - drone_3d_model=None, - horizon_view_size=8, - x_offset=0, - y_offset=0, - z_offset=0, - perspective_fovy=65., - perspective_aspect=4/3., # i.e. 800/600 - perspective_zNear=0.1, - perspective_zFar=60., - perspective_y_offset=3, - perspective_z_offset=3, - sky_light_blue='#00d4ff', - sky_dark_blue='#020024', - width=800, - height=600, - caption='quadrotor', - task='no_collision', - debug_mode=False): + + def __init__( + self, + drone_3d_model=None, + horizon_view_size=8, + x_offset=0, + y_offset=0, + z_offset=0, + perspective_fovy=65., + perspective_aspect=4 / 3., # i.e. 800/600 + perspective_zNear=0.1, + perspective_zFar=60., + perspective_y_offset=3, + perspective_z_offset=3, + sky_light_blue='#00d4ff', + sky_dark_blue='#020024', + width=800, + height=600, + caption='quadrotor', + task='no_collision', + debug_mode=False): if drone_3d_model is None: this_dir = os.path.realpath(os.path.dirname(__file__)) drone_3d_model = os.path.join(this_dir, 'quadcopter.stl') @@ -495,8 +498,14 @@ def __init__(self, # The label to display in the top-left of the canvas self.label = pyglet.text.Label( - '', font_name='Arial', font_size=18, x=10, y=self.height - 10, - anchor_x='left', anchor_y='top', color=(255, 0, 0, 255)) + '', + font_name='Arial', + font_size=18, + x=10, + y=self.height - 10, + anchor_x='left', + anchor_y='top', + color=(255, 0, 0, 255)) # Current (x, y, z) position of the drone in the world, # specified with floats. @@ -510,17 +519,22 @@ def __init__(self, self.rotation = (0, 0) # Config perspective - self.perspective = [perspective_fovy, perspective_aspect, - perspective_zNear, perspective_zFar] + self.perspective = [ + perspective_fovy, perspective_aspect, perspective_zNear, + perspective_zFar + ] self.perspective_over_drone = [ - perspective_y_offset, perspective_z_offset] + perspective_y_offset, perspective_z_offset + ] self.sector = None light_blue = Color(sky_light_blue) dark_blue = Color(sky_dark_blue) - self.colors = [list(i.rgb) + [1.0] for i in - list(light_blue.range_to(dark_blue, 700))] + self.colors = [ + list(i.rgb) + [1.0] + for i in list(light_blue.range_to(dark_blue, 700)) + ] self._gl_set_background(self.colors[0]) self._gl_enable_color_material() @@ -553,8 +567,10 @@ def view(self, drone_state, dt, expected_velocity=None): assert expected_velocity is not None ev_x, ev_y, ev_z = expected_velocity expected_velocity = np.array([ev_x, ev_z, ev_y]) - velocity = np.array([drone_state['g_v_x'], drone_state['g_v_z'], - drone_state['g_v_y']]) + velocity = np.array([ + drone_state['g_v_x'], drone_state['g_v_z'], + drone_state['g_v_y'] + ]) cid = abs(int(drone_state['z'] / 0.1)) % len(self.colors) self._gl_set_background(self.colors[cid]) @@ -569,8 +585,8 @@ def view(self, drone_state, dt, expected_velocity=None): self.internal_map.show_drone(self.position, rot) if self.task == 'velocity_control': - self.internal_map.show_velocity( - self.position, velocity, expected_velocity) + self.internal_map.show_velocity(self.position, velocity, + expected_velocity) self._setup_2d() self._draw_label() @@ -613,8 +629,8 @@ def _setup_3d(self): gl.glLoadIdentity() y, x = self.rotation gl.glRotatef(x, 0, 1, 0) - gl.glRotatef(-y, math.cos(math.radians(x)), - 0, math.sin(math.radians(x))) + gl.glRotatef(-y, math.cos(math.radians(x)), 0, math.sin( + math.radians(x))) # NOTE: for GL render, its x-z plane is the ground plane, # so we unpack the position using `(x, z, y)` instead of `(x, y, z)` x, z, y = self.position @@ -641,34 +657,24 @@ def _gl_unset_background(): @staticmethod def _gl_enable_color_material(): - gl.glColorMaterial(gl.GL_FRONT_AND_BACK, - gl.GL_AMBIENT_AND_DIFFUSE) + gl.glColorMaterial(gl.GL_FRONT_AND_BACK, gl.GL_AMBIENT_AND_DIFFUSE) gl.glEnable(gl.GL_COLOR_MATERIAL) gl.glShadeModel(gl.GL_SMOOTH) - gl.glMaterialfv(gl.GL_FRONT, - gl.GL_AMBIENT, - rendering.vector_to_gl( - 0.192250, 0.192250, 0.192250)) - gl.glMaterialfv(gl.GL_FRONT, - gl.GL_DIFFUSE, - rendering.vector_to_gl( - 0.507540, 0.507540, 0.507540)) - gl.glMaterialfv(gl.GL_FRONT, - gl.GL_SPECULAR, - rendering.vector_to_gl( - .5082730, .5082730, .5082730)) - - gl.glMaterialf(gl.GL_FRONT, - gl.GL_SHININESS, - .4 * 128.0) + gl.glMaterialfv(gl.GL_FRONT, gl.GL_AMBIENT, + rendering.vector_to_gl(0.192250, 0.192250, 0.192250)) + gl.glMaterialfv(gl.GL_FRONT, gl.GL_DIFFUSE, + rendering.vector_to_gl(0.507540, 0.507540, 0.507540)) + gl.glMaterialfv(gl.GL_FRONT, gl.GL_SPECULAR, + rendering.vector_to_gl(.5082730, .5082730, .5082730)) + + gl.glMaterialf(gl.GL_FRONT, gl.GL_SHININESS, .4 * 128.0) @staticmethod def _gl_enable_blending(): # enable blending for transparency gl.glEnable(gl.GL_BLEND) - gl.glBlendFunc(gl.GL_SRC_ALPHA, - gl.GL_ONE_MINUS_SRC_ALPHA) + gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA) @staticmethod def _gl_enable_smooth_lines(): @@ -697,9 +703,7 @@ def _gl_enable_lighting(scene): # convert light object to glLightfv calls multiargs = rendering.light_to_gl( - light=light, - transform=matrix, - lightN=lightN) + light=light, transform=matrix, lightN=lightN) # enable the light in question gl.glEnable(lightN) diff --git a/rlschool/quadrotor/utils.py b/rlschool/quadrotor/utils.py index 61a5c13..83c450f 100644 --- a/rlschool/quadrotor/utils.py +++ b/rlschool/quadrotor/utils.py @@ -75,17 +75,83 @@ def cube_vertices(position, n): return [ # 4 vertices on top face - x-n, y+n, z-n, x-n, y+n, z+n, x+n, y+n, z+n, x+n, y+n, z-n, + x - n, + y + n, + z - n, + x - n, + y + n, + z + n, + x + n, + y + n, + z + n, + x + n, + y + n, + z - n, # on bottom face - x-n, y-n, z-n, x+n, y-n, z-n, x+n, y-n, z+n, x-n, y-n, z+n, + x - n, + y - n, + z - n, + x + n, + y - n, + z - n, + x + n, + y - n, + z + n, + x - n, + y - n, + z + n, # on left face - x-n, y-n, z-n, x-n, y-n, z+n, x-n, y+n, z+n, x-n, y+n, z-n, + x - n, + y - n, + z - n, + x - n, + y - n, + z + n, + x - n, + y + n, + z + n, + x - n, + y + n, + z - n, # on right face - x+n, y-n, z+n, x+n, y-n, z-n, x+n, y+n, z-n, x+n, y+n, z+n, + x + n, + y - n, + z + n, + x + n, + y - n, + z - n, + x + n, + y + n, + z - n, + x + n, + y + n, + z + n, # on front face - x-n, y-n, z+n, x+n, y-n, z+n, x+n, y+n, z+n, x-n, y+n, z+n, + x - n, + y - n, + z + n, + x + n, + y - n, + z + n, + x + n, + y + n, + z + n, + x - n, + y + n, + z + n, # on back face - x+n, y-n, z-n, x-n, y-n, z-n, x-n, y+n, z-n, x+n, y+n, z-n, + x + n, + y - n, + z - n, + x - n, + y - n, + z - n, + x - n, + y + n, + z - n, + x + n, + y + n, + z - n, ] From 69d53377219f106b4c8722a50fe3c0ceacdc7526 Mon Sep 17 00:00:00 2001 From: YuechengLiu Date: Tue, 30 Mar 2021 21:02:10 +0800 Subject: [PATCH 2/4] add gym requirement --- setup.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 503a9cb..c926042 100755 --- a/setup.py +++ b/setup.py @@ -52,7 +52,8 @@ 'trimesh>=3.2.39', 'networkx>=2.2', 'colour>=0.1.5', - 'scipy>=0.12.0' + 'scipy>=0.12.0', + 'gym', ], zip_safe=False, ) From 62444cb379c8752aa58161650821523defc5f8f0 Mon Sep 17 00:00:00 2001 From: YuechengLiu Date: Wed, 31 Mar 2021 10:13:28 +0800 Subject: [PATCH 3/4] old codes recover --- rlschool/quadrotor/env.py | 48 +++---- rlschool/quadrotor/generate_default_map.py | 4 +- rlschool/quadrotor/quadrotorsim.py | 25 ++-- rlschool/quadrotor/render.py | 150 ++++++++++----------- rlschool/quadrotor/utils.py | 78 +---------- 5 files changed, 112 insertions(+), 193 deletions(-) diff --git a/rlschool/quadrotor/env.py b/rlschool/quadrotor/env.py index 6957ceb..a883ef0 100644 --- a/rlschool/quadrotor/env.py +++ b/rlschool/quadrotor/env.py @@ -53,12 +53,11 @@ def __init__(self, healthy_reward=1.0, **kwargs): # TODO: other possible tasks: precision_landing - assert task in [ - 'velocity_control', 'no_collision', 'hovering_control' - ], 'Invalid task setting' + assert task in ['velocity_control', 'no_collision', + 'hovering_control'], 'Invalid task setting' if simulator_conf is None: - simulator_conf = os.path.join( - os.path.dirname(__file__), 'config.json') + simulator_conf = os.path.join(os.path.dirname(__file__), + 'config.json') assert os.path.exists(simulator_conf), \ 'Simulator config file does not exist' @@ -132,17 +131,13 @@ def step(self, action): sensor_dict = self.simulator.get_sensor() state_dict = self.simulator.get_state() - old_pos = [ - self.simulator.global_position[0] + self.x_offset, - self.simulator.global_position[1] + self.y_offset, - self.simulator.global_position[2] + self.z_offset - ] + old_pos = [self.simulator.global_position[0] + self.x_offset, + self.simulator.global_position[1] + self.y_offset, + self.simulator.global_position[2] + self.z_offset] self._update_state(sensor_dict, state_dict) - new_pos = [ - self.simulator.global_position[0] + self.x_offset, - self.simulator.global_position[1] + self.y_offset, - self.simulator.global_position[2] + self.z_offset - ] + new_pos = [self.simulator.global_position[0] + self.x_offset, + self.simulator.global_position[1] + self.y_offset, + self.simulator.global_position[2] + self.z_offset] if self.task in ['no_collision', 'hovering_control']: is_collision = self._check_collision(old_pos, new_pos) reward = self._get_reward(collision=is_collision) @@ -170,11 +165,10 @@ def render(self): if self.viewer is None: if NO_DISPLAY: raise RuntimeError('[Error] Cannot connect to display screen.') - self.viewer = RenderWindow( - task=self.task, - x_offset=self.x_offset, - y_offset=self.y_offset, - z_offset=self.z_offset) + self.viewer = RenderWindow(task=self.task, + x_offset=self.x_offset, + y_offset=self.y_offset, + z_offset=self.z_offset) if 'z' not in self.state: # It's null state @@ -183,9 +177,8 @@ def render(self): state = self._get_state_for_viewer() if self.task == 'velocity_control': self.viewer.view( - state, - self.dt, - expected_velocity=self.velocity_targets[self.ct - 1]) + state, self.dt, + expected_velocity=self.velocity_targets[self.ct-1]) else: self.viewer.view(state, self.dt) @@ -216,7 +209,7 @@ def _get_reward(self, collision=False, velocity_target=(0.0, 0.0, 0.0)): """ # Make sure energy cost always smaller than healthy reward, # to encourage longer running - reward = -min(self.dt * self.simulator.power, self.healthy_reward) + reward = - min(self.dt * self.simulator.power, self.healthy_reward) if self.task == 'no_collision': task_reward = 0.0 if collision else self.healthy_reward reward += task_reward @@ -226,7 +219,8 @@ def _get_reward(self, collision=False, velocity_target=(0.0, 0.0, 0.0)): elif self.task == 'hovering_control': task_reward = 0.0 if collision else self.healthy_reward - velocity_norm = np.linalg.norm(self.simulator.global_velocity) + velocity_norm = np.linalg.norm( + self.simulator.global_velocity) angular_velocity_norm = np.linalg.norm( self.simulator.body_angular_velocity) @@ -254,7 +248,7 @@ def _check_collision(self, old_pos, new_pos): y_min, y_max = min_max(old_pos, new_pos, 1) z_min, z_max = min_max(old_pos, new_pos, 2) - taken_pos = self.map_matrix[y_min:y_max + 1, x_min:x_max + 1] + taken_pos = self.map_matrix[y_min:y_max+1, x_min:x_max+1] if z_min < np.any(taken_pos) or z_max < np.any(taken_pos): return True else: @@ -268,7 +262,7 @@ def _update_state(self, sensor, state): self.state[k] = v if self.task == 'velocity_control': - t = min(self.ct, self.nt - 1) + t = min(self.ct, self.nt-1) next_velocity_target = self.velocity_targets[t] self.state['next_target_g_v_x'] = next_velocity_target[0] self.state['next_target_g_v_y'] = next_velocity_target[1] diff --git a/rlschool/quadrotor/generate_default_map.py b/rlschool/quadrotor/generate_default_map.py index c43b886..ca74c18 100644 --- a/rlschool/quadrotor/generate_default_map.py +++ b/rlschool/quadrotor/generate_default_map.py @@ -26,8 +26,8 @@ def generate_map(length, width, height, map_file): pos_len = len(str(height)) with open(map_file, 'w') as f: for i in range(width): - line = ' '.join( - [str(pos).zfill(pos_len) for pos in list(map_matrix[i, :])]) + line = ' '.join([str(pos).zfill(pos_len) for pos in + list(map_matrix[i, :])]) f.write(line + '\n') diff --git a/rlschool/quadrotor/quadrotorsim.py b/rlschool/quadrotor/quadrotorsim.py index 919a9d4..5839cb1 100644 --- a/rlschool/quadrotor/quadrotorsim.py +++ b/rlschool/quadrotor/quadrotorsim.py @@ -36,10 +36,8 @@ def _save_state(self): current_rot_mat = np.copy(self.rotation_matrix) current_power = self.power - return [ - current_position, current_velocity, current_body_acc, - current_body_w, current_prop_w, current_rot_mat, current_power - ] + return [current_position, current_velocity, current_body_acc, + current_body_w, current_prop_w, current_rot_mat, current_power] def _restore_state(self, state): self.global_position, self.global_velocity, self.body_acceleration, \ @@ -115,8 +113,8 @@ def _get_pitch_roll_yaw(self): self.rotation_matrix[2, 2]) pitch = np.arctan2( -self.rotation_matrix[2, 0], - np.sqrt(self.rotation_matrix[2, 1]**2 + - self.rotation_matrix[2, 2]**2)) + np.sqrt(self.rotation_matrix[2, 1] ** 2 + + self.rotation_matrix[2, 2] ** 2)) yaw = np.arctan2(self.rotation_matrix[1, 0], self.rotation_matrix[0, 0]) return pitch, roll, yaw @@ -148,8 +146,8 @@ def _run_internal(self, act): l_m = np.linalg.norm(self._propeller_coord[i]) body_velocity = np.matmul(self._coordination_converter_to_body, self.global_velocity) - bw_to_v = np.cross(self.body_angular_velocity, - self._propeller_coord[i]) * l_m + bw_to_v = np.cross( + self.body_angular_velocity, self._propeller_coord[i]) * l_m v_1 = body_velocity[2] + bw_to_v[2] sign = 1.0 if v_1 > 0 else -1.0 @@ -245,18 +243,15 @@ def reset(self): sign = ((np.random.random(3) > 0.5).astype(np.int) * 2) - 1.0 noisy = float(init_velocity['noisy']) * np.random.random(3) self.global_velocity = np.array( - [init_velocity['x'], init_velocity['y'], init_velocity['z']], - dtype=np.float32) + noisy * sign + [init_velocity['x'], init_velocity['y'], + init_velocity['z']], dtype=np.float32) + noisy * sign if hasattr(self, 'cfg') and 'init_velocity' in self.cfg: init_angular_velocity = self.cfg['init_angular_velocity'] sign = ((np.random.random(3) > 0.5).astype(np.int) * 2) - 1.0 noisy = float(init_angular_velocity['noisy']) * np.random.random(3) self.body_angular_velocity = np.array( - [ - init_angular_velocity['x'], init_angular_velocity['y'], - init_angular_velocity['z'] - ], - dtype=np.float32) + noisy * sign + [init_angular_velocity['x'], init_angular_velocity['y'], + init_angular_velocity['z']], dtype=np.float32) + noisy * sign self._coordination_converter_to_world = self.rotation_matrix self._coordination_converter_to_body = np.linalg.inv( diff --git a/rlschool/quadrotor/render.py b/rlschool/quadrotor/render.py index c848d9d..49dcbad 100644 --- a/rlschool/quadrotor/render.py +++ b/rlschool/quadrotor/render.py @@ -48,7 +48,6 @@ class Map(object): task (str): name of the task setting. Currently, support `no_collision` and `velocity_control`. """ - def __init__(self, drone_3d_model, horizon_view_size=8, @@ -139,7 +138,7 @@ def _initialize(self, init_drone_z): def _is_exposed(self, position): x, y, z = position for dx, dy, dz in FACES: - if (x + dx, y + dy, z + dz) not in self.whole_map: + if (x+dx, y+dy, z+dz) not in self.whole_map: # At least one face is not covered by another cube block. return True return False @@ -169,9 +168,7 @@ def _add_drone(self): self.drone_texture = rendering.material_to_texture( geom.visual.material) - def _add_drone_velocity(self, - init_velocity_vector, - radius=0.008, + def _add_drone_velocity(self, init_velocity_vector, radius=0.008, color=[255, 0, 0]): """ Add the drone velocity vector as a cylinder into drone drawer batch. @@ -189,7 +186,7 @@ def _add_drone_velocity(self, radius=radius, height=height, transform=transform) velocity_axis.visual.face_colors = color axis_origin = trimesh.creation.uv_sphere( - radius=radius * 5, count=[10, 10]) + radius=radius*5, count=[10, 10]) axis_origin.visual.face_colors = color merge = trimesh.util.concatenate([axis_origin, velocity_axis]) @@ -240,7 +237,7 @@ def _remove_block(self, position, immediate=True): def _check_neighbors(self, position): x, y, z = position for dx, dy, dz in FACES: - pos = (x + dx, y + dy, z + dz) + pos = (x+dx, y+dy, z+dz) if pos not in self.whole_map: continue if self._is_exposed(pos): @@ -253,11 +250,13 @@ def _check_neighbors(self, position): def _show_block(self, position, texture): vertex_data = cube_vertices(position, 0.5) # 12x6=72 texture_data = list(texture) # 8x6=48 - vertex_count = len(vertex_data) // 3 # 24 - attributes = [('v3f/static', vertex_data), ('t2f/static', - texture_data)] - self._partial_map[position] = self.batch.add(vertex_count, gl.GL_QUADS, - self.group, *attributes) + vertex_count = len(vertex_data) // 3 # 24 + attributes = [ + ('v3f/static', vertex_data), + ('t2f/static', texture_data) + ] + self._partial_map[position] = self.batch.add( + vertex_count, gl.GL_QUADS, self.group, *attributes) def _hide_block(self, position): self._partial_map.pop(position).delete() @@ -349,8 +348,8 @@ def show_velocity(self, position, velocity, expected_velocity=None): if expected_velocity is not None and \ hasattr(self, 'drone_expected_velocity_drawer'): - transform = self._get_velocity_transform(expected_velocity, - position) + transform = self._get_velocity_transform( + expected_velocity, position) gl.glPushMatrix() gl.glMultMatrixf(rendering.matrix_to_gl(transform)) @@ -393,17 +392,17 @@ def change_sectors(self, before, after): # disappear before_set, after_set = set(), set() pad = self.horizon_view_size // 2 - for dx in range(-pad, pad + 1): - for dy in range(-pad, pad + 1): + for dx in range(-pad, pad+1): + for dy in range(-pad, pad+1): dz = 0 - if dx**2 + dy**2 + dz**2 > (pad + 1)**2: + if dx ** 2 + dy ** 2 + dz ** 2 > (pad + 1) ** 2: continue if before: x, y, z = before - before_set.add((x + dx, y + dy, z + dz)) + before_set.add((x+dx, y+dy, z+dz)) if after: x, y, z = after - after_set.add((x + dx, y + dy, z + dz)) + after_set.add((x+dx, y+dy, z+dz)) show = after_set - before_set hide = before_set - after_set @@ -456,27 +455,25 @@ class RenderWindow(pyglet.window.Window): task (str): name of the task setting. Currently, support `no_collision` and `velocity_control`. """ - - def __init__( - self, - drone_3d_model=None, - horizon_view_size=8, - x_offset=0, - y_offset=0, - z_offset=0, - perspective_fovy=65., - perspective_aspect=4 / 3., # i.e. 800/600 - perspective_zNear=0.1, - perspective_zFar=60., - perspective_y_offset=3, - perspective_z_offset=3, - sky_light_blue='#00d4ff', - sky_dark_blue='#020024', - width=800, - height=600, - caption='quadrotor', - task='no_collision', - debug_mode=False): + def __init__(self, + drone_3d_model=None, + horizon_view_size=8, + x_offset=0, + y_offset=0, + z_offset=0, + perspective_fovy=65., + perspective_aspect=4/3., # i.e. 800/600 + perspective_zNear=0.1, + perspective_zFar=60., + perspective_y_offset=3, + perspective_z_offset=3, + sky_light_blue='#00d4ff', + sky_dark_blue='#020024', + width=800, + height=600, + caption='quadrotor', + task='no_collision', + debug_mode=False): if drone_3d_model is None: this_dir = os.path.realpath(os.path.dirname(__file__)) drone_3d_model = os.path.join(this_dir, 'quadcopter.stl') @@ -498,14 +495,8 @@ def __init__( # The label to display in the top-left of the canvas self.label = pyglet.text.Label( - '', - font_name='Arial', - font_size=18, - x=10, - y=self.height - 10, - anchor_x='left', - anchor_y='top', - color=(255, 0, 0, 255)) + '', font_name='Arial', font_size=18, x=10, y=self.height - 10, + anchor_x='left', anchor_y='top', color=(255, 0, 0, 255)) # Current (x, y, z) position of the drone in the world, # specified with floats. @@ -519,22 +510,17 @@ def __init__( self.rotation = (0, 0) # Config perspective - self.perspective = [ - perspective_fovy, perspective_aspect, perspective_zNear, - perspective_zFar - ] + self.perspective = [perspective_fovy, perspective_aspect, + perspective_zNear, perspective_zFar] self.perspective_over_drone = [ - perspective_y_offset, perspective_z_offset - ] + perspective_y_offset, perspective_z_offset] self.sector = None light_blue = Color(sky_light_blue) dark_blue = Color(sky_dark_blue) - self.colors = [ - list(i.rgb) + [1.0] - for i in list(light_blue.range_to(dark_blue, 700)) - ] + self.colors = [list(i.rgb) + [1.0] for i in + list(light_blue.range_to(dark_blue, 700))] self._gl_set_background(self.colors[0]) self._gl_enable_color_material() @@ -567,10 +553,8 @@ def view(self, drone_state, dt, expected_velocity=None): assert expected_velocity is not None ev_x, ev_y, ev_z = expected_velocity expected_velocity = np.array([ev_x, ev_z, ev_y]) - velocity = np.array([ - drone_state['g_v_x'], drone_state['g_v_z'], - drone_state['g_v_y'] - ]) + velocity = np.array([drone_state['g_v_x'], drone_state['g_v_z'], + drone_state['g_v_y']]) cid = abs(int(drone_state['z'] / 0.1)) % len(self.colors) self._gl_set_background(self.colors[cid]) @@ -585,8 +569,8 @@ def view(self, drone_state, dt, expected_velocity=None): self.internal_map.show_drone(self.position, rot) if self.task == 'velocity_control': - self.internal_map.show_velocity(self.position, velocity, - expected_velocity) + self.internal_map.show_velocity( + self.position, velocity, expected_velocity) self._setup_2d() self._draw_label() @@ -629,8 +613,8 @@ def _setup_3d(self): gl.glLoadIdentity() y, x = self.rotation gl.glRotatef(x, 0, 1, 0) - gl.glRotatef(-y, math.cos(math.radians(x)), 0, math.sin( - math.radians(x))) + gl.glRotatef(-y, math.cos(math.radians(x)), + 0, math.sin(math.radians(x))) # NOTE: for GL render, its x-z plane is the ground plane, # so we unpack the position using `(x, z, y)` instead of `(x, y, z)` x, z, y = self.position @@ -657,24 +641,34 @@ def _gl_unset_background(): @staticmethod def _gl_enable_color_material(): - gl.glColorMaterial(gl.GL_FRONT_AND_BACK, gl.GL_AMBIENT_AND_DIFFUSE) + gl.glColorMaterial(gl.GL_FRONT_AND_BACK, + gl.GL_AMBIENT_AND_DIFFUSE) gl.glEnable(gl.GL_COLOR_MATERIAL) gl.glShadeModel(gl.GL_SMOOTH) - gl.glMaterialfv(gl.GL_FRONT, gl.GL_AMBIENT, - rendering.vector_to_gl(0.192250, 0.192250, 0.192250)) - gl.glMaterialfv(gl.GL_FRONT, gl.GL_DIFFUSE, - rendering.vector_to_gl(0.507540, 0.507540, 0.507540)) - gl.glMaterialfv(gl.GL_FRONT, gl.GL_SPECULAR, - rendering.vector_to_gl(.5082730, .5082730, .5082730)) - - gl.glMaterialf(gl.GL_FRONT, gl.GL_SHININESS, .4 * 128.0) + gl.glMaterialfv(gl.GL_FRONT, + gl.GL_AMBIENT, + rendering.vector_to_gl( + 0.192250, 0.192250, 0.192250)) + gl.glMaterialfv(gl.GL_FRONT, + gl.GL_DIFFUSE, + rendering.vector_to_gl( + 0.507540, 0.507540, 0.507540)) + gl.glMaterialfv(gl.GL_FRONT, + gl.GL_SPECULAR, + rendering.vector_to_gl( + .5082730, .5082730, .5082730)) + + gl.glMaterialf(gl.GL_FRONT, + gl.GL_SHININESS, + .4 * 128.0) @staticmethod def _gl_enable_blending(): # enable blending for transparency gl.glEnable(gl.GL_BLEND) - gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA) + gl.glBlendFunc(gl.GL_SRC_ALPHA, + gl.GL_ONE_MINUS_SRC_ALPHA) @staticmethod def _gl_enable_smooth_lines(): @@ -703,7 +697,9 @@ def _gl_enable_lighting(scene): # convert light object to glLightfv calls multiargs = rendering.light_to_gl( - light=light, transform=matrix, lightN=lightN) + light=light, + transform=matrix, + lightN=lightN) # enable the light in question gl.glEnable(lightN) diff --git a/rlschool/quadrotor/utils.py b/rlschool/quadrotor/utils.py index 83c450f..61a5c13 100644 --- a/rlschool/quadrotor/utils.py +++ b/rlschool/quadrotor/utils.py @@ -75,83 +75,17 @@ def cube_vertices(position, n): return [ # 4 vertices on top face - x - n, - y + n, - z - n, - x - n, - y + n, - z + n, - x + n, - y + n, - z + n, - x + n, - y + n, - z - n, + x-n, y+n, z-n, x-n, y+n, z+n, x+n, y+n, z+n, x+n, y+n, z-n, # on bottom face - x - n, - y - n, - z - n, - x + n, - y - n, - z - n, - x + n, - y - n, - z + n, - x - n, - y - n, - z + n, + x-n, y-n, z-n, x+n, y-n, z-n, x+n, y-n, z+n, x-n, y-n, z+n, # on left face - x - n, - y - n, - z - n, - x - n, - y - n, - z + n, - x - n, - y + n, - z + n, - x - n, - y + n, - z - n, + x-n, y-n, z-n, x-n, y-n, z+n, x-n, y+n, z+n, x-n, y+n, z-n, # on right face - x + n, - y - n, - z + n, - x + n, - y - n, - z - n, - x + n, - y + n, - z - n, - x + n, - y + n, - z + n, + x+n, y-n, z+n, x+n, y-n, z-n, x+n, y+n, z-n, x+n, y+n, z+n, # on front face - x - n, - y - n, - z + n, - x + n, - y - n, - z + n, - x + n, - y + n, - z + n, - x - n, - y + n, - z + n, + x-n, y-n, z+n, x+n, y-n, z+n, x+n, y+n, z+n, x-n, y+n, z+n, # on back face - x + n, - y - n, - z - n, - x - n, - y - n, - z - n, - x - n, - y + n, - z - n, - x + n, - y + n, - z - n, + x+n, y-n, z-n, x-n, y-n, z-n, x-n, y+n, z-n, x+n, y+n, z-n, ] From bfcfb4e7e2bde5a908c89c8f0fd1448f1279b98f Mon Sep 17 00:00:00 2001 From: YuechengLiu Date: Thu, 1 Apr 2021 09:54:02 +0800 Subject: [PATCH 4/4] gym version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index c926042..54dd150 100755 --- a/setup.py +++ b/setup.py @@ -53,7 +53,7 @@ 'networkx>=2.2', 'colour>=0.1.5', 'scipy>=0.12.0', - 'gym', + 'gym==0.18.0', ], zip_safe=False, )