def get_drone_frame_vector_from_world_frame_vector(self, airsim_quat, vec):
import numpy as np
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]]])
rotation_matrix = np.linalg.inv(rotation_matrix)
world_vec = np.array([vec.x_val, vec.y_val, vec.z_val])
drone_vec = np.matmul(rotation_matrix, world_vec)
return airsim.Vector3r(drone_vec[0], drone_vec[1], drone_vec[2])
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 takeoff_with_moveOnSpline(self, takeoff_height=2.0):
if self.level_name == "ZhangJiaJie_Medium":
takeoff_height = 1.0
# time.sleep(1)
start_position = self.airsim_client.simGetVehiclePose(
vehicle_name=self.drone_name).position
start_position.x_val += randf(0, 1)
start_position.y_val += randf(-1, 0)
# start_position.z_val += randf(0, 1)
# start_position.x_val += -2
# start_position.y_val += 2
# start_position.z_val += -1
takeoff_waypoint = airsim.Vector3r(
start_position.x_val, start_position.y_val,
start_position.z_val - takeoff_height)
if (self.plot_transform):
self.airsim_client.plot_transform(
[airsim.Pose(takeoff_waypoint, airsim.Quaternionr())],
vehicle_name=self.drone_name)
self.airsim_client.moveToPositionAsync(takeoff_waypoint.x_val,
takeoff_waypoint.y_val,
takeoff_waypoint.z_val,
1).join()
def takeoff_with_moveOnSpline(self, takeoff_height=0.1):
if self.level_name == "ZhangJiaJie_Medium":
takeoff_height = -3
# time.sleep(1)
start_position = self.airsim_client.simGetVehiclePose(
vehicle_name=self.drone_name).position
takeoff_waypoint = airsim.Vector3r(start_position.x_val,
start_position.y_val,
-takeoff_height)
if (self.plot_transform):
self.airsim_client.plot_transform(
[airsim.Pose(takeoff_waypoint, airsim.Quaternionr())],
vehicle_name=self.drone_name)
self.airsim_client.moveOnSplineAsync(
[takeoff_waypoint],
vel_max=15.0,
acc_max=5.0,
add_position_constraint=True,
add_velocity_constraint=False,
viz_traj=self.viz_traj,
vehicle_name=self.drone_name).join()
def get_world_frame_vel_from_drone_frame_vel(self, airsim_quat, velocity):
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]]])
drone_frame_vel_array = np.array([velocity.x_val, velocity.y_val, velocity.z_val])
world_vel = np.matmul(rotation_matrix, drone_frame_vel_array)
return airsim.Vector3r(world_vel[0], world_vel[1], world_vel[2])
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.pass_position_vec = takeoff_waypoint
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()
def takeoff_with_moveOnSpline(self, takeoff_height = 0.1):
if self.level_name == "ZhangJiaJie_Medium":
takeoff_height = 0.3
start_position = self.airsim_client.simGetVehiclePose(vehicle_name=self.drone_name).position
takeoff_waypoint = airsim.Vector3r(start_position.x_val, start_position.y_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()
def takeoff_with_moveOnSpline(self, takeoff_height = 0.1):
if self.level_name == "ZhangJiaJie_Medium":
takeoff_height = 0.3
takeoff_waypoint = airsim.Vector3r(0, 0, -takeoff_height)
if(self.plot_transform):
self.airsim_client.plot_transform([airsim.Pose(takeoff_waypoint, airsim.Quaternionr())], vehicle_name=self.drone_name)
self.airsim_client.moveOnSplineAsync([takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_curr_odom_position_constraint=True,
add_curr_odom_velocity_constraint=False, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join()
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 get_gate_facing_vector_from_quaternion(self, airsim_quat, scale = 1.0):
import numpy as np
q = np.array([airsim_quat.w_val, airsim_quat.x_val, airsim_quat.y_val, airsim_quat.z_val], dtype=np.float64, copy=True)
n = np.dot(q, q)
if n < np.finfo(float).eps:
return np.identity(4)
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], 0.0],
[ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0],
[ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
[ 0.0, 0.0, 0.0, 1.0]])
gate_facing_vector = rotation_matrix[:-1,1]
return airsim.Vector3r(scale * gate_facing_vector[0], scale * gate_facing_vector[1], scale * gate_facing_vector[2])
def __init__(self,
drone_name,
drone_params,
use_vel_constraints=False,
race_tier=1,
viz_traj=True):
super().__init__(drone_name=drone_name)
self.drone_name = drone_name
self.drone_params = drone_params
self.use_vel_constraints = use_vel_constraints
self.viz_traj = viz_traj
self.gates_complete = -1
self.logPath = "C:/Users/shubh/Documents/AirSim/AirSimExe/Saved/Logs/RaceLogs"
self.race_tier = race_tier
self.action_high = 1
self.action_low = -1
self.max_vel = drone_params["v_max"]
self.observation_space = [33, None]
self.action_space = [4, None]
self.reward = 0
self.prev_gate = -1
drone_state = self.airsim_client.getMultirotorState(self.drone_name)
self.time = drone_state.timestamp
self.prev_time = drone_state.timestamp
self.last_gate_change_time = drone_state.timestamp
start_position = drone_state.kinematics_estimated.position
self.cur_waypt = airsim.Vector3r(start_position.x_val,
start_position.y_val,
start_position.z_val - 1.0)
self.disq = False
self.scene_ls = self.airsim_client.simListSceneObjects()
self.scene_poses = []
for obj in self.scene_ls:
self.scene_poses.append(
self.airsim_client.simGetObjectPose(
obj).position.to_numpy_array())
self.scene_poses = np.array(self.scene_poses)
def reset_race(self):
self.airsim_client.simResetRace()
time.sleep(1)
self.gates_complete = -1
self.reward = 0
self.disq = False
self.prev_gate = -1
drone_state = self.airsim_client.getMultirotorState(self.drone_name)
self.time = drone_state.timestamp
self.prev_time = drone_state.timestamp
self.last_gate_change_time = drone_state.timestamp
start_position = drone_state.kinematics_estimated.position
self.cur_waypt = airsim.Vector3r(start_position.x_val,
start_position.y_val,
start_position.z_val - 1.0)
self.start_race(self.race_tier)
self.takeoff_with_moveOnSpline()
time.sleep(1)
return self.state_to_feature()
def main(args):
# ensure you have generated the neurips planning settings file by running python generate_settings_file.py
baseline_racer = BaselineRacer(drone_name="drone_1",
viz_traj=args.viz_traj,
viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0],
viz_image_cv2=args.viz_image_cv2)
baseline_racer.load_level(args.level_name)
if args.level_name == "Final_Tier_1":
args.race_tier = 1
if args.level_name == "Final_Tier_2":
args.race_tier = 2
if args.level_name == "Final_Tier_3":
args.race_tier = 3
baseline_racer.start_race(args.race_tier)
baseline_racer.initialize_drone()
baseline_racer.takeoff_with_moveOnSpline()
baseline_racer.get_ground_truth_gate_poses()
baseline_racer.start_image_callback_thread()
baseline_racer.start_odometry_callback_thread()
time.sleep(1)
start_drone_heading_vec = baseline_racer.get_world_frame_vel_from_drone_frame_vel(
baseline_racer.airsim_client.simGetVehiclePose("drone_1").orientation,
airsim.Vector3r(1, 0, 0))
baseline_racer.airsim_client.moveByVelocityAsync(
start_drone_heading_vec.x_val,
start_drone_heading_vec.y_val,
start_drone_heading_vec.z_val,
duration=0.5).join()
baseline_racer.start_control_thread()
if (baseline_racer.next_gate_idx == 21 and baseline_racer.isPassGate()):
print('TERMINATE PROGRAM')
baseline_racer.stop_image_callback_thread()
baseline_racer.stop_control_thread()
baseline_racer.reset_race()
kp_vel_z=0.4,
kd_vel_z=0.0,
kp_yaw=3.0,
kd_yaw=0.1)
client.setTrajectoryTrackerGains(traj_tracker_gains, vehicle_name=drone_name)
time.sleep(0.2)
# Start race
client.simStartRace(tier=tier)
# Take off
# client.takeoffAsync().join()
# client.reset()
start_position = client.simGetVehiclePose(vehicle_name=drone_name).position
# # print(start_position)
takeoff_waypoint = airsim.Vector3r(start_position.x_val, start_position.y_val,
start_position.z_val - takeoff_height)
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=viz_traj,
viz_traj_color_rgba=viz_traj_color_rgba,
vehicle_name=drone_name).join()
print(client.simGetLastGatePassed(drone_name))
# Gates
gate_names_sorted_bad = sorted(client.simListSceneObjects("Gate.*"))
gate_indices_bad = [
def main(args):
# ensure you have generated the neurips planning settings file by running python generate_settings_file.py
pygame.init()
done = False
pygame.joystick.init()
baseline_racer = BaselineRacer(drone_name="drone_1", viz_traj=args.viz_traj, viz_image_cv2=args.viz_image_cv2)
baseline_racer.load_level(args.level_name)
baseline_racer.start_race(args.race_tier)
baseline_racer.initialize_drone()
baseline_racer.takeoff_with_moveOnSpline()
baseline_racer.get_ground_truth_gate_poses()
baseline_racer.start_image_callback_thread()
# baseline_racer.start_odometry_callback_thread()
# baseline_racer.airsim_client.moveByVelocityAsync(0, 1, 0, duration = 10).join()
duration = 0.05
gain = 5
yaw_gain = 50
while not done:
pygame.event.get()
joystick = pygame.joystick.Joystick(0)
joystick.init()
# -1 ~ 1 value
# -1 :
# x_val = -joystick.get_axis(1)
# y_val = joystick.get_axis(0)
# z_val = joystick.get_axis(4)
# yaw_val = joystick.get_axis(3)
# x_val = -1.2*joystick.get_axis(4)
# y_val = joystick.get_axis(3)
# z_val = 1.3*joystick.get_axis(1)
# yaw_val = 2*joystick.get_axis(0)
x_val = -1.2*joystick.get_axis(1)
y_val = joystick.get_axis(3)
z_val = 1.3*joystick.get_axis(4)
yaw_val = 2*joystick.get_axis(0)
reset_button = joystick.get_button(5)
quit_button = joystick.get_button(4)
# print(x_val, y_val, z_val, yaw_val)
drone_vel_vec = airsim.Vector3r(x_val, y_val, z_val)
world_vel_vec = baseline_racer.get_world_frame_vel_from_drone_frame_vel(baseline_racer.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").orientation, drone_vel_vec)
# print(world_vel_vec.x_val, world_vel_vec.y_val, world_vel_vec.z_val)
drone_pos = baseline_racer.airsim_client.simGetVehiclePose("drone_1")
print(drone_pos)
baseline_racer.airsim_client.moveByVelocityAsync(world_vel_vec.x_val*gain, world_vel_vec.y_val*gain, world_vel_vec.z_val*gain, duration = duration, yaw_mode=airsim.YawMode(True,yaw_val*yaw_gain)).join()
# baseline_racer.airsim_client.moveByAngleRatesThrottleAsync(0, 0, yaw_val*gain, 0, duration = duration).join()
if(quit_button):
done = True
if(reset_button):
baseline_racer.reset_race()
baseline_racer.start_race()
baseline_racer.initialize_drone()
baseline_racer.takeoff_with_moveOnSpline()
baseline_racer.get_ground_truth_gate_poses()
pygame.quit()
pygame.init()
pygame.joystick.init()
baseline_racer.stop_image_callback_thread()
# baseline_racer.stop_odometry_callback_thread()
baseline_racer.reset_race()
pygame.quit()
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 control_callback(self):
Dist = 3.2
if (self.next_gate_idx < 3):
velocity_x_o = 6
Dist = -2
elif (self.next_gate_idx == 3):
velocity_x_o = 6
Dist = 1
elif(self.next_gate_idx == 4): # the small gate
velocity_x_o = 5.4
Dist = 1
elif(self.next_gate_idx == 5):
velocity_x_o = 5.5
Dist = -1
elif(self.next_gate_idx == 6): # the wall
velocity_x_o = 8
Dist = -2
elif(self.next_gate_idx == 7 or self.next_gate_idx == 8): # behind the tree
velocity_x_o = 7
Dist = -3
elif(self.next_gate_idx == 9): # before the long one
velocity_x_o = 7
Dist = 1
# elif(self.next_gate_idx == 10):
# velocity_x_o = 7
# Dist = 1
elif(self.next_gate_idx == 12): # the small gate before the big gate
velocity_x_o = 6
Dist = 1
elif(self.next_gate_idx == 13): # the big gate
velocity_x_o = 12
Dist = -3
elif(self.next_gate_idx == 18):
velocity_x_o = 3.5
elif (self.next_gate_idx == 19):
Dist = 3.5
velocity_x_o = 3
elif(self.next_gate_idx == 20):
Dist = -2
velocity_x_o = 6
else:
velocity_x_o = 6 # 3.8
duration = 0.05
last_gate_idx = self.airsim_client.simGetLastGatePassed("drone_1")
if(last_gate_idx < 30):
if(last_gate_idx == self.next_gate_idx):
self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position
self.next_gate_idx = last_gate_idx + 1
print(str(self.next_gate_idx) + " PASS")
elif(self.next_gate_idx == 13 and self.isPassGate()):
print("using isPassGate()")
self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position
print(str(self.next_gate_idx) + " PASS")
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(self.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(13, 0, 0))
# self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val - 3, duration = 1).join()
elif(self.next_gate_idx == 17 and self.isPassGate()):
print("using isPassGate() gate 17: Go ahead")
self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position
print(str(self.next_gate_idx) + " PASS")
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(self.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(6, 0, 0))
self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration = 1).join()
elif(self.next_gate_idx == 20 and self.isPassGate()):
print("using isPassGate()")
self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position
print(str(self.next_gate_idx) + " PASS")
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(self.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(2, 0, 0))
self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration = 2).join()
elif(self.next_gate_idx == 19 and self.isPassGate()):
print("using isPassGate()")
self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position
print(str(self.next_gate_idx) + " PASS")
elif(self.next_gate_idx == 5 and self.isPassGate()):
print("using isPassGate()")
self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position
print(str(self.next_gate_idx) + " PASS")
elif(self.next_gate_idx == 7 and self.gate6_go_ahead == False):
print("gate 6: go ahead")
self.pass_position_vec = self.airsim_client.simGetVehiclePose(vehicle_name = "drone_1").position
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(self.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(12, 0, 0))
self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val + 1.2, duration = 2).join()
self.gate6_go_ahead = True
print(str(self.next_gate_idx) + " PASS")
if self.detect_flag :
with self.img_mutex:
# velocity_x_o = ((1-self.vision_lam) * velocity_x_o + self.vision_lam * self.prev_vel)
#print("detection control")
####################model######################
## box_width : real_gate_width = u : distance_y
## distance_y = 1.5 * u / box_width
self.curr_time = time.time()
param_A = 13.401
param_B = -1.976
self.estimate_depth = param_A * np.exp(param_B * self.W)
#############Control Gain param################
velocity_x = velocity_x_o + (self.estimate_depth - Dist)*0.187 - abs(self.distance_y_curr) * 0.2 #- abs(self.distance_z_curr) * 0.1
Vely_P_Gain = velocity_x*0.62 # 0.47 is good when vel 3.8 #0.9 is good when vel = 3 #0.43
Vely_D_Gain = velocity_x*0.17
Velz_P_Gain = velocity_x*0.62 # 0.49 is good when vel 3.8 #0.9 is good when vel = 3 #0.43
Velz_D_Gain = velocity_x*0.09
Yaw_P_Gain = velocity_x*0.7 #1.5 is good when vel = 3
Yaw_D_Gain = 0.07
##################Get Error####################
Dead_zone = 0.025
if(self.next_gate_idx == 20):
self.distance_y_curr = (1.5 * (self.Mx - 0.50) / self.W)
self.distance_z_curr = (1.5 * (self.My - 0.40) / self.H)
if abs((self.Mx - 0.50)) < Dead_zone:
self.distance_y_curr = 0
if abs((self.My - 0.42) ) < Dead_zone:
self.distance_z_curr = 0
elif(self.next_gate_idx == 19):
self.distance_y_curr = (1.5 * (self.Mx - 0.80) / self.W)
self.distance_z_curr = (1.5 * (self.My - 0.42) / self.H)
if abs((self.Mx - 0.80)) < Dead_zone:
self.distance_y_curr = 0
if abs((self.My - 0.42) ) < Dead_zone:
self.distance_z_curr = 0
elif(self.next_gate_idx == 4): # the small gate
self.distance_y_curr = (1.5 * (self.Mx - 0.55) / self.W)
self.distance_z_curr = (1.5 * (self.My - 0.50) / self.H)
if abs((self.Mx - 0.55)) < Dead_zone:
self.distance_y_curr = 0
if abs((self.My - 0.50) ) < Dead_zone:
self.distance_z_curr = 0
elif(self.next_gate_idx == 13): # the big gate
self.distance_y_curr = (1.5 * (self.Mx - 0.50) / self.W)
self.distance_z_curr = (1.5 * (self.My - 0.7) / self.H)
if abs((self.Mx - 0.50)) < Dead_zone:
self.distance_y_curr = 0
if abs((self.My - 0.7) ) < Dead_zone:
self.distance_z_curr = 0
elif(self.next_gate_idx == 15): # the big curve after the big gate
self.distance_y_curr = (1.5 * (self.Mx - 0.55) / self.W)
self.distance_z_curr = (1.5 * (self.My - 0.50) / self.H)
if abs((self.Mx - 0.55)) < Dead_zone:
self.distance_y_curr = 0
if abs((self.My - 0.50) ) < Dead_zone:
self.distance_z_curr = 0
else:
self.distance_y_curr = (1.5 * (self.Mx - 0.5) / self.W)
self.distance_z_curr = (1.5 * (self.My - 0.48) / self.H)
if abs((self.Mx - 0.50)) < Dead_zone:
self.distance_y_curr = 0
if abs((self.My - 0.48) ) < Dead_zone:
self.distance_z_curr = 0
self.desired_yaw_curr = math.atan2(self.distance_y_curr, self.estimate_depth) *57.2859
# print(self.desired_yaw_curr)
if self.chk_first_flag:
cmd_vel_x = velocity_x#velocity_x*(1+0.1*(self.estimate_depth-6))
cmd_vel_y = self.distance_y_curr*Vely_P_Gain
cmd_vel_z = self.distance_z_curr*Velz_P_Gain
cmd_yaw = self.desired_yaw_curr*Yaw_P_Gain
self.chk_first_flag = False
else:
dt = self.curr_time - self.prev_time
df_distance_y = (self.distance_y_curr - self.distance_y_prev)/dt
df_distance_z = (self.distance_z_curr - self.distance_z_prev)/dt
df_yaw = (self.desired_yaw_curr - self.desired_yaw_prev)/dt
#print("[df_y, df] | ", df_distance_y, dt)
cmd_vel_x = velocity_x#velocity_x*(1+0.1*(self.estimate_depth-6))
cmd_vel_y = self.distance_y_curr*Vely_P_Gain + df_distance_y*Vely_D_Gain
cmd_vel_z = self.distance_z_curr*Velz_P_Gain + df_distance_z*Velz_D_Gain
cmd_yaw = self.desired_yaw_curr*Yaw_P_Gain + df_yaw*Yaw_D_Gain
velocity_mag = math.sqrt(cmd_vel_x**2 + cmd_vel_y**2 + cmd_vel_z**2)
velocity_gain = ((1-self.vision_lam) * velocity_mag + self.vision_lam * self.prev_vel) / velocity_mag
cmd_vel_x *= velocity_gain
cmd_vel_y *= velocity_gain
cmd_vel_z *= velocity_gain
velocity_vector_drone = airsim.Vector3r(cmd_vel_x, cmd_vel_y, cmd_vel_z)
v_pose = self.airsim_client.simGetVehiclePose(vehicle_name="drone_1")
velocity_vector_world = self.get_world_frame_vel_from_drone_frame_vel(v_pose.orientation, velocity_vector_drone)
####################Do Control#################
# print(" ", cmd_yaw)
self.airsim_client.moveByVelocityAsync(velocity_vector_world.x_val, velocity_vector_world.y_val, velocity_vector_world.z_val, duration = duration, yaw_mode=airsim.YawMode(True,cmd_yaw)).join()
#print("[E_Depth Y_vel Z_vel Yaw velocity_x] : ", self.estimate_depth, " | ", cmd_vel_y, " | ", cmd_vel_z, " | ", desired_yaw_curr, " | ", velocity_x)
# if(self.next_gate_idx > 18):
# print("[Depth disY desiredYaw] : ", self.estimate_depth, " | ", self.distance_y_curr, " | ", self.desired_yaw_curr)
# #print("[E_Depth] : ", self.estimate_depth)
################update variables###############
self.prev_time = self.curr_time
self.distance_y_prev = self.distance_y_curr
self.distance_z_prev = self.distance_z_curr
self.desired_yaw_prev = self.desired_yaw_curr
self.position_control_on = False
self.prev_vel = math.sqrt(velocity_vector_world.x_val ** 2 + velocity_vector_world.y_val ** 2 + velocity_vector_world.z_val ** 2)
elif(self.next_gate_idx < len(self.gate_poses_ground_truth)):
################################################
# get approximate yaw vector
##############################################
gate_pose = self.gate_poses_ground_truth[self.next_gate_idx].position
gate_ori = self.gate_poses_ground_truth[self.next_gate_idx].orientation
# next_gate_pose = self.gate_poses_ground_truth[self.next_gate_idx + 1].position
# next_gate_ori = self.gate_poses_ground_truth[self.next_gate_idx +1].orientation
drone_pose = self.airsim_client.simGetVehiclePose("drone_1").position
drone_ori = self.airsim_client.simGetVehiclePose("drone_1").orientation
error_pose_vec = gate_pose - drone_pose
# print(error_pose_vec)
error_pose_vec_in_drone = self.get_drone_frame_vector_from_world_frame_vector(drone_ori, error_pose_vec)
mag_error_pose_vec = math.sqrt(error_pose_vec_in_drone.x_val ** 2 + error_pose_vec_in_drone.y_val ** 2)
angle = math.acos(error_pose_vec_in_drone.x_val / mag_error_pose_vec)*57.2859
if (error_pose_vec_in_drone.y_val < 0):
angle = -angle
################################################
# get approximate yaw vector
##############################################
gate_pose_array = [gate_pose.x_val, gate_pose.y_val, gate_pose.z_val]
drone_pose_array = [drone_pose.x_val, drone_pose.y_val, drone_pose.z_val]
# next_gate_pose_array = [next_gate_pose.x_val, next_gate_pose.y_val, next_gate_pose.z_val]
target_lambda_pose = self.get_lam_point(gate_pose_array, drone_pose_array, self.lam)
target_lambda_z_pose = self.get_lam_point(gate_pose_array, drone_pose_array, self.lam_z)
## ground z val = 3.5
if(target_lambda_z_pose[2] > -1):
target_lambda_z_pose[2] = -1
target_pose = gate_pose_array
if(target_pose[2] > -1):
target_pose[2] = -1
# target_lambda_pose = gate_pose_array
# next_target_pose = next_gate_pose_array
# vel_error = self.pass_position_vec - airsim.Vector3r(target_lambda_pose[0], target_lambda_pose[1], target_pose[2])
vel_error = airsim.Vector3r(target_lambda_pose[0], target_lambda_pose[1], target_pose[2]) - drone_pose
vel_mag = math.sqrt(vel_error.x_val ** 2 + vel_error.y_val ** 2 + vel_error.z_val ** 2)
vel_fun = 0.80 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543
#########################################################
drone_vel_for_start = airsim.Vector3r(6,0,0)
world_vel_for_start = self.get_world_frame_vel_from_drone_frame_vel(drone_ori, drone_vel_for_start)
#########################################################
# print(drone_pose.z_val)
if(self.next_gate_idx == 7): # the gate behind the tree
vel_fun = 0.75 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543
self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle))
# elif(self.next_gate_idx == 10):
# vel_fun = 0.3 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543
# self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle))
elif(self.next_gate_idx == 0): # start gate
vel_fun = 0.9 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543
self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], self.pass_position_vec.z_val - 1, velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle))
elif(self.next_gate_idx == 4):
vel_fun = 0.9 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543
self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1] - 1.5, target_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle))
elif(self.next_gate_idx == 8 or self.next_gate_idx == 9):
vel_fun = 0.8 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543
self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_pose[2] + 1.5, velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle))
elif(self.next_gate_idx == 13):
vel_fun = 0.7 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543
self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle))
elif(self.next_gate_idx == 14):
vel_fun = 0.8 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543
self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1] - 1, target_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle))
elif(self.next_gate_idx == 15):
vel_fun = 0.9 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543
self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1] - 1, target_pose[2] + 5, velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle))
elif(self.next_gate_idx == 20):
vel_fun = 0.5 * vel_mag + (-0.2) * (abs(angle) / vel_mag) #0.543
self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_pose[2] + 3, velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle))
else:
self.airsim_client.moveToPositionAsync(target_lambda_pose[0], target_lambda_pose[1], target_lambda_z_pose[2], velocity = vel_fun, yaw_mode=airsim.YawMode(True,angle))
self.prev_vel = vel_fun
# 'z_val': 1.6281375885009766},
# 'linear_velocity': <Vector3r> { 'x_val': -6.9823994636535645,
# 'y_val': 4.4047698974609375,
# 'z_val': -0.12542438507080078},
# 'orientation': <Quaternionr> { 'w_val': 0.9962534308433533,
# 'x_val': -0.01565200462937355,
# 'y_val': -0.020513707771897316,
# 'z_val': -0.08254285156726837},
# 'position': <Vector3r> { 'x_val': -17.122417449951172,
# 'y_val': 45.57592010498047,
# 'z_val': -47.230995178222656}}
planner = planning.RapidPlanner(0,19.72,6.28,0.05,0.05)
state = asim.KinematicsState()
state.position = asim.Vector3r(77.43971252, -96.87151337, -5.48000002)
# state.linear_velocity = asim.Vector3r(7.5843505859375, 0.7305274605751038, 6.088780403137207)
# state.linear_acceleration = asim.Vector3r(-2.003018379211426, -0.30256304144859314, -5.39830207824707)
# state.orientation = asim.Quaternionr(-0.012809118255972862, 0.05772315710783005, -0.03406976908445358,0.9976689219474792)
# state.position.x_val = 6.373129367828369
# state.position.y_val = 81.43741607666016
# state.position.z_val = -42.87995529174805
# path = planner.getShortestPath(state, numpy.array( [ 6.373129367828369 , 81.43741607666016 , -43.87995529174805]))
path = planner.getShortestPath(state, numpy.array([-111.93122864, 120.21295929 , -46.08000031 , -0.64279248 , 0.76604036,0.]))
# path2 = planner.getExtendedPath(numpy.array([ 0, -1 , -20]))
# raw_path = numpy.concatenate((path, path2))
# path2 = planner.getExtendedPath(numpy.array([ 10.388415, 80.77406, -43.579998]))
# path3 = planner.getExtendedPath(numpy.array([ 18.110466 ,76.26078, -43.579998]))
def control_callback(self):
if (self.next_gate_idx < 10):
velocity_x_o = 6
elif (self.next_gate_idx == 8):
velocity_x_o = 5.5
elif (self.next_gate_idx == 20):
velocity_x_o = 7
elif (self.next_gate_idx < 12):
velocity_x_o = 7
elif (self.next_gate_idx == 21):
velocity_x_o = 10
elif (self.next_gate_idx == 18):
velocity_x_o = 7
else:
velocity_x_o = 9
duration = 0.05
velocity_x_o -= 1
if (self.isPassGate()):
self.go_ahead_flag = False
########### TaeYeon ############
if (self.next_gate_idx == 9):
print("pass gate next gate idx 9: go ahead")
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(
self.airsim_client.simGetVehiclePose(
"drone_1").orientation, airsim.Vector3r(8, 0, 0))
self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val,
drone_heading_vec.y_val,
drone_heading_vec.z_val,
duration=1).join()
if (self.next_gate_idx == 10):
print("pass gate next gate idx 10: go ahead")
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(
self.airsim_client.simGetVehiclePose(
"drone_1").orientation, airsim.Vector3r(4, 2, 0))
self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val,
drone_heading_vec.y_val,
drone_heading_vec.z_val,
duration=1).join()
if (self.next_gate_idx == 14):
print("pass gate next gate idx 14: go ahead")
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(
self.airsim_client.simGetVehiclePose(
"drone_1").orientation, airsim.Vector3r(8, 0, 0))
self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val,
drone_heading_vec.y_val,
drone_heading_vec.z_val,
duration=1).join()
if (self.next_gate_idx == 16):
print("pass gate next gate idx 16: go ahead")
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(
self.airsim_client.simGetVehiclePose(
"drone_1").orientation, airsim.Vector3r(3, 0, 0))
self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val,
drone_heading_vec.y_val,
drone_heading_vec.z_val,
duration=0.5).join()
if (self.next_gate_idx == 17):
print("pass gate next gate idx 17: go ahead")
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(
self.airsim_client.simGetVehiclePose(
"drone_1").orientation, airsim.Vector3r(8, 8, 2))
self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val,
drone_heading_vec.y_val,
drone_heading_vec.z_val,
duration=3).join()
if (self.next_gate_idx == 18):
print("pass gate next gate idx 18: go ahead")
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(
self.airsim_client.simGetVehiclePose(
"drone_1").orientation, airsim.Vector3r(4, -3, 0))
self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val,
drone_heading_vec.y_val,
drone_heading_vec.z_val,
duration=1).join()
if (self.next_gate_idx == 19):
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(
self.airsim_client.simGetVehiclePose(
"drone_1").orientation, airsim.Vector3r(6, 0, 0))
self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val,
drone_heading_vec.y_val,
drone_heading_vec.z_val,
duration=1).join()
# if(self.next_gate_idx == 20):
# drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(self.airsim_client.simGetVehiclePose("drone_1").orientation, airsim.Vector3r(10, -4, -10))
# self.airsim_client.moveByVelocityAsync(drone_heading_vec.x_val, drone_heading_vec.y_val, drone_heading_vec.z_val, duration = 2.5).join()
if (self.detect_flag
and (self.prev_vel_mag < 10 or self.next_gate_idx == 9
or self.next_gate_idx == 10)):
with self.img_mutex:
# velocity_x_o = ((1-self.vision_lam) * velocity_x_o + self.vision_lam * self.prev_vel)
#print("detection control")
####################model######################
## box_width : real_gate_width = u : distance_y
## distance_y = 1.5 * u / box_width
self.curr_time = time.time()
param_A = 6.834360049
param_B = -2.999596062
self.estimate_depth = param_A * np.exp(param_B * self.Area)
#############Control Gain param################
Dist = 10
velocity_x = velocity_x_o + (
self.estimate_depth - Dist) * 0.18 - abs(
self.distance_y_curr) * 0.3 - abs(
self.distance_z_curr) * 0.3 #0.35
Vely_P_Gain = velocity_x * 0.55 #0.7
Vely_D_Gain = velocity_x * 0.2
Velz_P_Gain = velocity_x * 0.62
Velz_D_Gain = velocity_x * 0.09
Yaw_P_Gain = velocity_x * 0.55 #1.5 is good when vel = 3
Yaw_D_Gain = 0.07
##################Get Error####################
self.distance_y_curr = (1.5 * (self.Mx - 0.5) / self.W)
self.distance_z_curr = (1.5 * (self.My - 0.48) / self.H)
self.desired_yaw_curr = math.atan2(
self.distance_y_curr, self.estimate_depth) * 57.2859
if self.chk_first_flag:
cmd_vel_x = velocity_x #velocity_x*(1+0.1*(self.estimate_depth-6))
cmd_vel_y = self.distance_y_curr * Vely_P_Gain
cmd_vel_z = self.distance_z_curr * Velz_P_Gain
cmd_yaw = self.desired_yaw_curr * Yaw_P_Gain
self.chk_first_flag = False
else:
dt = self.curr_time - self.prev_time
df_distance_y = (self.distance_y_curr -
self.distance_y_prev) / dt
df_distance_z = (self.distance_z_curr -
self.distance_z_prev) / dt
df_yaw = (self.desired_yaw_curr -
self.desired_yaw_prev) / dt
#print("[df_y, df] | ", df_distance_y, dt)
cmd_vel_x = velocity_x #velocity_x*(1+0.1*(self.estimate_depth-6))
cmd_vel_y = self.distance_y_curr * Vely_P_Gain + df_distance_y * Vely_D_Gain
cmd_vel_z = self.distance_z_curr * Velz_P_Gain + df_distance_z * Velz_D_Gain
cmd_yaw = self.desired_yaw_curr * Yaw_P_Gain + df_yaw * Yaw_D_Gain
velocity_mag = math.sqrt(cmd_vel_x**2 + cmd_vel_y**2 +
cmd_vel_z**2)
velocity_gain = (
(1 - self.vision_lam) * velocity_mag +
self.vision_lam * self.prev_vel) / velocity_mag
cmd_vel_x *= velocity_gain
cmd_vel_y *= velocity_gain
cmd_vel_z *= velocity_gain
if (self.next_gate_idx == 21):
print("last gate detect control")
velocity_vector_drone = airsim.Vector3r(
cmd_vel_z, cmd_vel_y, cmd_vel_x)
else:
velocity_vector_drone = airsim.Vector3r(
cmd_vel_x, cmd_vel_y, cmd_vel_z)
v_pose = self.airsim_client.simGetVehiclePose(
vehicle_name="drone_1")
velocity_vector_world = self.get_world_frame_vel_from_drone_frame_vel(
v_pose.orientation, velocity_vector_drone)
####################Do Control#################
self.airsim_client.moveByVelocityAsync(
velocity_vector_world.x_val,
velocity_vector_world.y_val,
velocity_vector_world.z_val,
duration=duration,
yaw_mode=airsim.YawMode(True, cmd_yaw)).join()
#print("[E_Depth Y_vel Z_vel Yaw velocity_x] : ", self.estimate_depth, " | ", cmd_vel_y, " | ", cmd_vel_z, " | ", desired_yaw_curr, " | ", velocity_x)
#print("[E_Depth] : ", self.estimate_depth)
################update variables###############
self.prev_time = self.curr_time
self.distance_y_prev = self.distance_y_curr
self.distance_z_prev = self.distance_z_curr
self.desired_yaw_prev = self.desired_yaw_curr
self.position_control_on = False
self.prev_vel = math.sqrt(velocity_vector_world.x_val**2 +
velocity_vector_world.y_val**2 +
velocity_vector_world.z_val**2)
elif (self.next_gate_idx < len(self.gate_poses_ground_truth)):
################################################
# get approximate yaw vector
##############################################
gate_pose = self.gate_poses_ground_truth[
self.next_gate_idx].position
gate_ori = self.gate_poses_ground_truth[
self.next_gate_idx].orientation
drone_pose = self.airsim_client.simGetVehiclePose(
"drone_1").position
drone_ori = self.airsim_client.simGetVehiclePose(
"drone_1").orientation
error_pose_vec = gate_pose - drone_pose
error_pose_vec_in_drone = self.get_drone_frame_vector_from_world_frame_vector(
drone_ori, error_pose_vec)
mag_error_pose_vec = math.sqrt(error_pose_vec_in_drone.x_val**2 +
error_pose_vec_in_drone.y_val**2)
angle = math.acos(
error_pose_vec_in_drone.x_val / mag_error_pose_vec) * 57.2859
if (error_pose_vec_in_drone.y_val < 0):
angle = -angle
################################################
# get approximate yaw vector
##############################################
gate_pose_array = [
gate_pose.x_val, gate_pose.y_val, gate_pose.z_val
]
drone_pose_array = [
drone_pose.x_val, drone_pose.y_val, drone_pose.z_val
]
if (self.next_gate_idx > 17):
target_lambda_pose = self.get_lam_point(
gate_pose_array, drone_pose_array, 0.7)
elif (self.next_gate_idx > 11):
target_lambda_pose = self.get_lam_point(
gate_pose_array, drone_pose_array, 0.56)
elif (self.next_gate_idx == 20):
target_lambda_pose = self.get_lam_point(
gate_pose_array, drone_pose_array, 0.25)
else:
target_lambda_pose = self.get_lam_point(
gate_pose_array, drone_pose_array, self.lam)
target_lambda_z_pose = self.get_lam_point(gate_pose_array,
drone_pose_array,
self.lam_z)
# else:
# target_lambda_pose = self.get_lam_point(gate_pose_array, [self.pass_position_vec.x_val, self.pass_position_vec.y_val, self.pass_position_vec.z_val], self.lam)
# target_lambda_z_pose = self.get_lam_point(gate_pose_array, [self.pass_position_vec.x_val, self.pass_position_vec.y_val, self.pass_position_vec.z_val], self.lam_z)
# target_lambda_z_pose = [0 ,0 ,gate_pose_array[2]]
# if(self.next_gate_idx < 11):
# vel_error = self.pass_position_vec - airsim.Vector3r(target_lambda_pose[0], target_lambda_pose[1], target_pose[2])
# vel_mag = math.sqrt(vel_error.x_val ** 2 + vel_error.y_val ** 2 + vel_error.z_val ** 2)
# else:
vel_error = airsim.Vector3r(target_lambda_pose[0],
target_lambda_pose[1],
target_lambda_z_pose[2]) - drone_pose
vel_mag = math.sqrt(vel_error.x_val**2 + vel_error.y_val**2 +
vel_error.z_val**2)
# vel_fun = 0.54 * vel_mag + (-0.084) * (abs(angle) / vel_mag) #0.543
vel_fun = 0.5 * vel_mag + (-0.084) * (abs(angle) / vel_mag)
if (vel_mag < 2):
self.go_ahead_flag = True
if (self.next_gate_idx == 0):
drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(
self.airsim_client.simGetVehiclePose(
"drone_1").orientation, airsim.Vector3r(3, 0, 0))
self.airsim_client.moveByVelocityAsync(
drone_heading_vec.x_val,
drone_heading_vec.y_val,
drone_heading_vec.z_val,
duration=self.duration_move_ahead).join()
self.airsim_client.moveToPositionAsync(target_lambda_pose[0],
target_lambda_pose[1],
target_lambda_z_pose[2],
velocity=vel_fun,
yaw_mode=airsim.YawMode(
True, angle))
elif (self.next_gate_idx == 5):
self.airsim_client.moveToPositionAsync(target_lambda_pose[0],
target_lambda_pose[1],
gate_pose_array[2] + 10,
velocity=vel_fun)
elif (2 < self.next_gate_idx < 7):
self.airsim_client.moveToPositionAsync(target_lambda_pose[0],
target_lambda_pose[1],
gate_pose_array[2] + 10,
velocity=vel_fun,
yaw_mode=airsim.YawMode(
True, angle))
elif (self.next_gate_idx < 5):
self.airsim_client.moveToPositionAsync(target_lambda_pose[0],
target_lambda_pose[1],
target_lambda_z_pose[2],
velocity=vel_fun,
yaw_mode=airsim.YawMode(
True, angle))
elif (self.next_gate_idx == 9):
pass
elif (self.next_gate_idx == 10):
start_drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(
self.airsim_client.simGetVehiclePose(
"drone_1").orientation, airsim.Vector3r(6, 0, 0))
self.airsim_client.moveByVelocityAsync(
start_drone_heading_vec.x_val,
start_drone_heading_vec.y_val,
start_drone_heading_vec.z_val,
duration=0.4).join()
elif (self.next_gate_idx == 21):
vel_error = airsim.Vector3r(
target_lambda_pose[0], target_lambda_pose[1],
target_lambda_z_pose[2] + 20) - drone_pose
vel_mag = math.sqrt(vel_error.x_val**2 + vel_error.y_val**2 +
vel_error.z_val**2)
# vel_fun = 0.3 * vel_mag + (-0.084) * (abs(angle) / vel_mag) #0.543
vel_fun = 8
self.airsim_client.moveToPositionAsync(
target_lambda_pose[0],
target_lambda_pose[1],
target_lambda_z_pose[2] + 10,
velocity=vel_fun,
yaw_mode=airsim.YawMode(True, angle))
else:
if (self.go_ahead_flag == False):
if (self.next_gate_idx > 10):
self.airsim_client.moveToPositionAsync(
target_lambda_pose[0],
target_lambda_pose[1],
target_lambda_z_pose[2],
velocity=vel_fun * 1.2,
yaw_mode=airsim.YawMode(True, angle))
elif (self.next_gate_idx == 20):
self.airsim_client.moveToPositionAsync(
target_lambda_pose[0],
target_lambda_pose[1],
target_lambda_z_pose[2] - 10,
velocity=vel_fun * 1.3,
yaw_mode=airsim.YawMode(True, angle))
else:
self.airsim_client.moveToPositionAsync(
target_lambda_pose[0],
target_lambda_pose[1],
target_lambda_z_pose[2],
velocity=vel_fun,
yaw_mode=airsim.YawMode(True, angle))
else:
start_drone_heading_vec = self.get_world_frame_vel_from_drone_frame_vel(
self.airsim_client.simGetVehiclePose(
"drone_1").orientation, airsim.Vector3r(3, 0, 0))
self.airsim_client.moveByVelocityAsync(
start_drone_heading_vec.x_val,
start_drone_heading_vec.y_val,
start_drone_heading_vec.z_val,
duration=0.5).join()
self.prev_vel = vel_fun
self.prev_vel_mag = vel_mag
def rotate_vector(q, v):
v_quat = v.to_Quaternionr()
v_rotated_ = q * v_quat * q.inverse()
return airsim.Vector3r(x_val=v_rotated_.x_val,
y_val=v_rotated_.y_val,
z_val=v_rotated_.z_val)
from scipy.interpolate import CubicSpline, CubicHermiteSpline
import airsimneurips as airsim
import cvxpy as cp
import numpy as np
import time
gate_dimensions = [1.6, 1.6]
gate_facing_vector = airsim.Vector3r(x_val=0, y_val=1, z_val=0)
def rotate_vector(q, v):
v_quat = v.to_Quaternionr()
v_rotated_ = q * v_quat * q.inverse()
return airsim.Vector3r(x_val=v_rotated_.x_val,
y_val=v_rotated_.y_val,
z_val=v_rotated_.z_val)
class SplinedTrack:
"""This class represents a Track defined by Gates.
A spline is fitted through the Gates with tangential constraints.
This spline is then sampled at 2048 points.
"""
def __init__(self, gate_poses):
self.gates = gate_poses
self.n_gates = np.size(gate_poses, 0)
positions = np.array(
[pose.position.to_numpy_array() for pose in gate_poses])
dists = np.linalg.norm(positions[1:, :] - positions[:-1, :], axis=1)
self.arc_length = np.zeros(shape=self.n_gates)
def run(self):
self.get_ground_truth_gate_poses(
) # Tier 2 gives noisy prior of gate poses
mu_1_airsimvector = self.gate_poses_ground_truth[0].position
mu_1 = np.reshape(
np.array([
mu_1_airsimvector.x_val, mu_1_airsimvector.y_val,
mu_1_airsimvector.z_val
]), (3, 1))
sigma_1 = 50 * np.identity(3)
sigma_2 = 50 * np.identity(3)
while self.airsim_client.isApiControlEnabled(
vehicle_name=self.drone_name):
# Take image
response = self.airsim_client.simGetImages([
airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False,
False)
])
img1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8)
image_rgb = img1d.reshape(response[0].height, response[0].width, 3)
# Get quad state when the image was taken
state = self.airsim_client.simGetVehiclePose()
rot_matrix_quad2global = self.get_rotation_matrix_quad_frame_to_global_frame(
state)
# Get gate mask
mask1 = cv2.inRange(image_rgb, self.lower_green1,
self.upper_green1)
mask2 = cv2.inRange(image_rgb, self.lower_green2,
self.upper_green2)
mask3 = cv2.inRange(image_rgb, self.lower_green3,
self.upper_green3)
mask4 = cv2.inRange(image_rgb, self.lower_green4,
self.upper_green4)
mask = mask1 + mask2 + mask3 + mask4
dilated_gate = cv2.dilate(mask, self.kernel, iterations=8)
gate_contours = None
gate_contours, ____ = cv2.findContours(dilated_gate, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
cv2.imshow("mask", mask)
cv2.imshow("dilated_gate", dilated_gate)
cv2.waitKey(1)
gate_corners = None
smallest_measurement_error = 100
# find four gate corners that give the smallest error between the gate center and mean
# check each contour
for contour in gate_contours:
gate_contour_epsilon = 0.01 * cv2.arcLength(contour, True)
gate_contour_approx = cv2.approxPolyDP(contour,
gate_contour_epsilon,
True)
gate_corners = cv2.convexHull(gate_contour_approx)
gate_corner_combinations = [None]
gate_corner_combinations = list(combinations(gate_corners, 4))
# check for each combination of four gate corner points within each contour
for comb in gate_corner_combinations:
four_gate_corners = np.empty((0, 2))
for i in comb:
i = np.reshape(i, (1, 2))
four_gate_corners = np.append(four_gate_corners,
i,
axis=0)
measurement_error, waypoint_glob_1, waypoint_glob_2, gate_center_pixel, gate_corners_plot = self.find_measurement_error(
four_gate_corners, mu_1, state, rot_matrix_quad2global)
aspect_ratio_too_large = self.find_aspect_ratio(
four_gate_corners)
if measurement_error < smallest_measurement_error and aspect_ratio_too_large == False:
waypoint_1_best = copy.deepcopy(waypoint_glob_1)
waypoint_2_best = copy.deepcopy(waypoint_glob_2)
smallest_measurement_error = copy.deepcopy(
measurement_error)
gate_center_pixel_best = copy.deepcopy(
gate_center_pixel)
gate_corners_plot_best = copy.deepcopy(
gate_corners_plot)
next_gate_position_dist = abs(
np.linalg.norm(
np.reshape(
np.array([
state.position.x_val, state.position.y_val,
state.position.z_val
]), (3, 1)) - mu_1))
self.moveonspline_count += 1
# If the smallest error if large, then continue toward the best estimate of the gate location
if smallest_measurement_error > 5.0:
self.no_gate_count += 1
print("Gate NOT detected")
if next_gate_position_dist > 30.0:
vmax = 10.0
amax = 3.0
no_gate_count_max = 15
else:
vmax = 5.0
amax = 3.0
no_gate_count_max = 3
if self.no_gate_count == no_gate_count_max and next_gate_position_dist >= self.largest_next_gate_position_dist:
self.no_gate_count = 0
self.too_close_count = 0
print("Move toward best estimate", next_gate_position_dist)
self.airsim_client.moveOnSplineAsync(
[airsim.Vector3r(mu_1[0, 0], mu_1[1, 0], mu_1[2, 0])],
vel_max=vmax,
acc_max=amax,
add_position_constraint=True,
add_velocity_constraint=True,
add_acceleration_constraint=True,
viz_traj=self.viz_traj,
viz_traj_color_rgba=self.viz_traj_color_rgba,
vehicle_name=self.drone_name,
replan_from_lookahead=False,
replan_lookahead_sec=1.0)
# else, input the measurement to the Kalman Filter and proceed to the updated estimate
else:
self.no_gate_count = 0
self.measurement_count += 1
if self.measurement_count == 1:
mu_2 = np.reshape(waypoint_2_best,
(3, 1)) #initialize mu_2
# filter both waypoints
mu_1, sigma_1 = self.kalman_filter(
mu_1, sigma_1, np.reshape(waypoint_1_best, (3, 1)))
mu_2, sigma_2 = self.kalman_filter(
mu_2, sigma_2, np.reshape(waypoint_2_best, (3, 1)))
waypoint_1 = airsim.Vector3r(mu_1[0, 0], mu_1[1, 0], mu_1[2,
0])
waypoint_2 = airsim.Vector3r(mu_2[0, 0], mu_2[1, 0], mu_2[2,
0])
if next_gate_position_dist > self.largest_next_gate_position_dist:
print("Gate detected, Measurement Taken")
print(self.measurement_count)
self.too_close_count = 0
#cv2.circle(image_rgb, (int(gate_center_pixel_best[0]), int(gate_center_pixel_best[1])), 10, (255, 0, 0), -1)
cv2.circle(image_rgb, (int(gate_corners_plot_best[0][0]),
int(gate_corners_plot_best[0][1])),
10, (255, 100, 0), -1)
cv2.circle(image_rgb, (int(gate_corners_plot_best[1][0]),
int(gate_corners_plot_best[1][1])),
10, (255, 0, 100), -1)
cv2.circle(image_rgb, (int(gate_corners_plot_best[2][0]),
int(gate_corners_plot_best[2][1])),
10, (255, 200, 0), -1)
cv2.circle(image_rgb, (int(gate_corners_plot_best[3][0]),
int(gate_corners_plot_best[3][1])),
10, (255, 0, 200), -1)
cv2.imshow("image_rgb", image_rgb)
cv2.waitKey(1)
if self.moveonspline_count >= 5:
if self.next_gate_idx >= len(
self.gate_poses_ground_truth) - 1:
self.next_gate_idx = -1
self.airsim_client.moveOnSplineAsync(
[
waypoint_1, waypoint_2,
self.gate_poses_ground_truth[self.next_gate_idx
+ 1].position
],
vel_max=7.0,
acc_max=5.0,
add_position_constraint=True,
add_velocity_constraint=True,
add_acceleration_constraint=True,
viz_traj=self.viz_traj,
viz_traj_color_rgba=self.viz_traj_color_rgba,
vehicle_name=self.drone_name,
replan_from_lookahead=False,
replan_lookahead_sec=1.0)
self.moveonspline_count = 0
# If too close to the gate don't collect measurements, continue to waypoint, reset filter with new mean and covariance
if next_gate_position_dist < self.largest_next_gate_position_dist:
print("Gate too close")
time.sleep(2)
self.no_gate_count = 0
self.too_close_count += 1
if self.too_close_count == 1:
print("reset gate too close")
self.next_gate_idx += 1
if self.next_gate_idx >= len(self.gate_poses_ground_truth):
self.next_gate_idx = 0
print("race end")
mu_1_airsimvector = self.gate_poses_ground_truth[
self.next_gate_idx].position
mu_1 = np.reshape(
np.array([
mu_1_airsimvector.x_val, mu_1_airsimvector.y_val,
mu_1_airsimvector.z_val
]), (3, 1))
sigma_1 = 50 * np.identity(3)
sigma_2 = 50 * np.identity(3)
self.measurement_count = 0
