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