def _reset(self):
self.current_target = 0
self.current_step = 0
self.target = (numpy.random.uniform(-0.25 * self.playing_area_width,
0.25 * self.playing_area_width),
numpy.random.uniform(-0.25 * self.playing_area_height,
0.25 * self.playing_area_height))
self.car_location = (numpy.random.uniform(
-0.25 * self.playing_area_width, 0.25 * self.playing_area_width),
numpy.random.uniform(
-0.25 * self.playing_area_height,
0.25 * self.playing_area_height))
self.car_heading = numpy.random.uniform(-math.pi, math.pi)
self.initial_location = self.car_location
desired_vector = (self.target[0] - self.car_location[0],
self.target[1] - self.car_location[1])
desired_vector = normalized(desired_vector)
current_vector = polar_to_cartesian((1, self.car_heading))
angle_difference_end = angle_signed(desired_vector, current_vector)
# create observation, return
vel_cart = polar_to_cartesian((1, self.car_heading))
observation = numpy.array([
self.target[0] / (0.5 * self.playing_area_width),
self.target[1] / (0.5 * self.playing_area_height),
self.car_location[0] / (0.5 * self.playing_area_width),
self.car_location[1] / (0.5 * self.playing_area_height),
vel_cart[0], vel_cart[1], angle_difference_end / math.pi
])
observation = observation.astype(numpy.float32)
return time_step.restart(observation)
def _find_decision_boundary_on_hypersphere(self,
centroid,
R,
penalize_known=False):
def objective(phi, grad=0):
# search on hypersphere surface in polar coordinates - map back to cartesian
cx = centroid + polar_to_cartesian(phi, R)
try:
cx2d = self.dimensionality_reduction.transform([cx])[0]
error = self.decision_boundary_distance(cx)
if penalize_known:
# slight penalty for being too close to already known decision boundary
# keypoints
db_distances = [
euclidean(cx2d, self.decision_boundary_points_2d[k])
for k in range(len(self.decision_boundary_points_2d))
]
error += (1e-8 *
((self.mean_2d_dist - np.min(db_distances)) /
self.mean_2d_dist)**2)
return error
except (Exception, ex):
print("Error in objective function:", ex)
return np.infty
optimizer = self._get_optimizer(
D=self.X.shape[1] - 1,
upper_bound=2 * np.pi,
iteration_budget=self.hypersphere_iteration_budget,
)
optimizer.set_min_objective(objective)
db_phi = optimizer.optimize(
[random.random() * 2 * np.pi for k in range(self.X.shape[1] - 1)])
db_point = centroid + polar_to_cartesian(db_phi, R)
return db_point
def _find_decision_boundary_on_hypersphere(self, centroid, R, penalize_known=False):
def objective(phi, grad=0):
# search on hypersphere surface in polar coordinates - map back to cartesian
cx = centroid + polar_to_cartesian(phi, R)
try:
cx2d = self.dimensionality_reduction.transform([cx])[0]
error = self.decision_boundary_distance(cx)
if penalize_known:
# slight penalty for being too close to already known decision boundary
# keypoints
db_distances = [euclidean(cx2d, self.decision_boundary_points_2d[k])
for k in range(len(self.decision_boundary_points_2d))]
error += 1e-8 * ((self.mean_2d_dist - np.min(db_distances)) /
self.mean_2d_dist)**2
return error
except (Exception, ex):
print("Error in objective function:", ex)
return np.infty
optimizer = self._get_optimizer(
D=self.X.shape[1] - 1, upper_bound=2 * np.pi, iteration_budget=self.hypersphere_iteration_budget)
optimizer.set_min_objective(objective)
db_phi = optimizer.optimize([rnd.random() * 2 * np.pi for k in range(self.X.shape[1] - 1)])
db_point = centroid + polar_to_cartesian(db_phi, R)
return db_point
def extract_filt_lr(sector_wid, ranges, angles_rad, range_min, range_max):
"""Extract and filter the LaserScan data
"""
# logg = logging.getLogger(f"c.{__name__}.extract_filt_lr")
# logg.debug(f"Start extract_filt_lr")
# get left values
left_center = 300
left_slice = slice(left_center - sector_wid, left_center + sector_wid)
left_ranges = ranges[left_slice]
left_angles_rad = angles_rad[left_slice]
# filter out overflow values
left_condition = np.where(
(range_min < left_ranges) & (left_ranges < range_max), True, False)
left_ranges_filt = np.extract(left_condition, left_ranges)
left_angles_rad_filt = np.extract(left_condition, left_angles_rad)
left_filt_x, left_filt_y = polar_to_cartesian(left_ranges_filt,
left_angles_rad_filt)
# , odom_robot_yaw, *odom_robot_pose
# we DO NOT rotate and translate the data, we work in the base_footprint frame to
# have better coefficient of the line, then we will translate the model found
# get right values
right_center = 100
right_slice = slice(right_center - sector_wid, right_center + sector_wid)
right_ranges = ranges[right_slice]
right_angles_rad = angles_rad[right_slice]
# filter out overflow values
right_condition = np.where(
(range_min < right_ranges) & (right_ranges < range_max), True, False)
right_ranges_filt = np.extract(right_condition, right_ranges)
right_angles_rad_filt = np.extract(right_condition, right_angles_rad)
right_filt_x, right_filt_y = polar_to_cartesian(right_ranges_filt,
right_angles_rad_filt)
# style = {"ls": "-", "marker": "."}
# ax = plot_add_create(left_filt_x, left_filt_y, **style)
# ax.set_title("Filtered left/right data")
# plot_add_create(0, 0, ax=ax, **style)
# plot_add_create(right_filt_x, right_filt_y, ax=ax, **style)
return left_filt_x, left_filt_y, right_filt_x, right_filt_y
def objective(phi, grad=0):
# search on hypersphere surface in polar coordinates - map back to cartesian
cx = centroid + polar_to_cartesian(phi, R)
try:
cx2d = self.dimensionality_reduction.transform([cx])[0]
error = self.decision_boundary_distance(cx)
if penalize_known:
# slight penalty for being too close to already known decision boundary
# keypoints
db_distances = [euclidean(cx2d, self.decision_boundary_points_2d[k])
for k in range(len(self.decision_boundary_points_2d))]
error += 1e-8 * ((self.mean_2d_dist - np.min(db_distances)) /
self.mean_2d_dist)**2
return error
except (Exception, ex):
print("Error in objective function:", ex)
return np.infty
def _step(self, action):
# calculate difference between current heading and desired heading
desired_vector = (self.target[0] - self.car_location[0],
self.target[1] - self.car_location[1])
desired_vector = normalized(desired_vector)
current_vector = polar_to_cartesian((1, self.car_heading))
angle_difference_start = angle_signed(desired_vector, current_vector)
# calculate angle change
angle = 0
# if action == 0 continue straight...
if action == 1: # turn left
angle = -self.car_steering_angle
if action == 2: # turn right
angle = self.car_steering_angle
# calculate location of front and rear axles.
front_axle = (self.car_location[0] +
(self.car_wheel_base / 2) * math.cos(self.car_heading),
self.car_location[1] +
(self.car_wheel_base / 2) * math.sin(self.car_heading))
rear_axle = (self.car_location[0] -
(self.car_wheel_base / 2) * math.cos(self.car_heading),
self.car_location[1] -
(self.car_wheel_base / 2) * math.sin(self.car_heading))
# calculate new location of front and rear axles
loc_rear = (
rear_axle[0] +
self.car_speed * self.time_step * math.cos(self.car_heading),
rear_axle[1] +
self.car_speed * self.time_step * math.sin(self.car_heading))
loc_front = (front_axle[0] + self.car_speed * self.time_step *
math.cos(self.car_heading + angle),
front_axle[1] + self.car_speed * self.time_step *
math.sin(self.car_heading + angle))
# calculate new location and heading, update environment state values
self.car_location = ((loc_front[0] + loc_rear[0]) / 2,
(loc_front[1] + loc_rear[1]) / 2)
self.car_heading = math.atan2(loc_front[1] - loc_rear[1],
loc_front[0] - loc_rear[0])
# calculate difference between current heading and desired heading
desired_vector = (self.target[0] - self.car_location[0],
self.target[1] - self.car_location[1])
desired_vector = normalized(desired_vector)
current_vector = polar_to_cartesian((1, self.car_heading))
angle_difference_end = angle_signed(desired_vector, current_vector)
done = False
angle_change = abs(angle_difference_start) - abs(angle_difference_end)
reward = angle_change / math.radians(self.car_steering_angle)
# check if car is within bounds
if abs(self.car_location[0]) > 0.5 * self.playing_area_width or abs(
self.car_location[1]) > 0.5 * self.playing_area_height:
done = True
reward = -100
# check if car has reached the current target
distance = euclid_dist_2D(self.target, self.car_location)
if distance < self.visited_distance:
reward = 25 + 75 * (self.current_step / self.max_steps)
# add new target and reset step...
self.current_target += 1
if self.current_target < self.max_target_count:
self.target = (numpy.random.uniform(
-0.25 * self.playing_area_width,
0.25 * self.playing_area_width),
numpy.random.uniform(
-0.25 * self.playing_area_height,
0.25 * self.playing_area_height))
self.current_step = 0
else:
done = True
# check if max steps were reached
if self.current_step >= self.max_steps:
done = True
distance_initial = euclid_dist_2D(self.target,
self.initial_location)
if distance_initial:
reward = 25 * (1 - distance / distance_initial)
else:
reward = 2.5
# create observation, return...
vel_cart = polar_to_cartesian((1, self.car_heading))
observation = numpy.array([
self.target[0] / (0.5 * self.playing_area_width),
self.target[1] / (0.5 * self.playing_area_height),
self.car_location[0] / (0.5 * self.playing_area_width),
self.car_location[1] / (0.5 * self.playing_area_height),
vel_cart[0], vel_cart[1], angle_difference_end / math.radians(180)
])
observation = observation.astype(numpy.float32)
# increment step, return
self.current_step += 1
# print(reward)
if done:
return time_step.termination(observation, reward)
else:
return time_step.transition(observation, reward, discount=0.98)
