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)