From 6b005ae8c19e07337d5595a52bb539446425ca11 Mon Sep 17 00:00:00 2001 From: madratman Date: Thu, 31 Oct 2019 16:35:43 -0700 Subject: [PATCH] local airsim client -> airsimdroneracingvae --- .../action_generator/src/soccer_datagen.py | 16 +++---- imitation_learning/bc_navigation.py | 44 +++++++++---------- imitation_learning/vel_regressor.py | 9 +--- racing_utils/geom_utils.py | 15 ++----- racing_utils/trajectory_utils.py | 37 ++++------------ 5 files changed, 40 insertions(+), 81 deletions(-) diff --git a/datagen/action_generator/src/soccer_datagen.py b/datagen/action_generator/src/soccer_datagen.py index 1e79ad0d..2b6ab9f1 100644 --- a/datagen/action_generator/src/soccer_datagen.py +++ b/datagen/action_generator/src/soccer_datagen.py @@ -6,13 +6,9 @@ import threading import os,sys -curr_dir = os.path.dirname(os.path.abspath(__file__)) -airsim_path = os.path.join(curr_dir, '..', '..', '..', 'airsim') -sys.path.insert(0, airsim_path) -import setup_path -import airsim -import airsim.types -import airsim.utils +import airsimdroneracingvae +import airsimdroneracingvae.types +import airsimdroneracingvae.utils # import utils models_path = os.path.join(curr_dir, '..', 'racing_utils') @@ -76,7 +72,7 @@ def __init__(self, self.acc_max = acc_max # todo encapsulate in function - self.client = airsim.MultirotorClient() + self.client = airsimdroneracingvae.MultirotorClient() self.client.confirmConnection() self.client.enableApiControl(True, vehicle_name=self.drone_name) time.sleep(0.05) @@ -146,7 +142,7 @@ def set_current_track_gate_poses_from_default_track_in_binary(self): # print(gate_pose.position.x_val, gate_pose.position.y_val,gate_pose.position.z_val) def takeoff_with_moveOnSpline(self, takeoff_height, vel_max, acc_max): - self.client.moveOnSplineAsync(path=[airsim.Vector3r(4, -2, takeoff_height)], + self.client.moveOnSplineAsync(path=[airsimdroneracingvae.Vector3r(4, -2, takeoff_height)], vel_max=vel_max, acc_max=acc_max, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, @@ -297,7 +293,7 @@ def start_training_data_generator(self, num_training_laps=100, level_name='Socce time.sleep(0.01) self.client.armDisarm(True, vehicle_name=self.drone_name) time.sleep(0.01) - self.client.setTrajectoryTrackerGains(airsim.TrajectoryTrackerGains().to_list(), vehicle_name=self.drone_name) + self.client.setTrajectoryTrackerGains(airsimdroneracingvae.TrajectoryTrackerGains().to_list(), vehicle_name=self.drone_name) time.sleep(0.01) self.takeoff_with_moveOnSpline(takeoff_height=-2, vel_max=self.vel_max, acc_max=self.acc_max) self.set_num_training_laps(num_training_laps) diff --git a/imitation_learning/bc_navigation.py b/imitation_learning/bc_navigation.py index 6ac0a14b..48e8631a 100644 --- a/imitation_learning/bc_navigation.py +++ b/imitation_learning/bc_navigation.py @@ -7,13 +7,9 @@ import tensorflow as tf import os, sys -curr_dir = os.path.dirname(os.path.abspath(__file__)) -airsim_path = os.path.join(curr_dir, '..', 'airsim') -sys.path.insert(0, airsim_path) -import setup_path -import airsim -import airsim.types -import airsim.utils +import airsimdroneracingvae +import airsimdroneracingvae.types +import airsimdroneracingvae.utils # import utils models_path = os.path.join(curr_dir, '..', 'racing_utils') @@ -32,7 +28,7 @@ ########################################### def process_image(client, img_res): - image_response = client.simGetImages([airsim.ImageRequest('0', airsim.ImageType.Scene, False, False)])[0] + image_response = client.simGetImages([airsimdroneracingvae.ImageRequest('0', airsimdroneracingvae.ImageType.Scene, False, False)])[0] img_1d = np.fromstring(image_response.image_data_uint8, dtype=np.uint8) # get numpy array img_bgr = img_1d.reshape(image_response.height, image_response.width, 3) # reshape array to 4 channel image array H X W X 3 img_resized = cv2.resize(img_bgr, (img_res, img_res)).astype(np.float32) @@ -48,11 +44,11 @@ def move_drone(client, vel_cmd): vel_cmd[0:2] = vel_cmd[0:2] *1.0 # usually base speed is 3/ms vel_cmd[3] = vel_cmd[3] * 1.0 # yaw rate is given in deg/s!! not rad/s - yaw_mode = airsim.YawMode(is_rate=True, yaw_or_rate=vel_cmd[3]*180.0/np.pi) + yaw_mode = airsimdroneracingvae.YawMode(is_rate=True, yaw_or_rate=vel_cmd[3]*180.0/np.pi) client.moveByVelocityAsync(vel_cmd[0], vel_cmd[1], vel_cmd[2], duration=0.1, yaw_mode=yaw_mode) -print(os.path.abspath(airsim.__file__)) +print(os.path.abspath(airsimdroneracingvae.__file__)) os.environ['TF_CPP_MIN_LOG_LEVEL'] = '1' os.environ['TF_FORCE_GPU_ALLOW_GROWTH'] = 'true' @@ -64,7 +60,7 @@ def move_drone(client, vel_cmd): if __name__ == "__main__": # set airsim client - client = airsim.MultirotorClient() + client = airsimdroneracingvae.MultirotorClient() client.confirmConnection() # client.simLoadLevel('Soccer_Field_Easy') client.simLoadLevel('Soccer_Field_Easy') @@ -76,7 +72,7 @@ def move_drone(client, vel_cmd): time.sleep(0.01) client.armDisarm(True, vehicle_name=drone_name) time.sleep(0.01) - client.setTrajectoryTrackerGains(airsim.TrajectoryTrackerGains().to_list(), vehicle_name=drone_name) + client.setTrajectoryTrackerGains(airsimdroneracingvae.TrajectoryTrackerGains().to_list(), vehicle_name=drone_name) time.sleep(0.01) # destroy all previous gates in map @@ -93,22 +89,22 @@ def move_drone(client, vel_cmd): time.sleep(1.0) - # takeoff_position = airsim.Vector3r(25, 7, -1.5) - # takeoff_orientation = airsim.Vector3r(.2, -0.9, 0) + # takeoff_position = airsimdroneracingvae.Vector3r(25, 7, -1.5) + # takeoff_orientation = airsimdroneracingvae.Vector3r(.2, -0.9, 0) # - # takeoff_position = airsim.Vector3r(25, -7, -1.5) - # takeoff_orientation = airsim.Vector3r(-.2, 0.9, 0) + # takeoff_position = airsimdroneracingvae.Vector3r(25, -7, -1.5) + # takeoff_orientation = airsimdroneracingvae.Vector3r(-.2, 0.9, 0) - takeoff_position = airsim.Vector3r(5.5, -4, -1.5+offset[2]) - takeoff_orientation = airsim.Vector3r(0.4, 0.9, 0) + takeoff_position = airsimdroneracingvae.Vector3r(5.5, -4, -1.5+offset[2]) + takeoff_orientation = airsimdroneracingvae.Vector3r(0.4, 0.9, 0) - # takeoff_position = airsim.Vector3r(0, 0, -2) - # takeoff_position = airsim.Vector3r(0, 0, 10) - # takeoff_orientation = airsim.Vector3r(1, 0, 0) + # takeoff_position = airsimdroneracingvae.Vector3r(0, 0, -2) + # takeoff_position = airsimdroneracingvae.Vector3r(0, 0, 10) + # takeoff_orientation = airsimdroneracingvae.Vector3r(1, 0, 0) # client.plot_tf([takeoff_pose], duration=20.0, vehicle_name=drone_name) - # client.moveOnSplineAsync([airsim.Vector3r(0, 0, -3)], vel_max=15.0, acc_max=5.0, vehicle_name=drone_name, viz_traj=True).join() + # client.moveOnSplineAsync([airsimdroneracingvae.Vector3r(0, 0, -3)], vel_max=15.0, acc_max=5.0, vehicle_name=drone_name, viz_traj=True).join() client.moveOnSplineVelConstraintsAsync([takeoff_position], [takeoff_orientation], vel_max=vel_max, acc_max=acc_max, vehicle_name=drone_name, viz_traj=False).join() - # client.moveOnSplineVelConstraintsAsync([airsim.Vector3r(1, 0, 8)], [airsim.Vector3r(1, 0, 0)], vel_max=vel_max, acc_max=acc_max, vehicle_name=drone_name, viz_traj=True) + # client.moveOnSplineVelConstraintsAsync([airsimdroneracingvae.Vector3r(1, 0, 8)], [airsimdroneracingvae.Vector3r(1, 0, 0)], vel_max=vel_max, acc_max=acc_max, vehicle_name=drone_name, viz_traj=True) time.sleep(1.0) img_res = 64 @@ -152,7 +148,7 @@ def move_drone(client, vel_cmd): img_batch_1, cam_pos, cam_orientation = process_image(client, img_res) elapsed_time_net = time.time() - start_time times_net[count] = elapsed_time_net - p_o_b = airsim.types.Pose(cam_pos, cam_orientation) + p_o_b = airsimdroneracingvae.types.Pose(cam_pos, cam_orientation) vel_cmd = vel_regressor.predict_velocities(img_batch_1, p_o_b) # print(vel_cmd) # print('Before sending vel cmd') diff --git a/imitation_learning/vel_regressor.py b/imitation_learning/vel_regressor.py index d4b81ef4..48557dcf 100644 --- a/imitation_learning/vel_regressor.py +++ b/imitation_learning/vel_regressor.py @@ -3,12 +3,7 @@ import os import sys -curr_dir = os.path.dirname(os.path.abspath(__file__)) - -airsim_path = os.path.join(curr_dir, '..', 'airsim') -sys.path.insert(0, airsim_path) -import setup_path -import airsim +import airsimdroneracingvae # import model models_path = os.path.join(curr_dir, '..', 'racing_models') @@ -57,6 +52,6 @@ def predict_velocities(self, img, p_o_b): predictions = predictions.numpy() predictions = racing_utils.dataset_utils.de_normalize_v(predictions) # print('Predicted body vel: \n {}'.format(predictions[0])) - v_xyz_world = racing_utils.geom_utils.convert_t_body_2_world(airsim.Vector3r(predictions[0,0], predictions[0,1], predictions[0,2]), p_o_b.orientation) + v_xyz_world = racing_utils.geom_utils.convert_t_body_2_world(airsimdroneracingvae.Vector3r(predictions[0,0], predictions[0,1], predictions[0,2]), p_o_b.orientation) return np.array([v_xyz_world.x_val, v_xyz_world.y_val, v_xyz_world.z_val, predictions[0,3]]) diff --git a/racing_utils/geom_utils.py b/racing_utils/geom_utils.py index 17a8fb2e..b09b0261 100644 --- a/racing_utils/geom_utils.py +++ b/racing_utils/geom_utils.py @@ -2,16 +2,9 @@ import sys from scipy.spatial.transform import Rotation import math -curr_dir = os.path.dirname(os.path.abspath(__file__)) - -airsim_path = os.path.join(curr_dir, '..', 'airsim') -sys.path.insert(0, airsim_path) -import setup_path -import airsim - -from airsim.utils import to_eularian_angles, to_quaternion +from airsimdroneracingvae.utils import to_eularian_angles, to_quaternion import numpy as np -from airsim.types import Pose, Vector3r, Quaternionr +from airsimdroneracingvae.types import Pose, Vector3r, Quaternionr def interp_vector(a, b, n): @@ -146,9 +139,9 @@ def get_gate_facing_vector_from_quaternion(airsim_quat, direction, scale=1.0,): [ 0.0, 0.0, 0.0, 1.0]]) gate_facing_vector = rotation_matrix[:-1, 1] if direction == 0: - return airsim.Vector3r(scale*gate_facing_vector[0], scale*gate_facing_vector[1], scale*gate_facing_vector[2]) + return airsimdroneracingvae.Vector3r(scale*gate_facing_vector[0], scale*gate_facing_vector[1], scale*gate_facing_vector[2]) else: - return airsim.Vector3r(-scale*gate_facing_vector[0], -scale*gate_facing_vector[1], scale*gate_facing_vector[2]) + return airsimdroneracingvae.Vector3r(-scale*gate_facing_vector[0], -scale*gate_facing_vector[1], scale*gate_facing_vector[2]) diff --git a/racing_utils/trajectory_utils.py b/racing_utils/trajectory_utils.py index 05c613e2..2fc6cc86 100644 --- a/racing_utils/trajectory_utils.py +++ b/racing_utils/trajectory_utils.py @@ -3,18 +3,10 @@ import random import math import time - -import os, sys -curr_dir = os.path.dirname(os.path.abspath(__file__)) -airsim_path = os.path.join(curr_dir, '..', 'airsim') -sys.path.insert(0, airsim_path) -import setup_path -import airsim -import airsim.types -import airsim.utils -from airsim.types import Pose, Vector3r, Quaternionr -from airsim.utils import to_eularian_angles, to_quaternion - +import airsimdroneracingvae.types +import airsimdroneracingvae.utils +from airsimdroneracingvae.types import Pose, Vector3r, Quaternionr +from airsimdroneracingvae.utils import to_eularian_angles, to_quaternion def MoveCheckeredGates(client): gate_names_sorted = sorted(client.simListSceneObjects("Gate.*")) @@ -25,13 +17,11 @@ def MoveCheckeredGates(client): client.simSetObjectPose(gate, pose_far) # time.sleep(0.05) - def AllGatesDestroyer(client): for gate_object in client.simListSceneObjects(".*[Gg]ate.*"): client.simDestroyObject(gate_object) time.sleep(0.05) - def RedGateSpawner(client, num_gates, noise_amp): gate_poses=[] for idx in range(num_gates): @@ -42,7 +32,6 @@ def RedGateSpawner(client, num_gates, noise_amp): time.sleep(0.05) return gate_poses - def RedGateSpawnerCircle(client, num_gates, radius, radius_noise, height_range, track_offset=[0, 0, 0]): track = generate_gate_poses(num_gates=num_gates, race_course_radius=radius, radius_noise=radius_noise, height_range=height_range, direction=0, offset=track_offset) for idx in range(num_gates): @@ -50,7 +39,6 @@ def RedGateSpawnerCircle(client, num_gates, radius, radius_noise, height_range, client.simSpawnObject("gate_" + str(idx), "RedGate16x16", track[idx], 0.75) time.sleep(0.05) - def RedGateSpawnerTrack(client, num_gates, radius, radius_noise, height_range, num_ignore = 0, track_offset=[0, 0, 0]): offset_0 = [sum(x) for x in zip(track_offset, [radius, 0, 0])] track_0 = generate_gate_poses(num_gates=num_gates, race_course_radius=radius, radius_noise=radius_noise, height_range=height_range, direction=0, offset=offset_0) @@ -68,16 +56,15 @@ def RedGateSpawnerTrack(client, num_gates, radius, radius_noise, height_range, n client.simSpawnObject("gate_" + str(idx) + "track_1", "RedGate16x16", track_1[idx], 0.75) time.sleep(0.05) - def generate_gate_poses(num_gates, race_course_radius, radius_noise, height_range, direction, offset=[0,0,0], type_of_segment="circle"): if type_of_segment == "circle": (x_t, y_t, z_t) = tuple([generate_circle(i, num_gates, race_course_radius, radius_noise, direction) for i in range(3)]) # todo unreadable code # todo un-hardcode - # airsim.Vector3r((x_t[t_i][0] - x_t[0][0]), (y_t[t_i][0] - y_t[0][0]), random.uniform(height_range[0], height_range[1])), \ + # airsimdroneracingvae.Vector3r((x_t[t_i][0] - x_t[0][0]), (y_t[t_i][0] - y_t[0][0]), random.uniform(height_range[0], height_range[1])), \ gate_poses = [ - airsim.Pose( - airsim.Vector3r((x_t[t_i][0]+offset[0]), + airsimdroneracingvae.Pose( + airsimdroneracingvae.Vector3r((x_t[t_i][0]+offset[0]), (y_t[t_i][0]+offset[1]), random.uniform(height_range[0], height_range[1])+offset[2]), quaternionFromUnitGradient(x_t[t_i][1], y_t[t_i][1], z_t[t_i][1]) @@ -86,11 +73,10 @@ def generate_gate_poses(num_gates, race_course_radius, radius_noise, height_rang # elif type_of_segment == "cubic": return gate_poses - def quaternionFromUnitGradient(dx_dt, dy_dt, dz_dt): default_gate_facing_vector = type("", (), dict(x=0, y=1, z=0))() r0 = default_gate_facing_vector - q = airsim.Quaternionr( + q = airsimdroneracingvae.Quaternionr( r0.y * dz_dt - r0.z * dy_dt, r0.z * dx_dt - r0.x * dz_dt, r0.x * dy_dt - r0.y * dx_dt, @@ -107,7 +93,6 @@ def quaternionFromUnitGradient(dx_dt, dy_dt, dz_dt): q.z_val /= length return q - def generate_circle(i, num_gates, race_course_radius, radius_amp, direction): ts = [t / (num_gates) for t in range(0, num_gates)] samples = [0 for t in ts] @@ -147,9 +132,3 @@ def generate_circle(i, num_gates, race_course_radius, radius_amp, direction): samples[idx] = 0. derivatives[idx] = 0. return list(zip(samples, derivatives)) - - - - - -