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(
)
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), _ = projection(np.matrix(position_target),
np.matrix(position_hunter),
np.matrix(orientation_hunter),
w=float(self.width),
h=float(self.height))
except ValueError:
self.distance = 100
return self.state
relative_x, relative_y = absolute_x / self.width, absolute_y / self.height
target_coordinate_in_view = np.array(
(relative_x, relative_y)).flatten()
self.distance = np.linalg.norm(position_hunter - 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
def _update_display(self):
if self.display:
# self.env.render()
pos_hunter, ori_hunter, acc_hunter, pos_target, ori_target, acc_target, thrust = dronesim.siminfo(
)
self.renderer.render(pos_hunter, ori_hunter, pos_target,
ori_target)