def reset(self):
# camera
dronesim.installcamera([0,-15,180], 110, 63, 0.01, 500)
# state related property
position_hunter = [0.0, 0.0, 10.0] # x, y, z
orientation_hunter = [0.0, 0.0, 0.0] # roll, pitch, taw
#the position of target is generated randomly and should not exceed the vision range of hunter
position_target = (np.array([10.0, 0.0, 10.0]) + np.random.normal(0, 5)).tolist() # x, y, z
orientation_target = [0.0, 0.0, 0.0] # roll, pitch, yaw
self.roll_target, self.pitch_target, self.yaw_target, self.thrust_target = 0, 0, 0, 0
absolute_x, absolute_y, target_in_front = dronesim.projection(position_hunter, orientation_hunter, position_target, float(self.width), float(self.height))
distance = np.linalg.norm(np.array(position_hunter) - np.array(position_target))
# invalid initialization
while (not target_in_front or absolute_x > self.max_absolute_x or absolute_x < self.min_absolute_x or absolute_y > self.max_absolute_y or absolute_y < self.min_absolute_y or distance > self.max_initial_distance or distance < self.min_initial_distance):
position_target = (np.array([10.0, 0.0, 10.0]) + np.random.normal(0, 5)).tolist()
absolute_x, absolute_y, target_in_front = dronesim.projection(position_hunter, orientation_hunter, position_target, float(self.width), float(self.height))
distance = np.linalg.norm(np.array(position_hunter) - np.array(position_target))
dronesim.siminit(np.squeeze(np.asarray(position_hunter)),np.squeeze(np.asarray(orientation_hunter)), \
np.squeeze(np.asarray(position_target)),np.squeeze(np.asarray(orientation_target)), 20, 5)
self.init_pos_target = position_target
self.previous_distance = distance
self.state = self.get_state()
self.iteration = 0
self.episodes = 0
return self.state
def reset(self):
# camera
dronesim.installcamera([0, -15, 180], 110, 63, 0.01, 500)
# state related property
position_hunter = [0.0, 0.0, 10.0] # x, y, z
orientation_hunter = [0.0, 0.0, 0.0] # roll, pitch, taw
# position_hunter = np.matrix([44.41015975 -3.96066086 7.88967305])
# orientation_hunter = np.matrix([-11.276222823141545, 8.627114153116235, 27.256829024182572])
# position_target = np.matrix([17.49014858 -5.98676401 21.02755752])
#the position of target is generated randomly and should not exceed the vision range of hunter
position_target = (np.array([10.0, 0.0, 10.0]) +
np.random.normal(0, 5)).tolist() # x, y, z
orientation_target = [0.0, 0.0, 0.0] # roll, pitch, yaw
self.roll_target, self.pitch_target, self.yaw_target, self.thrust_target = 0, 0, 0, 0
# print('error c')
# print('lala: ', position_hunter, orientation_hunter, position_target, self.width, self.height)
absolute_x, absolute_y, target_in_front = dronesim.projection(
position_hunter, orientation_hunter, position_target,
float(self.width), float(self.height))
# print('position: ', position_hunter)
# print('error e')
# print('pos_hunter: ', position_hunter)
# print('pos_target: ', position_target)
distance = np.linalg.norm(
np.array(position_hunter) - np.array(position_target))
# invalid initialization
# print('error d')
while (not target_in_front or absolute_x > self.max_absolute_x
or absolute_x < self.min_absolute_x
or absolute_y > self.max_absolute_y
or absolute_y < self.min_absolute_y
or distance > self.max_initial_distance
or distance < self.min_initial_distance):
position_target = (np.array([10.0, 0.0, 10.0]) +
np.random.normal(0, 5)).tolist()
absolute_x, absolute_y, target_in_front = dronesim.projection(
position_hunter, orientation_hunter, position_target,
float(self.width), float(self.height))
distance = np.linalg.norm(
np.array(position_hunter) - np.array(position_target))
# print(position_hunter, orientation_hunter, position_target)
# position_hunter = np.matrix([38.5, 26.8, 30.3])
# orientation_hunter = np.matrix([20.5,12.9,84.9])
# position_target = np.matrix([16.4,8.8,39.9])
# print('error a')
dronesim.siminit(np.squeeze(np.asarray(position_hunter)),np.squeeze(np.asarray(orientation_hunter)), \
np.squeeze(np.asarray(position_target)),np.squeeze(np.asarray(orientation_target)), 20, 5)
# print('error b')
self.previous_distance = distance
self.state = self.get_state()
self.iteration = 0
self.episodes = 0
return self.state
def get_state(self):
position_hunter, orientation_hunter, acc_hunter, position_target, orientation_target, acc_target, thrust_hunter = dronesim.siminfo(
)
orientation_hunter = [
math.degrees(degree) for degree in orientation_hunter
]
orientation_target = [
math.degrees(degree) for degree in orientation_target
]
self.flag = False
try:
absolute_x, absolute_y, target_in_front = dronesim.projection(
position_hunter, orientation_hunter, position_target,
float(self.width), float(self.height))
except ValueError:
logger.info('###########################################')
logger.info('episodes: {}, iteration: {}'.format(
self.episodes, self.iteration))
logger.info('prev pos hunter: {}'.format(self.prev_pos_hunter))
logger.info('prev ori hunter: {}'.format(self.prev_ori_hunter))
logger.info('prev pos target: {}'.format(self.prev_pos_target))
logger.info('prev ori target: {}'.format(self.prev_ori_target))
logger.info('prev acc hunter: {}'.format(self.prev_acc_hunter))
logger.info('prev acc target: {}'.format(self.prev_acc_target))
logger.info('prev thrust hunter: {}'.format(
self.prev_thrust_hunter))
logger.info('hunter position: {}'.format(position_hunter))
logger.info('hunter ori: {}'.format(orientation_hunter))
logger.info('target position: {}'.format(position_target))
logger.info('target ori: {}'.format(orientation_target))
logger.info('hunter acc: {}'.format(acc_hunter))
logger.info('target acc: {}'.format(acc_target))
logger.info('thrust hunter: {}'.format(thrust_hunter))
logger.info('init pos target: {}'.format(self.init_pos_target))
logger.info('target action: {},{},{},{}'.format(
self.roll_target, self.pitch_target, self.yaw_target,
self.thrust_target))
logger.info('hunter action: {},{},{},{}'.format(
self.test_roll_hunter, self.test_pitch_hunter,
self.test_yaw_hunter, self.test_thrust_hunter))
self.flag = True # force program to terminate
# raise ValueError
return self.state
self.prev_pos_hunter = position_hunter
self.prev_ori_hunter = orientation_hunter
self.prev_pos_target = position_target
self.prev_ori_target = orientation_target
self.prev_acc_hunter = acc_hunter
self.prev_acc_target = acc_target
self.prev_thrust_hunter = thrust_hunter
'''
if (1 + self.iteration) % 200 == 0:
logger.info('episodes: {},iteration: {}'.format(self.episodes, self.iteration))
logger.info('pos hunter: {}'.format(position_hunter))
logger.info('pos target: {}'.format(position_target))
logger.info('ori hunter: {}'.format(orientation_hunter))
logger.info('ori target: {}'.format(orientation_target))
logger.info('acc hunter: {}'.format(acc_hunter))
logger.info('acc target: {}'.format(acc_target))
logger.info('thrust hunter: {}'.format(thrust_hunter))
logger.info('target action: {},{},{},{}'.format(self.roll_target, self.pitch_target, self.yaw_target, self.thrust_target))
logger.info('hunter action: {},{},{},{}'.format(self.test_roll_hunter, self.test_pitch_hunter, self.test_yaw_hunter, self.test_thrust_hunter))
'''
if target_in_front:
relative_x, relative_y = absolute_x / self.width, absolute_y / self.height
target_coordinate_in_view = np.array((relative_x, relative_y)).flatten(
) if target_in_front and self.min_absolute_x < absolute_x < self.max_absolute_x and self.min_absolute_y < absolute_y < self.max_absolute_y else np.array(
(-1, -1))
self.distance = np.linalg.norm(
np.array(position_hunter) - np.array(position_target))
# get distance within hunter and target
distance = np.array(
[self.distance / self.max_initial_distance]
) # if (target_coordinate_in_view == np.array((-1, -1)))[0] else np.array([0])
# maintain a 8-length deque
if self.coordinate_queue is None and self.distance_queue is None:
self.coordinate_queue = deque([target_coordinate_in_view] * 8)
self.distance_queue = deque([distance] * self.queue_length)
else:
self.coordinate_queue.append(target_coordinate_in_view)
self.coordinate_queue.popleft()
self.distance_queue.append(distance)
self.distance_queue.popleft()
coordinate_state = np.concatenate(list(self.coordinate_queue))
distance_state = np.concatenate(list(self.distance_queue))
# define state
state = np.concatenate([
np.array([
orientation_hunter[0] / self.max_roll_angle,
orientation_hunter[1] / self.max_pitch_angle,
orientation_hunter[2] / self.max_yaw_angle
]).flatten(),
np.array((thrust_hunter - self.min_absolute_thrust) /
self.thrust_sensity + self.min_thrust).flatten(),
coordinate_state, distance_state
], 0)
# if self.tmp_pos.any() != None :
# tmp_pos = self.tmp_pos
# self.tmp_pos = position_hunter
return state
def get_state(self):
position_hunter, orientation_hunter, acc_hunter, position_target, orientation_target, acc_target, thrust_hunter = dronesim.siminfo(
)
# print('ori: ', orientation_hunter)
orientation_hunter = [
math.degrees(degree) for degree in orientation_hunter
]
orientation_target = [
math.degrees(degree) for degree in orientation_target
]
try:
absolute_x, absolute_y, target_in_front = dronesim.projection(
position_hunter, orientation_hunter, position_target,
float(self.width), float(self.height))
except ValueError:
self.distance = 100
return self.state
if target_in_front:
relative_x, relative_y = absolute_x / self.width, absolute_y / self.height
target_coordinate_in_view = np.array((relative_x, relative_y)).flatten(
) if target_in_front and self.min_absolute_x < absolute_x < self.max_absolute_x and self.min_absolute_y < absolute_y < self.max_absolute_y else np.array(
(-1, -1))
# print('!!!!####')
# print(position_hunter, orientation_hunter, position_target)
# print(0 < absolute_x < 256, 0 < absolute_y < 144, target_in_front)
# print(target_coordinate_in_view)
self.distance = np.linalg.norm(
np.array(position_hunter) - np.array(position_target))
# get distance within hunter and target
distance = np.array([self.distance / self.max_initial_distance])
# maintain a 8-length deque
if self.coordinate_queue is None and self.distance_queue is None:
self.coordinate_queue = deque([target_coordinate_in_view] * 8)
self.distance_queue = deque([distance] * self.queue_length)
else:
self.coordinate_queue.append(target_coordinate_in_view)
self.coordinate_queue.popleft()
self.distance_queue.append(distance)
self.distance_queue.popleft()
coordinate_state = np.concatenate(list(self.coordinate_queue))
distance_state = np.concatenate(list(self.distance_queue))
# define state
state = np.concatenate([
np.array([
orientation_hunter[0] / self.max_roll_angle,
orientation_hunter[1] / self.max_pitch_angle,
orientation_hunter[2] / self.max_yaw_angle
]).flatten(),
np.array((thrust_hunter - self.min_absolute_thrust) /
self.thrust_sensity + self.min_thrust).flatten(),
coordinate_state, distance_state
], 0)
# if self.tmp_pos.any() != None :
# tmp_pos = self.tmp_pos
# self.tmp_pos = position_hunter
return state