def get_gate_facing_vector_from_quaternion(self, airsim_quat, scale=1.0): import numpy as np # convert gate quaternion to rotation matrix. # ref: https://en.wikipedia.org/wiki/Rotation_matrix#Quaternion; https://www.lfd.uci.edu/~gohlke/code/transformations.py.html q = np.array( [ airsim_quat.w_val, airsim_quat.x_val, airsim_quat.y_val, airsim_quat.z_val, ], dtype=np.float64, ) n = np.dot(q, q) if n < np.finfo(float).eps: return airsim.Vector3r(0.0, 1.0, 0.0) q *= np.sqrt(2.0 / n) q = np.outer(q, q) rotation_matrix = np.array( [ [1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0]], [q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0]], [q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2]], ] ) gate_facing_vector = rotation_matrix[:, 1] return airsim.Vector3r( scale * gate_facing_vector[0], scale * gate_facing_vector[1], scale * gate_facing_vector[2], )
def __init__(self, gate_pose_dims_tuple): self.gate_pose = gate_pose_dims_tuple[0] self.gate_half_dims = gate_pose_dims_tuple[1] self.facing_vector = self.get_gate_facing_vector_from_quaternion( self.gate_pose.orientation) self.width_constraint_unit_vector = self.rotate_vector_by_quaternion( airsim.Vector3r(1.0, 0.0, 0.0), self.gate_pose.orientation) self.height_constraint_unit_vector = self.rotate_vector_by_quaternion( airsim.Vector3r(0.0, 0.0, 1.0), self.gate_pose.orientation)
def convex_combination(vec1, vec2, eta): ''' 0 <= eta <= 1, indicating how close it is to the vec2 e.g. eta = 1, vec_result = vec2 eta = 0, vec_result = vec1 ''' vec_result = airsim.Vector3r(0, 0, 0) vec_result.x_val = (1 - eta) * vec1.x_val + eta * vec2.x_val vec_result.y_val = (1 - eta) * vec1.y_val + eta * vec2.y_val vec_result.z_val = (1 - eta) * vec1.z_val + eta * vec2.z_val return vec_result
def takeoff_with_moveOnSpline(self, takeoff_height=1.0): start_position = self.airsim_client.simGetVehiclePose( vehicle_name=self.drone_name).position takeoff_waypoint = airsim.Vector3r( start_position.x_val, start_position.y_val, start_position.z_val - takeoff_height) self.airsim_client.moveOnSplineAsync( [takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name).join() self.racing_started = True
DEFAULT_JS1_Z_GAIN = -0.5 DEFAULT_JS2_PITCH_GAIN = 0.5 DEFAULT_JS2_ROLL_GAIN = 0.5 DEFAULT_JS2_YAW_GAIN = -2.0 DEFAULT_JS2_Z_GAIN = -0.5 JS_CONFIG_TIME = 5.0 JOYSTICKS_SETTINGS_FILE = PG_GAME_FOLDER_PATH / "js.obj" # airsim constants: DRONE1_NAME = 'drone_1' # this is the name of the first drone in the .json file DRONE2_NAME = 'drone_2' # this is the name of the second drone in the .json file DEFAULT_DRONE1_OFFSET = airsim.Vector3r(0.0, 0.0, 0.0) # this is the start position of the first drone in the .json file DEFAULT_DRONE2_OFFSET = airsim.Vector3r(0.0, 0.0, 0.0) # this is the start position of the second drone in the .json file DISPLAY_MODE = "NO_DISPLAY" # setting this to "WINDOWED_DISPLAY" will launch the arisim display alongside the game # environment constants: print(SYS_STR) TIME_STEP_METHOD = 'CONTINUE_FOR_TIME' if SYS_STR == 'Linux' else 'JOIN' # CONTINUE_FOR_TIME seems to break rendering on Windows IMG_OBS_NAME = 'front_camera_dc' DEFAULT_CONTROL_API = 'moveByRollPitchYawrateZAsync' DEFAULT_OBS_COORD = "all" # 'dc', 'ned', 'global' or 'all' DEFAULT_ACT_COORD = "ned" # 'dc', 'ned' or 'global' (!: check API support in the environment before changing this) DEFAULT_RF_CONFIG = { # parameters of the reward function 'constant_penalty': -1.0, # constant penalty per time-step 'collision_radius': 0.5, # at this distance from the opponen, the lagging drone dies (don't change for now, this is enforced by the airsim .pak file) 'velocity_gain': 10.0, # not real velocity: difference of distance to next objective between 2 get_reward()
def to_airsim_vector(np_arr): assert np.size(np_arr) == 3 return airsim.Vector3r(np.float(np_arr[0]), np.float(np_arr[1]), np.float(np_arr[2]))
def get_gate_facing_vector_from_quaternion(quat): return Gate.rotate_vector_by_quaternion(airsim.Vector3r(0.0, 1.0, 0.0), quat)
def rotate_vector_by_quaternion(vect, quat): res = quat * vect.to_Quaternionr() * quat.inverse() return airsim.Vector3r(res.x_val, res.y_val, res.z_val)
def get_ground_truth_gate_poses_and_half_dims(self): """ Returns a list of lists each list contains the pose and half dimensions of a gates one list per gate (sorted from first to last gate) """ gate_names_sorted_bad = sorted( self.airsim_client.simListSceneObjects("Gate.*")) assert len( gate_names_sorted_bad ) > 0, "ERROR: the gate list returned by simListSceneObjects is empty" gate_indices_bad = [ int(gate_name.split('_')[0][4:]) for gate_name in gate_names_sorted_bad ] gate_indices_correct = sorted(range(len(gate_indices_bad)), key=lambda k: gate_indices_bad[k]) gate_names_sorted = [ gate_names_sorted_bad[gate_idx] for gate_idx in gate_indices_correct ] nominal_inner_dims = self.airsim_client.simGetNominalGateInnerDimensions( ) res = [] for gate_name in gate_names_sorted: p = self.airsim_client.simGetObjectPose(object_name=gate_name) s = self.airsim_client.simGetObjectScale(object_name=gate_name) # print(f"DEBUG: gate {gate_name}") # print(f"DEBUG: scale {gate_name}: {s}") cpt = 0 while (math.isnan(p.position.x_val) or math.isnan(p.position.y_val) or math.isnan(p.position.z_val) or math.isnan(s.x_val) or math.isnan(s.y_val) or math.isnan( s.z_val)) and cpt < MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS: print(f"DEBUG: p for {gate_name} is {p}") print(f"DEBUG: s for {gate_name} is {s}") print(f"DEBUG: {gate_name} is nan, retrying...") cpt += 1 p = self.airsim_client.simGetObjectPose(gate_name) assert not math.isnan( p.position.x_val ), f"ERROR: {gate_name} p.position.x_val is still {p.position.x_val} after {cpt} trials" assert not math.isnan( p.position.y_val ), f"ERROR: {gate_name} p.position.y_val is still {p.position.y_val} after {cpt} trials" assert not math.isnan( p.position.z_val ), f"ERROR: {gate_name} p.position.z_val is still {p.position.z_val} after {cpt} trials" assert not math.isnan( s.x_val ), f"ERROR: {gate_name} p.position.x_val is still {s.x_val} after {cpt} trials" assert not math.isnan( s.y_val ), f"ERROR: {gate_name} p.position.y_val is still {s.y_val} after {cpt} trials" assert not math.isnan( s.z_val ), f"ERROR: {gate_name} p.position.z_val is still {s.z_val} after {cpt} trials" res.append([ p, airsim.Vector3r(s.x_val * nominal_inner_dims.x_val / 2, s.y_val * nominal_inner_dims.y_val / 2, s.z_val * nominal_inner_dims.z_val / 2) ]) assert len(res) > 0, "ERROR: the final gates list is empty" return res, gate_names_sorted