def updateState(self, action, baseline_racer):
max_vel = -10
angle_threshold = 3
duration = 0.05
ground_z_vel = 0
if (action == 1):
baseline_racer.airsim_client.moveByVelocityAsync(max_vel,
0,
ground_z_vel,
duration=duration)
if (action == 2):
baseline_racer.airsim_client.moveByVelocityAsync(0,
max_vel,
ground_z_vel,
duration=duration)
if (action == 3):
baseline_racer.airsim_client.moveByVelocityAsync(0,
0,
max_vel,
duration=duration)
if (action == 4):
baseline_racer.airsim_client.moveByVelocityAsync(
0,
0,
ground_z_vel,
duration=duration,
yaw_mode=airsim.YawMode(False, angle_threshold))
if (action == 5):
baseline_racer.airsim_client.moveByVelocityAsync(-max_vel,
0,
ground_z_vel,
duration=duration)
if (action == 6):
baseline_racer.airsim_client.moveByVelocityAsync(0,
-max_vel,
ground_z_vel,
duration=duration)
if (action == 7):
baseline_racer.airsim_client.moveByVelocityAsync(0,
0,
-max_vel,
duration=duration)
if (action == 8):
baseline_racer.airsim_client.moveByVelocityAsync(
0,
0,
ground_z_vel,
duration=duration,
yaw_mode=airsim.YawMode(False, -angle_threshold))
return self.getState(baseline_racer)
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 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
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",
plot_transform=args.plot_transform,
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()
# angle_gain = 0.5
# throttle = 0.5
# duration = 10
# z = -5
# baseline_racer.airsim_client.moveByAngleRatesZAsync(0, angle_gain, 0, z = z, duration = duration).join()
# baseline_racer.airsim_client.moveByAngleRatesThrottleAsync(0, angle_gain, 0, throttle = throttle, duration = duration).join()
# baseline_racer.airsim_client.moveByRollPitchYawThrottleAsync(0, angle_gain, 0, throttle = throttle, duration = duration).join()
# baseline_racer.airsim_client.moveByRollPitchYawZAsync(0, angle_gain, 0, z = z, duration = duration).join()
# baseline_racer.airsim_client.moveByRollPitchYawrateThrottleAsync(0, angle_gain, 0, throttle = throttle, duration = duration).join()
# baseline_racer.airsim_client.moveByRollPitchYawrateZAsync(0, angle_gain, 0, z = z, duration = duration).join()
# time.sleep(10)
# baseline_racer.airsim_client.moveByVelocityAsync(1, 0, 0, duration = 1).join()
# baseline_racer.airsim_client.moveByVelocityAsync(0, 0, 0, duration = 1, yaw_mode = airsim.YawMode(False, 90)).join()
# baseline_racer.airsim_client.moveByVelocityAsync(1, 0, 0, duration = 1).join()
for i in range(len(baseline_racer.gate_poses_ground_truth)):
while (1):
gate_pose = baseline_racer.gate_poses_ground_truth[i].position
gate_ori = baseline_racer.gate_poses_ground_truth[i].orientation
drone_pose = baseline_racer.airsim_client.simGetVehiclePose(
"drone_1").position
drone_ori = baseline_racer.airsim_client.simGetVehiclePose(
"drone_1").orientation
gate_vec_in_drone = baseline_racer.get_drone_frame_vector_from_world_frame_vector(
drone_ori,
baseline_racer.get_gate_facing_vector_from_quaternion(
gate_ori))
angle = math.acos(gate_vec_in_drone.x_val) * (180 / math.pi)
if (gate_vec_in_drone.y_val < 0):
angle = -angle
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
]
baseline_racer.airsim_client.moveToPositionAsync(
gate_pose_array[0],
gate_pose_array[1],
gate_pose_array[2],
velocity=1,
yaw_mode=airsim.YawMode(True, angle))
if (baseline_racer.isPassGate()):
break
# if args.planning_baseline_type == "all_gates_at_once" :
# if args.planning_and_control_api == "moveOnSpline":
# baseline_racer.fly_through_all_gates_at_once_with_moveOnSpline().join()
# if args.planning_and_control_api == "moveOnSplineVelConstraints":
# baseline_racer.fly_through_all_gates_at_once_with_moveOnSplineVelConstraints().join()
# if args.planning_baseline_type == "all_gates_one_by_one":
# if args.planning_and_control_api == "moveOnSpline":
# baseline_racer.fly_through_all_gates_one_by_one_with_moveOnSpline().join()
# if args.planning_and_control_api == "moveOnSplineVelConstraints":
# baseline_racer.fly_through_all_gates_one_by_one_with_moveOnSplineVelConstraints().join()
baseline_racer.stop_image_callback_thread()
baseline_racer.stop_odometry_callback_thread()
baseline_racer.reset_race()
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()