def step(self, action, objective):
self.prev_position = self.position.copy()
# Rate of Turn
# https://dspace.lib.cranfield.ac.uk/bitstream/handle/1826/2912/CHAPTER_8_july27_2.pdf?sequence=5&isAllowed=y
ROT = np.float64(action) * 9.81 * np.sqrt(self.maxLatAx * self.maxLatAx - 1) / Util.rss(self.velocity)
ROD = 0
pol_vel = Util.cart2sph(self.velocity)
pol_vel[0] = pol_vel[0] + ROT
self.velocity = Util.sph2cart(pol_vel)
# Apply accel to vel
self.position += self.velocity * self.Ts
self.objective_in_fov = Util.getAngle(self.velocity + self.position, self.position, objective.position) < self.fov / 2
def _get_reward(self):
""" Reward is given for XY. """
logsize = 10000
scale = Util.rss(self.limits[1, :])
dpos = Util.rss(self.objective.position - self.platform.position)
idx = np.int32((1-np.int32(dpos)/scale) * logsize)
logrew_pos = np.logspace(-2, 2, logsize)
rew = logrew_pos[idx]
if self.platform.objective_in_fov:
rew += 0.1
if dpos < self.objective.radius:
return 1000 # *(1-self.t/self.max_time)
elif self._is_over() and Util.rss(self.platform.position - self.objective.position) > self.objective.radius:
return -1000
else:
a = self.platform.position + self.platform.velocity
b = self.platform.position
c = self.objective.position
return rew # util.getAngle(a, b, c)/10
def PN(self, t_pos, t_vel):
# TODO - FIX PN
# To find omega:
# omega = (R*Vr)/dot(R,R)
# Vr = Vt - Vm
# Rr = Rt - Rm
Vm = self.velocity
Rm = self.position
Rr = t_pos - Rm
Vr = t_vel - Vm
eq1 = np.cross(Rr, Vr)
eq2 = np.dot(Rr, Rr)
omega = np.divide(eq1, eq2)
# To find accel:
# a = -N*|Vr|*(Vm/|Vm|)*omega
# eq1 = (Vm/|Vm|)
# eq2 = N*|Vr|
# eq3 = cross(eq1*eq2,omega)
n = self.N
eq1 = np.divide(Vm, Util.rss(Vm))
a = n * np.cross(eq1, omega)
a_sph_rad = Util.cart2sph(a)
v_sph_rad = a_sph_rad * self.Ts
tmp_velocity_sph = Util.cart2sph(self.velocity.copy())
# Max Rate of Turn
# https://dspace.lib.cranfield.ac.uk/bitstream/handle/1826/2912/CHAPTER_8_july27_2.pdf?sequence=5&isAllowed=y
ROT_max_rad = 9.81 * np.sqrt(self.max_lat_ax**2 - 1) / Util.rss(
self.velocity) * self.Ts
ROT_max_deg = np.rad2deg(ROT_max_rad)
# if a_sph_deg < ROT_max_deg:
# min(v_sph_rad[0], ROT_max_rad[0])
tmp_velocity_sph[0] += v_sph_rad[0]
tmp_velocity = Util.sph2cart(tmp_velocity_sph)
# else:
# tmp_velocity += ROT_max_deg * self.Ts
self.velocity = tmp_velocity.copy()
self.position += np.sign(a) * self.velocity * self.Ts
def get_state(self):
plat_pos = self.platform.position
plat_vel = self.platform.velocity
cat = np.concatenate([plat_pos, plat_vel], axis = None)
for i in range(self.n_targets):
targ_pos = self.targets[i].get_position()
targ_vel = self.targets[i].get_velocity()
cat = np.concatenate([cat, targ_pos, targ_vel], axis=None)
cat = np.concatenate([cat, self.objective.position, self.action_prev], axis=None)
scale = cat / Util.rss(self.limits[1, :] - self.limits[0, :])
cat2 = np.concatenate([scale, self.action_prev], axis=None)
# reshape = cat.reshape([10, 1, 1, 1])
return cat2
def step(self, t_pos, t_vel):
if self.isactive:
temp_pos = self.position.copy()
# use PN equation to get new vel/pos
# https://en.wikipedia.org/wiki/Proportional_navigation
if self.nav_type == "CLOS":
self.CLOS(t_pos)
elif self.nav_type == "PN":
self.PN(t_pos, t_vel)
self.prev_position = temp_pos
else:
if Util.rss(self.position - t_pos) < self.start_range:
self.isactive = True
def CLOS(self, t_pos):
Pm = self.position
Pt = t_pos
Vm = self.velocity
theta_t = Util.cart2sph(Pt - Pm)
theta_m = Util.cart2sph(Vm)
angle = Util.getAngle(Vm + Pm, Pm, Pt)
# Max Rate of Turn
# https://dspace.lib.cranfield.ac.uk/bitstream/handle/1826/2912/CHAPTER_8_july27_2.pdf?sequence=5&isAllowed=y
ROT_max_rad = 9.81 * np.sqrt(self.max_lat_ax**2 - 1) / Util.rss(
self.velocity) * self.Ts
theta_m[0] += np.sign(theta_t[0] - theta_m[0]) * Util.absMin(
ROT_max_rad[0], angle)
self.velocity = Util.sph2cart(theta_m.copy())
self.position += self.velocity * self.Ts
def step(self, platform_pos):
if Util.rss(self.position - platform_pos) < self.radius:
self.iswon = True