def get_obstacle_forces(K_o, MIN, ENV):
for s in MIN.sensors:
s.sense(ENV)
vecs_to_obs = [(R_z(MIN.heading) @ R_z(s.host_relative_angle))[:2, :2]
@ p2v(s.measurement.get_val(), s.measurement.get_angle())
for s in MIN.sensors if s.measurement.is_valid()]
return get_generic_force_vector(vecs_to_obs, K_o, d_o=MIN.range)
def get_exploration_velocity(self, MIN, beacons, ENV):
if self.__exploration_dir is None:
self.__exploration_dir = HeuristicExplore.__get_exploration_dir(
MIN, self.k)
self.__exploration_vec = p2v(1, self.__exploration_dir)
obs_vec = HeuristicExplore.__get_obstacle_avoidance_vec(MIN, ENV)
if np.abs(self.__exploration_dir -
gva(self.__exploration_vec +
obs_vec)) > np.pi / 2 or MIN.get_RSSI(
self.target) < self.MIN_RSSI_STRENGTH_BEFORE_LAND:
raise AtLandingConditionException
return normalize(self.__exploration_vec + obs_vec)
def get_repulsive_force(K_n, K_o, MIN, ENV):
vecs_to_neighs = [
MIN.get_vec_to_other(n).reshape(2, 1) for n in MIN.neighbors
if not (MIN.get_vec_to_other(n) == 0).all()
]
for s in MIN.sensors:
s.sense(ENV)
vecs_to_obs = [(R_z(MIN.heading) @ R_z(s.host_relative_angle))[:2, :2]
@ p2v(s.measurement.get_val(),
s.measurement.get_angle()).reshape(2, )
for s in MIN.sensors if s.measurement.is_valid()]
# vecs_to_obs = [
# p2v(s.measurement.get_val(), s.measurement.get_angle()).reshape(2,)
# for s in MIN.sensors if s.measurement.is_valid()
# ]
return get_generic_force_vector(vecs_to_obs, K_o, d_o=MIN.range)
def generate_target_pos(self, beacons, ENV, next_min):
angle = np.pi / 15
next_min.target_pos = p2v(self.range, angle)
next_min.target_angle = angle
next_min.first_target_pos = deepcopy(next_min.target_pos)
def __sense_aux(self, corners):
"""Computes the distance to an obstacle
Args:
corners ndarray: n-by-2 array representing the n corners of an obstacle as (x, y)-coords. It is assumed that
there is a line between the first and last corner defined in the array.
Returns:
float: distance along sensor-frame x-axis to nearest obstacle (inf if no obstacle is within range)
"""
valid_crossings_dict = {
"lengths": np.array([]),
"angles": np.array([])
}
valid_crossings = np.array([np.inf])
closed_corners = np.vstack(
(corners, corners[0, :]
)) #Contains all the line segments that defines an obstacle
num_of_rays = 11 # 11 rays total, 5 pairs + "0-angle"
fov_angle = np.deg2rad(27) #Total field-of-view
start_ang = -fov_angle / 2.0
if num_of_rays == 1:
delta_ang = 0
else:
delta_ang = fov_angle / (num_of_rays - 1)
for i in range(num_of_rays):
current_ray_angle = start_ang + i * delta_ang
A_1 = p2v(
1, self.host_relative_angle + self.host.heading +
current_ray_angle).reshape(2, 1)
max_t = np.array([self.max_range, 1])
for j in np.arange(corners.shape[0]):
x1, x2 = closed_corners[j, :], closed_corners[j + 1, :]
A = np.hstack((A_1, (x1 - x2).reshape(2, 1)))
b = x1 - self.host.pos
try:
t = np.linalg.solve(A, b)
if (t >= 0).all() and (t <= max_t).all():
valid_crossings = np.hstack(
(valid_crossings, np.linalg.norm(t)))
valid_crossings_dict["lengths"] = np.hstack(
(valid_crossings_dict["lengths"],
np.linalg.norm(t)))
valid_crossings_dict["angles"] = np.hstack(
(valid_crossings_dict["angles"],
current_ray_angle))
except np.linalg.LinAlgError:
pass
if len(valid_crossings_dict['lengths']) > 0:
length = min(valid_crossings_dict['lengths'])
index = np.argmin((valid_crossings_dict['lengths']))
angle = valid_crossings_dict['angles'][index]
else:
length = np.inf
angle = None
return length, angle