def get_action(self, timestep_i, current_orientation, actions_checked=[]): perlin_noise = noise.pnoise1( (float(timestep_i * self.action_repeat) + self.offset0) / self.scale0) perlin_noise += noise.pnoise1( (float(timestep_i * self.action_repeat) + self.offset1) / self.scale1) action = int(perlin_noise * self.max_scaler) if action > self.max_scaler: action = self.max_scaler elif action < -self.max_scaler: action = -self.max_scaler action_samples = 0 while action in actions_checked and action_samples < 50: action_samples += 1 self.reset_action() perlin_noise = noise.pnoise1( (float(timestep_i * self.action_repeat) + self.offset0) / self.scale0) perlin_noise += noise.pnoise1( (float(timestep_i * self.action_repeat) + self.offset1) / self.scale1) action = int(perlin_noise * self.max_scaler) if action > self.max_scaler: action = self.max_scaler elif action < -self.max_scaler: action = -self.max_scaler steering_force = vector(action * self.wander_range + current_orientation) return action, steering_force
def turn_robot_around(self): previous_angle = vector(self.robot.body.angle) previous_position = self.robot.body.position steering_vector = -previous_angle + .01 * np.random.randn(2) turn_len = random.randint(0, 1) if turn_len == 0: for i in range(180): self.step(steering_vector, ignore_collisions=True) else: for i in range(250): self.step(steering_vector, ignore_collisions=True) self.robot.body.angular_velocity = 0 self.robot.body.velocity = (0, 0)
def get_action(self, current_position, current_orientation): seek_vector = self.target_position - current_position # if la.norm(seek_vector) < 50: # print('GOAL') # pdb.set_trace() # print(la.norm(seek_vector)) steering_vector = seek_vector - vector(current_orientation) action_space = np.arange(-5, 6) min_diff = 9999999 min_a = 0 for a in action_space: steering_force = vector(a * self.wander_range + current_orientation) diff = la.norm(steering_force - steering_vector) if diff <= min_diff: min_a = a min_diff = diff steering_force = vector(min_a * self.wander_range + current_orientation) return min_a, steering_force
def _reset_robot(self, center=False, collision_points=None): previous_angle = vector(self.robot.body.angle) previous_position = self.robot.body.position if collision_points: self.robot.body.angle = angle(collision_points[0].point_a - collision_points[0].point_b) else: self.robot.body.angle = angle(-previous_angle) if center: self.robot.body.position = self.CENTER else: self.robot.body.position = previous_position - (previous_angle * 25) self.robot.body.angular_velocity = 0 self.robot.body.velocity = (0,0)
def add_sensors(self, sensor_range=150.0): self.sensor_range = sensor_range sensor_shapes = [] sensor_end_points = [] sensor_angles = [66, 33, 0, -33, -66] for a in sensor_angles: angle = self.body.angle + math.radians(a) v = vector(angle) p = v * sensor_range sensor_end_points.append(p) thickness = 1 for p in sensor_end_points: sensor_shape = pm.Segment(self.body, (0, 0), p, thickness) sensor_shape.color = BLUE sensor_shape.sensor = True sensor_shapes.append(sensor_shape) return sensor_shapes, sensor_angles, sensor_range
def raycasting(self, print_sensors=False): robot_filter = pm.ShapeFilter(mask=pm.ShapeFilter.ALL_MASKS ^ 0b1) sensor_end_points=[] for a in self.robot.sensor_angles: angle = self.robot.body.angle + math.radians(a) v = vector(angle) p = v * self.robot.sensor_range + self.robot.body.position sensor_end_points.append(p) segment_queries = [] for i, p in enumerate(sensor_end_points): segment_query = self.space.segment_query_first(self.robot.body.position,p,1,robot_filter) if segment_query: segment_queries.append(la.norm(segment_query.point - self.robot.body.position)) else: segment_queries.append(self.robot.sensor_range) sq = np.array(segment_queries) if print_sensors: print("%d %d %d %d %d"%(int(sq[0]),int(sq[1]),int(sq[2]),int(sq[3]),int(sq[4]))) return sq
def get_steering_force(self, action, current_orientation): steering_force = vector(action * self.wander_range + current_orientation) return steering_force