def select_next_action(self): self._min_cur_radar_scans = sys.maxsize humans = self._create_human_data() obstacles = self._create_obstacles_data() self.debug_info['min_proximities'].append(self._min_cur_radar_scans) location = self._gps.location() ## we might need to limit te magnitude of this vector v_0 = self._get_target_direction_input_data() v_0 = v_0 * 0.306 / math.sqrt(np.sum(np.power(v_0, 2))) if self.count == 0: v = 0.05 * Vector.unit_vec_from_radians( Vector.radians_between(location, self._target.position)) theta = math.atan2(v[1], v[0]) self._old_position = self._start_pos else: v = location - self._old_position theta = math.atan2(v[1], v[0]) self._old_position = location force = self.force(v_0, v, np.array([location[0], location[1], theta]), humans, obstacles) #force = force*0.306/math.sqrt(np.sum(np.power(force,2))) force_norm = math.sqrt(np.sum(np.power(force, 2))) force = force * 0.306 / force_norm if force_norm > 0.306 else force v_next = v + force * 1 speed = math.sqrt(np.sum(np.power(v_next, 2))) direction = math.atan2(v_next[1], v_next[0]) * 180 / math.pi self.count += 1 return RobotControlInput(speed, direction)
def select_next_action(self): self._step_num += 1 msgs = self._sensors['recv'].get_recent_bcasts() for msg in msgs: if msg['msg']['action'] == 'cleared_node' and msg['msg'][ 'node'] in self._node_props: self._update_node_props_from_visit(msg['msg']['node'], msg['msg']['visit_time'], msg['msg']['avg_reward']) for node in self._node_props: self._node_props[node]['expected_reward'] += self._node_props[ node]['RAR'] if self._gps.distance_to(self._cur_path[-1].location) <= 1: if self._cur_path[-1] in self._node_props: self._visit_cur_node(self._cur_path[-1]) final_node = self._cur_path.pop() while len(self._cur_path) == 0: self._choose_next_path(nearest_node=final_node) direction = self._gps.angle_to(self._cur_path[-1].location) speed = min(self._gps.distance_to(self._cur_path[-1].location), 15) return RobotControlInput(speed, direction)
def select_next_action(self): radar_data = self._radar.scan(self._gps.location()) self.debug_info['min_proximities'].append(np.min(radar_data)) self._movement.step(1) next_pos = self._movement.get_pos() direction = self._gps.angle_to(next_pos) speed = self._gps.distance_to(next_pos) return RobotControlInput(speed, direction)
def select_next_action(self): radar_data = self._radar.scan(self._gps.location()); radar_data_size = len(radar_data) target_angle = self._gps.angle_to(self._target.position) intervals = self._split_radar_to_intervals(radar_data); self.debug_info['intervals'] = intervals; direction = self._gps.angle_to(self._target.position); if(not np.all(radar_data >= self._range_limit/1.05)): tmp_heading = self._heading + (self._gps.angle_to(self._target.position) - self._heading)*0.00 chosen_interval = self._get_closest_interval(intervals, self._heading); direction = ((chosen_interval[0] + chosen_interval[1]) / 2 - self._heading) * 1 + self._heading self._heading = direction; speed = self._range_limit; # Go as fast as possible without leaving the observable zone return RobotControlInput(speed, direction);
def _select_next_action_local(self): next_waypoint = self._node_list[np.random.randint(len( self._node_list))] if self._cur_path_index < len(self._cur_path): next_waypoint = self._cur_path[self._cur_path_index].to_node else: print("Next waypoint out of bounds in select_next_action_local", file=sys.stderr) angle = Vector.getAngleBetweenPoints(self._gps.location(), next_waypoint.pos) speed = Vector.getDistanceBetweenPoints(self._gps.location(), next_waypoint.pos) if self._normal_speed < speed: speed = self._normal_speed return RobotControlInput(speed, angle)
def select_next_action(self): self._mapper.add_observation() self.debug_info['mapdata'] = self._mapper.get_grid_data() self._invalidateNodes() robot_location = self._gps.location() if not (self._radar._env.get_obsflags(robot_location) & ObsFlag.DYNAMIC_OBSTACLE or self._mapper.get_grid_data()[int(robot_location[0])][int( robot_location[1])] & ObsFlag.STATIC_OBSTACLE): if any([ n.flag == 1 for n in self._solution ]) or (len(self._solution) > 0 and self._collides(robot_location, self._solution[0].data)): self._regrow_rrt() self._extract_solution() while 0 < len(self._solution) and Vector.distance_between( robot_location, self._solution[0].data) < self._waypoint_threshold: del self._solution[0] if len(self._solution) > 0: direction = Vector.degrees_between(robot_location, self._solution[0].data) dist = min( self._maxstepsize, Vector.distance_between(robot_location, self._solution[0].data)) self.debug_info["path"] = self._solution else: #No valid path found self._has_given_up = True dist = 0 direction = np.random.uniform(low=0, high=360) elif self._mapper.get_grid_data()[int(robot_location[0])][int( robot_location[1])] & ObsFlag.STATIC_OBSTACLE: self._has_given_up = True dist = 0 direction = np.random.uniform(low=0, high=360) else: #If robot is inside dynamic obstacle dist = 0 direction = np.random.uniform(low=0, high=360) self.debug_info['rrt_tree'] = self._rrt return RobotControlInput(dist, direction)
def select_next_action(self): predicted_values = None if self._net_type == 'perl': input_dict = self._create_perl_input_data() predicted_values = self._model.eval((input_dict, [ True if Vector.distance_between( self._gps.location(), self._start_pos) < 0.01 else False ]))[0][0] else: input_dict = self._create_grp_input_data() predicted_values = self._model.eval(input_dict)[0] direction = np.argmax(predicted_values[0:360]) * 360 / 359 speed = predicted_values[360] * 0.31 #print(self._gps.location(), direction, speed, True if Vector.distance_between(self._gps.location(), self._start_pos) < 0.01 else False) return RobotControlInput(speed, direction)
def select_next_action(self): return RobotControlInput(0, 0)
def select_next_action(self): # Variable naming note: some variables have _pdf on their # name, which is meant to indicate that they represent some # kind of probability distribution over the possible # movement directions self.stepNum += 1 targetpoint_pdf = self._create_targetpoint_pdf() # The combined PDF will store the combination of all the # PDFs, but for now that's just the targetpoint PDF combined_pdf = targetpoint_pdf # Scan the radar to get obstacle information raw_radar_data = self._radar.scan(self._gps.location()) raw_dynamic_radar_data = self._radar.scan_dynamic_obstacles( self._gps.location()) if self._with_predictor: self._obstacle_predictor.add_observation( self._gps.location(), raw_radar_data, raw_dynamic_radar_data, self._obstacle_predictor_dynobs_getter_func) normalized_radar_data = raw_radar_data / self._radar.radius if (0 < self._cmdargs.radar_noise_level): normalized_radar_data += self._gaussian_noise( self._cmdargs.radar_noise_level, normalized_radar_data.size) # Add the obstacle distribution into the combined PDF combined_pdf = self._combine_pdfs(combined_pdf, normalized_radar_data) # Process memory if self._cmdargs.enable_memory: mem_bias_pdf = self._create_memory_bias_pdf() # Add the memory distribution to the combined PDF combined_pdf = self._combine_pdfs(combined_pdf, mem_bias_pdf) obstacle_predictor_pdf = np.zeros(self._radar.get_data_size()) if self._with_predictor: obstacle_predictor_pdf = self._create_obstacle_predictor_pdf() combined_pdf = self._combine_pdfs(combined_pdf, obstacle_predictor_pdf) # Possibly smooth the combined distribution, to avoid # having sudden jumps in value if (self._cmdargs.enable_pdf_smoothing_filter): combined_pdf = self._putfilter(combined_pdf) combined_pdf = np.maximum(combined_pdf, 0) direction = self._pdf_angle_selector( combined_pdf) * self._PDF.degree_resolution # Set the speed speed = self._select_speed(direction, raw_dynamic_radar_data) if (self._cmdargs.show_real_time_plot): self._update_plot([ combined_pdf, targetpoint_pdf, mem_bias_pdf, normalized_radar_data, obstacle_predictor_pdf ]) return RobotControlInput(speed, direction)
def select_next_action(self): state = self._get_state() action = self._get_action(state) return RobotControlInput(action[1], action[0])
def select_next_action(self): return RobotControlInput(float('inf'), self._gps.angle_to(self._target.position));
def select_next_action(self): #Scan the radar self._dynamic_radar_data = self._radar.scan_dynamic_obstacles( self._gps.location()) self._radar_data = self._radar.scan(self._gps.location()) # Give the current observation to the obstacle motion predictor self.debug_info[ 'future_obstacles'] = self._obstacle_predictor.add_observation( self._gps.location(), self._radar_data, self._dynamic_radar_data, self._obstacle_predictor_dynobs_getter_func) if self._last_solution_node.data[:2] != (int( self._gps.location()[0]), int(self._gps.location()[1])): self._solution.insert(0, self._last_solution_node) self._pruneAndPrepend() if not self._rrt.hasGoal(self._targetpos, self._goalThreshold): self._rrt = Tree( Node((self._gps.location()[0], self._gps.location()[1], self._time))) qgoal = Node( (self._targetpos[0], self._targetpos[1], self._minTimeMultiplier * self._minTime(self._gps.location(), self._targetpos))) self._status = self._grow_rrt(self._rrt, qgoal, self._goalThreshold, True) self._extract_solution() if self._status == 0: #There is a valid path from robot to goal while 0 < len(self._solution) and ( Vector.distance_between(self._gps.location(), self._solution[0].data[:2]) < 1.5 or self._solution[0].data[2] <= self._time): del self._solution[0] timeDiff = self._solution[0].data[2] - self._time #Should always be > 0 direction = Vector.degrees_between(self._gps.location(), self._solution[0].data[:2]) dist = min( self._maxstepsize, Vector.distance_between(self._gps.location(), self._solution[0].data[:2]) / timeDiff) self.debug_info["path"] = self._solution self._last_solution_node = self._solution[0] self._time += 1 if self._status == 1: #No valid path found self._has_given_up = True dist = 0 direction = np.random.uniform(low=0, high=360) if self._status == 2: #Robot or goal is inside dynamic obstacle dist = 0 direction = np.random.uniform(low=0, high=360) return RobotControlInput(dist, direction)