def __createCircleShape(info: 'Dict[str, Any]') -> 'Circle': shape = Circle(None, info['radius'], (info.get('x', 0), info.get('y', 0))) shape.mass = info['mass'] shape.elasticity = info.get('elasticity', 0.5) shape.friction = info.get('friction', 0.5) return shape
def __init__(self, width=600, height=600, obstacle_num=5, obstacle_radius=30, feed_num=0, feed_radius=5): import pyglet from pymunk import Space, Segment, Body, Circle, moment_for_circle, pyglet_util super(VehicleSimulator, self).__init__() self.__left_sensor_val = 0 self.__right_sensor_val = 0 self.__feed_sensor_val = False self.__feed_touch_counter = {} self.__feed_bodies = [] self.__feed_radius = feed_radius self.__window = pyglet.window.Window( self.ARENA_SIZE + self.DISPLAY_MARGIN * 2, self.ARENA_SIZE + self.DISPLAY_MARGIN * 2, vsync=False) self.__draw_options = pyglet_util.DrawOptions() self.__closed = False @self.__window.event def on_draw(): pyglet.gl.glClearColor(255, 255, 255, 255) self.__window.clear() self.__simulation_space.debug_draw(self.__draw_options) @self.__window.event def on_close(): pyglet.app.EventLoop().exit() self.__closed = True self.__simulation_space = Space() self.__simulation_space.gravity = 0, 0 # arena walls = [ Segment( self.__simulation_space.static_body, (self.DISPLAY_MARGIN, self.DISPLAY_MARGIN), (self.ARENA_SIZE + self.DISPLAY_MARGIN, self.DISPLAY_MARGIN), 0), Segment( self.__simulation_space.static_body, (self.ARENA_SIZE + self.DISPLAY_MARGIN, self.DISPLAY_MARGIN), (self.ARENA_SIZE + self.DISPLAY_MARGIN, self.ARENA_SIZE + self.DISPLAY_MARGIN), 0), Segment( self.__simulation_space.static_body, (self.ARENA_SIZE + self.DISPLAY_MARGIN, self.ARENA_SIZE + self.DISPLAY_MARGIN), (self.DISPLAY_MARGIN, self.ARENA_SIZE + self.DISPLAY_MARGIN), 0), Segment( self.__simulation_space.static_body, (self.DISPLAY_MARGIN, self.ARENA_SIZE + self.DISPLAY_MARGIN), (self.DISPLAY_MARGIN, self.DISPLAY_MARGIN), 0) ] for w in walls: w.collision_type = self.COLLISION_TYPE.OBJECT w.friction = 0.2 self.__simulation_space.add(walls) # vehicle mass = 1 self.__vehicle_body = Body( mass, moment_for_circle(mass, 0, self.VEHICLE_RADIUS)) self.__vehicle_shape = Circle(self.__vehicle_body, self.VEHICLE_RADIUS) self.__vehicle_shape.friction = 0.2 self.__vehicle_shape.collision_type = self.COLLISION_TYPE.VEHICLE self.__simulation_space.add(self.__vehicle_body, self.__vehicle_shape) # left sensor sensor_l_s = Segment(self.__vehicle_body, (0, 0), (self.SENSOR_RANGE * np.cos(self.SENSOR_ANGLE), self.SENSOR_RANGE * np.sin(self.SENSOR_ANGLE)), 0) sensor_l_s.sensor = True sensor_l_s.collision_type = self.COLLISION_TYPE.LEFT_SENSOR handler_l = self.__simulation_space.add_collision_handler( self.COLLISION_TYPE.LEFT_SENSOR, self.COLLISION_TYPE.OBJECT) handler_l.pre_solve = self.__left_sensr_handler handler_l.separate = self.__left_sensr_separate_handler self.__simulation_space.add(sensor_l_s) # right sensor sensor_r_s = Segment(self.__vehicle_body, (0, 0), (self.SENSOR_RANGE * np.cos(-self.SENSOR_ANGLE), self.SENSOR_RANGE * np.sin(-self.SENSOR_ANGLE)), 0) sensor_r_s.sensor = True sensor_r_s.collision_type = self.COLLISION_TYPE.RIGHT_SENSOR handler_r = self.__simulation_space.add_collision_handler( self.COLLISION_TYPE.RIGHT_SENSOR, self.COLLISION_TYPE.OBJECT) handler_r.pre_solve = self.__right_sensr_handler handler_r.separate = self.__right_sensr_separate_handler self.__simulation_space.add(sensor_r_s) # obstacles for a in (np.linspace(0, np.pi * 2, obstacle_num, endpoint=False) + np.pi / 2): body = Body(body_type=Body.STATIC) body.position = (self.DISPLAY_MARGIN + self.ARENA_SIZE / 2 + self.ARENA_SIZE * 0.3 * np.cos(a), self.DISPLAY_MARGIN + self.ARENA_SIZE / 2 + self.ARENA_SIZE * 0.3 * np.sin(a)) shape = Circle(body, obstacle_radius) shape.friction = 0.2 shape.collision_type = self.COLLISION_TYPE.OBJECT self.__simulation_space.add(shape) for i in range(feed_num): body = Body(1, 1) self.__feed_bodies.append(body) shape = Circle(body, self.__feed_radius) shape.sensor = True shape.color = self.FEED_COLOR shape.collision_type = self.COLLISION_TYPE.FEED handler = self.__simulation_space.add_collision_handler( self.COLLISION_TYPE.VEHICLE, self.COLLISION_TYPE.FEED) handler.pre_solve = self.__feed_touch_handler handler.separate = self.__feed_separate_handler self.__simulation_space.add(body, shape) self.__feed_touch_counter[shape] = 0 self.reset()