def sample_goal(landmark_pos_dict, landmark, side, start_x, start_z):
landmark_x, landmark_z = landmark_pos_dict[landmark]
radius = get_landmark_radius(landmark)
# calculate F and R vectors
start_to_landmark = np.array([landmark_x - start_x, landmark_z - start_z])
landmark_dist = (start_to_landmark**2).sum()**0.5
f_vector = start_to_landmark / landmark_dist * (radius + 2.0)
r_vector = np.array([f_vector[1], -f_vector[0]])
# calculate center of goal sample
landmark_vector = np.array([landmark_x, landmark_z])
if side == Sides.FRONT:
center = landmark_vector - f_vector
elif side == Sides.RIGHT:
center = landmark_vector + r_vector
elif side == Sides.LEFT:
center = landmark_vector - r_vector
elif side == Sides.BACK:
center = landmark_vector + f_vector
else:
assert False
noise = np.random.multivariate_normal([0.0, 0.0], [[1.5, 0.0], [0.0, 1.5]])
goal = center + noise
end_x, end_z = goal
end_rot = 90.0 - np.arctan2(end_z - start_z,
end_x - start_x) * 180.0 / math.pi
return float(end_x), float(end_z), float(end_rot)
def successor(state, action, landmark_pos_dict):
x, z, pose = state
if action == "Forward":
# calculate new x and new z after moving forward
pose_x = np.cos((90.0 - pose) * math.pi / 180.0)
pose_z = np.sin((90.0 - pose) * math.pi / 180.0)
new_x = x + 1.5 * pose_x
new_z = z + 1.5 * pose_z
# find out if next state is valid
if new_x < 225.0 or new_x > 275.0:
return None
elif new_z < 225.0 or new_z > 275.0:
return None
for landmark, (l_x, l_z) in landmark_pos_dict.items():
if landmark in CORNER_LANDMARKS:
continue
l_dist = ((new_x - l_x)**2 + (new_z - l_z)**2)**0.5
l_radius = get_landmark_radius(landmark)
if l_dist < l_radius:
return None
return new_x, new_z, pose
elif action == "TurnRight":
# calculate new pose after turning right
new_pose = (pose + 15.0) % 360.0
return x, z, new_pose
elif action == "TurnLeft":
# calculate new pose after turning left
new_pose = (pose - 15.0) % 360.0
return x, z, new_pose
def heuristic(state, goal_x, goal_z, landmark_pos_dict):
x, z, pose = state
# return heuristic_simple(x, z, pose, goal_x, goal_z, 1.5)
start_pos = np.array([x, z])
goal_pos = np.array([goal_x, goal_z])
landmark, gamma = get_closest_obstacle(start_pos, goal_pos,
landmark_pos_dict)
if landmark is None:
# calculate simple heuristic since there are no obstacles in the way
return heuristic_simple(x, z, pose, goal_x, goal_z, 1.5)
else:
# calculate 2-part heuristic, passing obstacle on left vs right
landmark_pos = np.array(landmark_pos_dict[landmark])
landmark_angle = calc_target_angle(start_pos, landmark_pos)
landmark_dist = ((landmark_pos - start_pos)**2).sum()**0.5
landmark_radius = get_landmark_radius(landmark)
angular_width = np.arcsin(
landmark_radius / landmark_dist) * 180.0 / math.pi
side_distance = (landmark_dist**2 - landmark_radius**2)**0.5
landmark_goal_dist = ((goal_pos - landmark_pos)**2).sum()**0.5
# calculate left heuristic
left_angle = landmark_angle - angular_width
l_vector = get_angle_unit_vector(left_angle)
landmark_left = start_pos + l_vector * side_distance
h1_left = heuristic_simple(x, z, pose, landmark_left[0],
landmark_left[1], 0.0)
left_pose = calc_target_angle(start_pos, landmark_left)
h2_left = heuristic_simple(landmark_left[0], landmark_left[1],
left_pose, goal_x, goal_z, 1.5)
left_goal_dist = ((goal_pos - landmark_left)**2).sum()**0.5
left_arc_cost = calc_extra_arc_distance(landmark_radius,
landmark_goal_dist,
left_goal_dist)
h_left = h1_left + h2_left + left_arc_cost
# calculate right heuristic
right_angle = landmark_angle + angular_width
r_vector = get_angle_unit_vector(right_angle)
landmark_right = start_pos + r_vector * side_distance
h1_right = heuristic_simple(x, z, pose, landmark_right[0],
landmark_right[1], 0.0)
right_pose = calc_target_angle(start_pos, landmark_right)
h2_right = heuristic_simple(landmark_right[0], landmark_right[1],
right_pose, goal_x, goal_z, 1.5)
right_goal_dist = ((goal_pos - landmark_right)**2).sum()**0.5
right_arc_cost = calc_extra_arc_distance(landmark_radius,
landmark_goal_dist,
right_goal_dist)
h_right = h1_right + h2_right + right_arc_cost
return min(h_left, h_right)
def sample_goal(landmark_pos_dict, landmarks, direction, start_x, start_z,
start_rot):
if direction != Direction.BETWEEN:
# goal next to a landmark
landmark_x, landmark_z = landmark_pos_dict[landmarks[0]]
radius = get_landmark_radius(landmarks[0])
# calculate F and R vectors
start_to_landmark = np.array([landmark_x - start_x, landmark_z - start_z])
landmark_dist = (start_to_landmark ** 2).sum() ** 0.5
f_vector = start_to_landmark / landmark_dist * (radius + 2.0)
r_vector = np.array([f_vector[1], -f_vector[0]])
# calculate center of goal sample
landmark_vector = np.array([landmark_x, landmark_z])
if direction == Direction.FRONT:
center = landmark_vector - f_vector
elif direction == Direction.RIGHT:
center = landmark_vector + r_vector
elif direction == Direction.LEFT:
center = landmark_vector - r_vector
elif direction == Direction.BACK:
center = landmark_vector + f_vector
else:
assert False
else:
# goal between two landmarks
landmark_x_1, landmark_z_1 = landmark_pos_dict[landmarks[0]]
landmark_x_2, landmark_z_2 = landmark_pos_dict[landmarks[1]]
center = np.array([(landmark_x_1 + landmark_x_2) / 2.0,
(landmark_z_1 + landmark_z_2) / 2.0])
noise = np.random.multivariate_normal([0.0, 0.0], [[1.5, 0.0], [0.0, 1.5]])
goal = center + noise
end_x, end_z = goal
end_rot = 90.0 - np.arctan2(end_z - start_z, end_x - start_x) * 180.0 / math.pi
# make sure goal is in range
angle_diff = end_rot - start_rot
while angle_diff > 180.0:
angle_diff -= 360.0
while angle_diff < -180.0:
angle_diff += 360.0
if abs(angle_diff) >= 30.0:
raise RestartException()
return float(end_x), float(end_z), float(end_rot)
def get_closest_obstacle(start_pos, goal_pos, landmark_pos_dict):
sg_vec = goal_pos - start_pos
sg_sq_norm = np.dot(sg_vec, sg_vec)
min_gamma = 1.0
best_landmark = None
for landmark, (landmark_x, landmark_z) in landmark_pos_dict.items():
if landmark in CORNER_LANDMARKS:
continue
landmark_pos = np.array([landmark_x, landmark_z])
gamma = np.dot(landmark_pos - start_pos, sg_vec) / sg_sq_norm
if 0.0 < gamma < 1.0 and gamma < min_gamma:
# check if landmark is blocking straight path
closest_pos = start_pos + gamma * sg_vec
dist = ((landmark_pos - closest_pos)**2).sum()**0.5
l_radius = get_landmark_radius(landmark)
if dist < l_radius:
min_gamma = gamma
best_landmark = landmark
return best_landmark, min_gamma
def sample_start_pos(landmark_pos_dict, min_dist=10.0):
x, z, angle = None, None, None
while True:
x, z = (225.0 + 0.05 * random.randint(*START_RANGE),
225.0 + 0.05 * random.randint(*START_RANGE))
pos = np.array([x, z])
fail = False
for landmark, (landmark_x, landmark_z) in landmark_pos_dict.items():
if landmark in CORNER_LANDMARKS:
continue
landmark_pos = np.array([landmark_x, landmark_z])
radius = get_landmark_radius(landmark)
dist = ((landmark_pos - pos) ** 2).sum() ** 0.5
if dist < max(radius, min_dist):
fail = True
break
if fail:
continue
angle = random.random() * 360.0
valid_landmarks = sample_valid_landmarks(landmark_pos_dict, x, z, angle)
if valid_landmarks is not None:
break
return x, z, angle