|
from dataclasses import dataclass |
|
import numpy as np |
|
from dronemodel import DroneModel |
|
|
|
import param_ as param_ |
|
from settings import Settings |
|
|
|
|
|
@dataclass |
|
class Drone: |
|
""" |
|
Creates a drone (it is either red or blue / foe or friend |
|
""" |
|
is_blue: bool = True |
|
position: np.ndarray((3,)) = np.zeros((3,)) |
|
position_noise: np.ndarray((3,)) = np.zeros((3,)) |
|
drone_model: DroneModel = None |
|
max_speeds: np.ndarray((3,)) = None |
|
min_speeds: np.ndarray((3,)) = None |
|
init_position: np.ndarray((3,)) = None |
|
init_speed: np.ndarray((3,)) = None |
|
color = np.ndarray((3,)) |
|
is_alive: bool = True |
|
is_fired: int = 0 |
|
fires = 0 |
|
step_ = 0 |
|
id_: int = -1 |
|
ttl: float = param_.DURATION |
|
speed: np.ndarray((3,)) = np.zeros((3,)) |
|
min_positions = np.zeros((3,)) |
|
max_positions = np.array([Settings.perimeter, 2*np.pi, Settings.perimeter_z]) |
|
|
|
def __post_init__(self): |
|
self.drone_model = DroneModel(self.is_blue) |
|
self.max_speeds = np.array([self.drone_model.max_speed, |
|
2*np.pi, |
|
self.drone_model.max_up_speed]) |
|
self.min_speeds = np.array([0, |
|
0, |
|
-self.drone_model.max_down_speed]) |
|
|
|
self.init_position = np.copy(self.position) |
|
self.init_position_noise = np.copy(self.position_noise) |
|
self.init_speed = np.copy(self.speed) |
|
self.color = param_.BLUE_COLOR if self.is_blue else param_.RED_COLOR |
|
self.ttl = (self.position[0] / self.max_speeds[0]) * param_.TTL_RATIO + param_.TTL_MIN |
|
|
|
def reset(self): |
|
self.is_alive = True |
|
self.speed = self.init_speed |
|
self.color = param_.BLUE_COLOR if self.is_blue else param_.RED_COLOR |
|
distance_factor = Settings.blue_distance_factor if self.is_blue else Settings.red_distance_factor |
|
self.position[0] = self.init_position[0]*distance_factor |
|
self.position[2] = self.init_position[2]*distance_factor |
|
self.position_noise *= distance_factor |
|
|
|
self.position[0] = np.clip(self.position[0] + np.random.rand() * self.position_noise[0], |
|
param_.GROUNDZONE * 1.1, |
|
param_.PERIMETER * 0.9) |
|
self.position[1] = self.position[1] + np.random.rand() * self.position_noise[1] |
|
self.position[2] = np.clip(self.position[2] + np.random.rand() * self.position_noise[2], |
|
param_.GROUNDZONE * 1.1, |
|
param_.PERIMETER_Z * 0.9) |
|
|
|
self.ttl = (self.position[0] / self.max_speeds[0]) * param_.TTL_RATIO + param_.TTL_MIN |
|
|
|
|
|
def step(self, action): |
|
self.step_ = self.step_ + 1 |
|
reward = 0 |
|
info = {'ttl': param_.DURATION} |
|
if self.is_alive: |
|
pos_xyz, speed_xyz = self.to_xyz(self.position), self.to_xyz(self.speed) |
|
pos_s, speed_s = \ |
|
self.drone_model.get_trajectory(pos_xyz, speed_xyz, action, np.linspace(0, param_.STEP, 10)) |
|
pos, speed = pos_s.T[-1], speed_s.T[-1] |
|
self.position, self.speed = self.from_xyz(pos), self.from_xyz(speed) |
|
self.ttl -= param_.STEP |
|
info['ttl'] = self.ttl |
|
|
|
|
|
|
|
if self.is_blue: |
|
''' |
|
for further usage |
|
straight_action, time_to_catch = self.simple_blue() |
|
tolerance = 0.05 if 4 < time_to_catch else 1 if 2 < time_to_catch else 3 |
|
distance = 1 if tolerance < np.linalg.norm(straight_action - action) else 0 |
|
''' |
|
distance = 1 |
|
else: |
|
straight_action = self.simple_red() |
|
distance = 1 if 0.1 < np.linalg.norm(straight_action - action) else 0 |
|
info['distance_to_straight_action'] = distance |
|
|
|
|
|
if self._hits_target(): |
|
info['hits_target'] = 1 |
|
reward = -param_.TARGET_HIT_COST |
|
self.color = param_.RED_SUCCESS_COLOR |
|
self.is_alive = False |
|
|
|
if self._out_of_bounds(): |
|
coef = -1 if self.is_blue else 1 |
|
reward = coef * param_.OOB_COST |
|
self.is_alive = False |
|
info['oob'] = 1 |
|
|
|
obs = self.get_observation() |
|
done = not self.is_alive |
|
|
|
return obs, reward, done, info |
|
|
|
def _out_of_bounds(self): |
|
return not (0 < self.position[2] < Settings.perimeter_z and self.position[0] < Settings.perimeter) |
|
|
|
def _hits_target(self): |
|
if self.is_blue: |
|
return False |
|
else: |
|
distance_to_zero = np.sqrt(self.position[0]**2 + self.position[2]**2) |
|
return distance_to_zero < Settings.groundzone |
|
|
|
def fires_(self, foe) -> bool: |
|
""" |
|
checks if the foe drone is hit by self |
|
:param foe: a foe drone |
|
:return: True= yes, got you |
|
""" |
|
|
|
if not (self.is_alive and foe.is_alive): |
|
return False |
|
|
|
|
|
pos_xyz = - self.to_xyz(self.position) + self.to_xyz(foe.position) |
|
distance = np.linalg.norm(pos_xyz) |
|
pos_xyz /= distance |
|
|
|
if distance < self.drone_model.distance_to_neutralisation: |
|
return self.is_in_the_cone(foe) |
|
|
|
return False |
|
|
|
|
|
def is_in_the_cone(self, foe) -> bool: |
|
''' |
|
verifies if foe is in the cone (without any regard to distance) |
|
:param foe: |
|
:return: |
|
''' |
|
pos_xyz = - self.to_xyz(self.position) + self.to_xyz(foe.position) |
|
pos_xyz /= np.linalg.norm(pos_xyz) |
|
vit_xyz = self.to_xyz(self.speed) |
|
vit_xyz /= np.linalg.norm(vit_xyz) |
|
cos_theta = np.dot(pos_xyz, vit_xyz) |
|
in_the_cone = False |
|
if 0 < cos_theta: |
|
theta = np.arccos(cos_theta) |
|
in_the_cone = theta < self.drone_model.angle_to_neutralisation |
|
return in_the_cone |
|
|
|
|
|
|
|
def is_killed(self, is_blue=True): |
|
self.is_alive = False |
|
self.position[2] = 0 |
|
self.color = param_.BLUE_DEAD_COLOR if is_blue else param_.RED_DEAD_COLOR |
|
|
|
def to_xyz(self, rho_theta_z: np.ndarray(shape=(3,))) -> np.ndarray(shape=(3,)): |
|
""" |
|
allows to get the 3D xyz coordinates from a polar representation |
|
:param rho_theta_z: array (3,) with rho in meter, theta in rad, zed in meter for positions, /s for speeds, etc. |
|
:return: float array (3,) with x, y, z in meter, /s for speeds, etc. |
|
""" |
|
xy_ = rho_theta_z[0] * np.exp(1j * rho_theta_z[1]) |
|
return np.array([np.real(xy_), np.imag(xy_), rho_theta_z[2]]) |
|
|
|
def from_xyz(self, xyz: np.ndarray(shape=(3,))) -> np.ndarray(shape=(3,)): |
|
""" |
|
""" |
|
z_complex = xyz[0] + 1j*xyz[1] |
|
rho = np.abs(z_complex) |
|
theta = np.angle(z_complex) |
|
return np.array([rho, theta, xyz[2]], dtype='float32') |
|
|
|
def to_norm(self, |
|
rho_theta_z: np.ndarray(shape=(3,)), |
|
max_vector: np.ndarray(shape=(3,)), |
|
min_vector: np.ndarray(shape=(3,)) = np.array([0, 0, 0]))\ |
|
-> np.ndarray(shape=(3,), dtype='float32'): |
|
""" |
|
normalises the position/speed in order to have all space in a [0;1]**3 space |
|
:return: rho, theta, zed in a [0;1]**3 space |
|
""" |
|
rho = rho_theta_z[0] / max_vector[0] |
|
theta = (rho_theta_z[1] / (2 * np.pi)) % 1 |
|
zed = (rho_theta_z[2] - min_vector[2]) / (max_vector[2] - min_vector[2]) |
|
return np.array([rho, theta, zed], dtype='float32') |
|
|
|
def from_norm(self, |
|
norm: np.ndarray(shape=(3,)), |
|
max_vector: np.ndarray(shape=(3,)), |
|
min_vector: np.ndarray(shape=(3,)) = np.array([0, 0, 0]))\ |
|
-> np.ndarray(shape=(3,), dtype='float32'): |
|
""" |
|
denormalises and renders into cylindric coordinates |
|
:param norm: |
|
:param max_vector: |
|
:param min_vector: |
|
:return: |
|
""" |
|
rho = norm[0] * max_vector[0] |
|
theta = norm[1] * 2*np.pi |
|
zed = norm[2] * (max_vector[2] - min_vector[2]) + min_vector[2] |
|
return np.array([rho, theta, zed], dtype='float32') |
|
|
|
def to_lat_lon_zed(self, lat, lon): |
|
z = self.position[0] * np.exp(1j * self.position[1]) |
|
lat = np.imag(z) * 360 / (40075 * 1000) + lat |
|
lon = np.real(z) * 360 / (40075 * 1000 * np.cos(np.pi / 180 * lat)) + lon |
|
return lat, lon, self.position[2] |
|
|
|
def distance(self, other_drone=None): |
|
|
|
if other_drone: |
|
distance = np.sqrt(np.abs(self.position[0] * np.exp(1j * self.position[1]) - |
|
other_drone.position[0] * np.exp(1j * other_drone.position[1])) ** 2 + |
|
(self.position[2] - other_drone.position[2]) ** 2) |
|
else: |
|
distance = np.sqrt((self.position[0] ** 2) + self.position[2] ** 2) |
|
return distance |
|
|
|
def get_observation(self): |
|
""" |
|
get normalised and transformed position and speed |
|
:return: |
|
""" |
|
|
|
normalised_position = self.to_norm(self.position, self.max_positions, self.min_positions) |
|
|
|
|
|
normalised_speed = self.to_norm(self.speed, self.max_speeds, self.min_speeds) |
|
|
|
return np.append(normalised_position, normalised_speed) |
|
|
|
|
|
def simple_red(self, target: np.ndarray(3,)=np.zeros(3), z_margin: float=50) -> np.ndarray(shape=(3,)): |
|
''' |
|
defines the actions for a trajectory targeting zero |
|
:return: |
|
''' |
|
|
|
self_z = self.position[0] * np.exp(1j * self.position[1]) |
|
target_z = target[0] * np.exp(1j * target[1]) |
|
|
|
direction = np.zeros(3) |
|
direction[0] = np.abs(self_z - target_z) |
|
direction[1] = np.angle(self_z - target_z) |
|
direction[2] = self.position[2] - target[2] - z_margin |
|
|
|
theta = (direction[1] + np.pi) / (2*np.pi) % 1 |
|
|
|
|
|
tan_phi = np.sign(direction[2]) * np.inf if direction[0] == 0 else direction[2]/direction[0] |
|
|
|
tan_phi_point = np.sign(self.speed[2]) * np.inf if self.speed[0] == 0 else self.speed[2]/self.speed[0] |
|
|
|
f_ratio = self.drone_model.Fxy / self.drone_model.Fz_minus |
|
|
|
psy = -np.arctan(tan_phi * f_ratio) / np.pi + 0.5 |
|
|
|
action = np.array([1, theta, psy]) |
|
|
|
return action |
|
|
|
def simple_target(self, target: (np.ndarray(shape=(3,)))) -> np.ndarray(shape=(3,)): |
|
''' |
|
defines the actions for a trajectory targeting ... the given target |
|
:return: |
|
''' |
|
|
|
self_z = self.position[0] * np.exp(1j * self.position[1]) |
|
target_z = target[0] * np.exp(1j * target[1]) |
|
|
|
direction = np.zeros(3) |
|
direction[0] = np.abs(self_z - target_z) |
|
direction[1] = np.angle(self_z - target_z) |
|
direction[2] = self.position[2] - target[2] |
|
|
|
theta = (direction[1] + np.pi) / (2*np.pi) % 1 |
|
|
|
|
|
tan_phi = np.sign(direction[2]) * np.inf if direction[0] == 0 else direction[2]/direction[0] |
|
|
|
tan_phi_point = np.sign(self.speed[2]) * np.inf if self.speed[0] == 0 else self.speed[2]/self.speed[0] |
|
|
|
f_ratio = self.drone_model.Fxy / self.drone_model.Fz_minus |
|
|
|
psy = 0.5 if tan_phi_point < -tan_phi else -np.arctan(tan_phi * f_ratio) / np.pi + 0.5 |
|
|
|
if Settings.perimeter_z / 2 < direction[2]: |
|
psy = min(0.2, psy) |
|
|
|
if self.position[0] < 1.5 * Settings.groundzone: |
|
psy = min(0.2, psy) |
|
|
|
|
|
action = np.array([1, theta, psy]) |
|
|
|
|
|
|
|
return action |
|
|
|
def next_simple_pos(self): |
|
next_pos = np.zeros(3) |
|
simple_next = self.pos[0] * np.exp(1j * self.pos[1]) + self.speed[0] * np.exp(1j * self.speed[1]) * param_.step |
|
next_pos[0] = np.abs(simple_next) |
|
next_pos[1] = np.arg(simple_next) |
|
next_pos[2] = self.pos[2] + self.speed[2] * param_.step |
|
return next_pos |
|
|
|
def copy_pos_speed(self, drone_to_copy): |
|
self.position = drone_to_copy.position |
|
self.speed = drone_to_copy.speed |
|
|
|
|
|
|