예제 #1
0
    def __init__(self):
        super(Pulley, self).__init__()
        y, L, a, b = 16.0, 12.0, 1.0, 2.0
        # The ground
        ground = self.world.create_static_body(
            shapes=[edgeShape(vertices=[(-40, 0), (40, 0)]),
                    circleShape(radius=2, pos=(-10.0, y + b + L)),
                    circleShape(radius=2, pos=(10.0, y + b + L))]
        )

        body_a = self.world.create_dynamic_body(
            position=(-10, y),
            fixtures=fixtureDef(shape=polygonShape(box=(a, b)), density=5.0),
        )
        body_b = self.world.create_dynamic_body(
            position=(10, y),
            fixtures=fixtureDef(shape=polygonShape(box=(a, b)), density=5.0),
        )

        self.pulley = self.world.CreatePulleyJoint(
            body_a=body_a,
            body_b=body_b,
            anchorA=(-10.0, y + b),
            anchorB=(10.0, y + b),
            groundAnchorA=(-10.0, y + b + L),
            groundAnchorB=(10.0, y + b + L),
            ratio=1.5,
        )
예제 #2
0
파일: car_dynamics.py 프로젝트: Aishunk/gym
 def __init__(self, world, init_angle, init_x, init_y):
     self.world = world
     self.hull = self.world.CreateDynamicBody(
         position = (init_x, init_y),
         angle = init_angle,
         fixtures = [
             fixtureDef(shape = polygonShape(vertices=[ (x*SIZE,y*SIZE) for x,y in HULL_POLY1 ]), density=1.0),
             fixtureDef(shape = polygonShape(vertices=[ (x*SIZE,y*SIZE) for x,y in HULL_POLY2 ]), density=1.0),
             fixtureDef(shape = polygonShape(vertices=[ (x*SIZE,y*SIZE) for x,y in HULL_POLY3 ]), density=1.0),
             fixtureDef(shape = polygonShape(vertices=[ (x*SIZE,y*SIZE) for x,y in HULL_POLY4 ]), density=1.0)
             ]
         )
     self.hull.color = (0.8,0.0,0.0)
     self.wheels = []
     self.fuel_spent = 0.0
     WHEEL_POLY = [
         (-WHEEL_W,+WHEEL_R), (+WHEEL_W,+WHEEL_R),
         (+WHEEL_W,-WHEEL_R), (-WHEEL_W,-WHEEL_R)
         ]
     for wx,wy in WHEELPOS:
         front_k = 1.0 if wy > 0 else 1.0
         w = self.world.CreateDynamicBody(
             position = (init_x+wx*SIZE, init_y+wy*SIZE),
             angle = init_angle,
             fixtures = fixtureDef(
                 shape=polygonShape(vertices=[ (x*front_k*SIZE,y*front_k*SIZE) for x,y in WHEEL_POLY ]),
                 density=0.1,
                 categoryBits=0x0020,
                 maskBits=0x001,
                 restitution=0.0)
                 )
         w.wheel_rad = front_k*WHEEL_R*SIZE
         w.color = WHEEL_COLOR
         w.gas   = 0.0
         w.brake = 0.0
         w.steer = 0.0
         w.phase = 0.0  # wheel angle
         w.omega = 0.0  # angular velocity
         w.skid_start = None
         w.skid_particle = None
         rjd = revoluteJointDef(
             bodyA=self.hull,
             bodyB=w,
             localAnchorA=(wx*SIZE,wy*SIZE),
             localAnchorB=(0,0),
             enableMotor=True,
             enableLimit=True,
             maxMotorTorque=180*900*SIZE*SIZE,
             motorSpeed = 0,
             lowerAngle = -0.4,
             upperAngle = +0.4,
             )
         w.joint = self.world.CreateJoint(rjd)
         w.tiles = set()
         w.userData = w
         self.wheels.append(w)
     self.drawlist =  self.wheels + [self.hull]
     self.particles = []
예제 #3
0
    def _create_lander(self, lander, number_of_landers):
        initial_y = VIEWPORT_H/SCALE
        body_xpos = (lander.player_id + 1) * VIEWPORT_W/SCALE / (number_of_landers + 1)
        body_ypos = initial_y
        body = self.world.CreateDynamicBody(
            position = (body_xpos, body_ypos),
            angle=0.0,
            fixtures = fixtureDef(
                shape=polygonShape(vertices=[ (x/SCALE,y/SCALE) for x,y in LANDER_POLY ]),
                density=5.0,
                friction=0.1,
                categoryBits=0x0010,
                maskBits=0x001,  # collide only with ground
                restitution=0.0) # 0.99 bouncy
                )
        body.color1 = (0.5,0.4,0.9)
        body.color2 = (0.3,0.3,0.5)
        body.ApplyForceToCenter( (
            self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM),
            self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM)
            ), True)

        legs = []
        for i in [-1,+1]:
            leg = self.world.CreateDynamicBody(
                position = (body_xpos - i*LEG_AWAY/SCALE, body_ypos),
                angle = (i*0.05),
                fixtures = fixtureDef(
                    shape=polygonShape(box=(LEG_W/SCALE, LEG_H/SCALE)),
                    density=1.0,
                    restitution=0.0,
                    categoryBits=0x0020,
                    maskBits=0x001)
                )
            leg.ground_contact = False
            leg.color1 = (0.5,0.4,0.9)
            leg.color2 = (0.3,0.3,0.5)
            rjd = revoluteJointDef(
                bodyA=body,
                bodyB=leg,
                localAnchorA=(0, 0),
                localAnchorB=(i*LEG_AWAY/SCALE, LEG_DOWN/SCALE),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=LEG_SPRING_TORQUE,
                motorSpeed=+0.3*i  # low enough not to jump back into the sky
                )
            if i==-1:
                rjd.lowerAngle = +0.9 - 0.5  # Yes, the most esoteric numbers here, angles legs have freedom to travel within
                rjd.upperAngle = +0.9
            else:
                rjd.lowerAngle = -0.9
                rjd.upperAngle = -0.9 + 0.5
            leg.joint = self.world.CreateJoint(rjd)
            legs.append(leg)
        lander.body, lander.legs = body, legs
예제 #4
0
    def __init__(self):
        self.trackWidth = 5.0
        self.cartWidth = 0.3
        self.cartHeight = 0.2
        self.cartMass = 10
        self.poleMass = 1
        self.force = 0.2
        self.trackThickness = self.cartHeight
        self.poleLength = 0.5
        self.poleThickness = 0.04

        self.screenSize = (640,480) #origin upper left
        self.worldSize = (float(self.trackWidth),float(self.trackWidth)) #origin at center

        self.world = b2.world(gravity=(0,-9.81),doSleep=True)
        self.framesPerSecond = 10 # used for dynamics update and for graphics update
        self.velocityIterations = 8
        self.positionIterations = 6

        # Make track bodies and fixtures
        self.trackColor = (100,100,100)
        poleCategory = 0x0002

        f = b2.fixtureDef(shape=b2.polygonShape(box=(self.trackWidth/2,self.trackThickness/2)),
                          friction=0.000, categoryBits=0x0001, maskBits=(0xFFFF & ~poleCategory))
        self.track = self.world.CreateStaticBody(position = (0,0), 
                                                 fixtures=f, userData={'color': self.trackColor})
        self.trackTop = self.world.CreateStaticBody(position = (0,self.trackThickness+self.cartHeight*1.1),
                                                    fixtures = f)

        f = b2.fixtureDef(shape=b2.polygonShape(box=(self.trackThickness/2,self.trackThickness/2)),
                          friction=0.000, categoryBits=0x0001, maskBits=(0xFFFF & ~poleCategory))
        self.wallLeft = self.world.CreateStaticBody(position = (-self.trackWidth/2+self.trackThickness/2, self.trackThickness),
                                               fixtures=f, userData={'color': self.trackColor})
        self.wallRight = self.world.CreateStaticBody(position = (self.trackWidth/2-self.trackThickness/2, self.trackThickness),
                                                fixtures=f,userData={'color': self.trackColor})

        # Make cart body and fixture
        f = b2.fixtureDef(shape=b2.polygonShape(box=(self.cartWidth/2,self.cartHeight/2)),
                          density=self.cartMass, friction=0.000, restitution=0.5, categoryBits=0x0001,
                          maskBits=(0xFFFF & ~poleCategory))
        self.cart = self.world.CreateDynamicBody(position=(0,self.trackThickness),
                                            fixtures=f, userData={'color':(20,200,0)})

        # Make pole pody and fixture
        # Initially pole is hanging down, which defines the zero angle.
        f = b2.fixtureDef(shape=b2.polygonShape(box=(self.poleThickness/2, self.poleLength/2)),
                          density=self.poleMass, categoryBits=poleCategory)
        self.pole = self.world.CreateDynamicBody(position=(0,self.trackThickness+self.cartHeight/2+self.poleThickness-self.poleLength/2),
                                            fixtures=f, userData={'color':(200,20,0)})

        # Make pole-cart joint
        rj = self.world.CreateRevoluteJoint(bodyA = self.pole, bodyB = self.cart,
                                  anchor=(0,self.trackThickness+self.cartHeight/2+self.poleThickness))
예제 #5
0
파일: chain.py 프로젝트: aaronma37/pybox2d
    def __init__(self):
        super(Chain, self).__init__()

        # The ground
        ground = self.world.CreateBody(
            shapes=edgeShape(vertices=[(-40, 0), (40, 0)])
        )

        plank = fixtureDef(
            shape=polygonShape(box=(0.6, 0.125)),
            density=20,
            friction=0.2,
        )

        # Create one Chain (Only the left end is fixed)
        prevBody = ground
        y = 25
        numPlanks = 30
        for i in range(numPlanks):
            body = self.world.CreateDynamicBody(
                position=(0.5 + i, y),
                fixtures=plank,
            )

            # You can try a WeldJoint for a slightly different effect.
            # self.world.CreateWeldJoint(
            self.world.CreateRevoluteJoint(
                bodyA=prevBody,
                bodyB=body,
                anchor=(i, y),
            )

            prevBody = body
예제 #6
0
    def __init__(self, world):
        super(Level1, self).__init__(world)
        layer = core.Layer()
        self.add_layer(layer)
        
        self.ground = world.CreateStaticBody(
                                             position=(0, -2),
                                             shapes=b2.polygonShape(box=(50, 5))
                                             )
        
        self.im0 = sf.Texture.load_from_file(b'princess.png')
        for x in range(100, 800, 200):
            sprite = sf.Sprite(self.im0)
            sprite.origin = (self.im0.width//2, self.im0.height//2)
#            sprite.position = (x, 300)
            body = world.CreateDynamicBody()
            body.linearDamping = 10.0
            body.CreateCircleFixture(radius=2.5, density=10.0, friction=0.3)
            body.position = self.convert_coords_to_b2((x, 300))
            body.fixedRotation = True
            actor = core.Actor(sprite, behavior=box2d.Box2DBehavior(body))
            layer.add_actor(actor)
        actors = self.layers[0].actors
        act = actions.Chain([actions.Move(10.0, 400, -50),
                             actions.Rotate(3.0, 360),
                             actions.MoveTo(2.0, 100, 300),
                             actions.MoveTo(10.0, 700, 300)])
        actors[0].add_action(act)
        actors[1].add_action(actions.Move(5.0, -400, -50))
        actors[2].add_action(actions.MoveTo(15.0, 10, 180))
        act2 = actions.Chain([actions.Pause(5.0), actions.Kill()])
        actors[3].add_action(act2)
예제 #7
0
    def __init__(self):
        self.seed()
        self.viewer = None

        self.world = Box2D.b2World()
        self.terrain = None
        self.hull = None

        self.prev_shaping = None

        self.fd_polygon = fixtureDef(
                        shape = polygonShape(vertices=
                        [(0, 0),
                         (1, 0),
                         (1, -1),
                         (0, -1)]),
                        friction = FRICTION)

        self.fd_edge = fixtureDef(
                    shape = edgeShape(vertices=
                    [(0, 0),
                     (1, 1)]),
                    friction = FRICTION,
                    categoryBits=0x0001,
                )

        self.reset()

        high = np.array([np.inf]*24)
        self.action_space = spaces.Box(np.array([-1,-1,-1,-1]), np.array([+1,+1,+1,+1]))
        self.observation_space = spaces.Box(-high, high)
def example_world():
    """Create an example Box2D world for testing

    Returns:
        tuple(world, bodies, configuration):
            - world is a Box2D world with multiple dynamic bodies
            - bodies is a dictionary mapping object names to their Box2D
              body
            - configuration is an example collision-free configuration
    """
    get_triangles = metis.geometry.box2d_triangles_from_shapely

    obstacle_geometry = shapely.geometry.box(0, 0, 10, 10)
    obstacle_geometry = obstacle_geometry.difference(
        obstacle_geometry.buffer(-.2))
    obstacle_geometry = obstacle_geometry.union(
        shapely.geometry.LineString([(5, 0), (5, 10)]).buffer(.1, cap_style=2))
    obstacle_geometry = obstacle_geometry.difference(
        shapely.geometry.Point(5, 2.5).buffer(1, cap_style=1))
    obstacle_geometry = obstacle_geometry.difference(
        shapely.geometry.Point(5, 7.5).buffer(1, cap_style=1))

    world = b2.world()
    obstacles = world.CreateStaticBody()
    for triangle in get_triangles(obstacle_geometry):
        _ = obstacles.CreateFixture(shape=triangle)

    agent = world.CreateDynamicBody()
    agent_geometry = shapely.geometry.Polygon([
        (2./3., 0.), (-1./3., .4), (-1./3., -.4)])
    for triangle in get_triangles(agent_geometry):
        _ = agent.CreateFixture(shape=triangle)

    boxes = [world.CreateDynamicBody() for _ in xrange(2)]
    for box in boxes:
        box.CreateFixture(shape=b2.polygonShape(box=(.8, .8)))

    bodies = {'robot': agent, 'box1': boxes[0], 'box2': boxes[1]}
    sample_configuration = {
        'robot': (1, 2, 0), 'box1': (3, 2, -.2), 'box2': (5, 2.5, 0.1)}

    return world, bodies, sample_configuration
예제 #9
0
    def addPhysicsObject(self, physObj, color=None):
        '''
        Adds a PhysicsObject to this SimulationEnvironment,
        adding it to the collision manager and to the renderables
        '''
        if isinstance(physObj, StaticPhysicsObject):
            body = self.world.CreateStaticBody(position=physObj.position,
                                        shapes=b2.polygonShape(box=physObj.size))
        elif isinstance(physObj,DynamicPhysicsObject):
            if len(physObj.size) != 2:
                raise ValueError("Size must be a two-tuple")
            body = self.world.CreateDynamicBody(position=physObj.position, angle=physObj.angle) 
            halfSize = tuple(x/2.0 for x in physObj.size) #box takes half size, not full size
            body.CreatePolygonFixture(box=halfSize, density=physObj.density, friction=physObj.friction)
        else:
            raise ValueError("Object was neither a StaticPhysicsObject nor a DynamicPhysicsObject. Unexpected")


        self.objectDict[physObj.identifier] = body
        return body
예제 #10
0
파일: box2d_01.py 프로젝트: klavdijv/pug2d
 def __init__(self, world):
     super(Level1, self).__init__(world)
     layer = core.Layer()
     self.add_layer(layer)
     
     self.ground = world.CreateStaticBody(
                                          position=(0, -2),
                                          shapes=b2.polygonShape(box=(50, 5))
                                          )
     
     self.im0 = sf.Texture.load_from_file(b'princess.png')
     for y in range(0, 500, 120):
         for x in range(200, 700, 120):
             sprite = sf.Sprite(self.im0)
             sprite.origin = (self.im0.width//2, self.im0.height//2)
             sprite.position = (x, y)
             actor = core.Actor(sprite)
             body = world.CreateDynamicBody()
             body.CreateCircleFixture(radius=2.5, density=1.0, friction=0.3)
             actor.add_action(Updater(body), name='box2d')
             layer.add_actor(actor)
예제 #11
0
파일: box2d_los.py 프로젝트: klavdijv/pug2d
 def __init__(self, world):
     super(Level1, self).__init__(world)
     layer = core.Layer()
     self.add_layer(layer)
     
     self.ground = world.CreateStaticBody(
                                          position=(0, -2),
                                          shapes=b2.polygonShape(box=(50, 5))
                                          )
     
     self.im0 = sf.Texture.load_from_file(b'princess.png')
     for x in range(100, 800, 300):
         sprite = sf.Sprite(self.im0)
         sprite.origin = (self.im0.width//2, self.im0.height//2)
         sprite.position = (x, 300)
         actor = core.Actor(sprite)
         body = world.CreateDynamicBody()
         body.CreateCircleFixture(radius=2.5, density=1.0, friction=0.3)
         actor.add_action(Updater(body), name='box2d')
         layer.add_actor(actor)
     actors = self.layers[0].actors
     actors[1].obstacle = True
     actors[0].add_action(actions.DefferedCall(1.0, self.test_los))
예제 #12
0
파일: room.py 프로젝트: ctoth/shooter
	def __init__(self, world, name=None, size=(1, 1), exits=None, floor=None, contents=None):
		self.world = world
		self.name = name
		corners = [
			(0, 0),
			(0, size[0] + 1),
			(size[1] + 1, size[0] + 1),
			(size[1] + 1, 0)
		]
		exit_coords = []
		for exit in exits:
			exit_pos = exit.values()[0]
			exit_coords.append((exit_pos[0]+1, exit_pos[1]))
		filled_tiles = []
		filled_tiles.extend(tiles_for_wall(corners[0], corners[1], exit_coords))
		filled_tiles.extend(tiles_for_wall(corners[1], corners[2], exit_coords))
		filled_tiles.extend(tiles_for_wall(corners[2], corners[3], exit_coords))
		filled_tiles.extend(tiles_for_wall(corners[3], corners[0], exit_coords))
		self.exit_coords = exit_coords
		self.filled_tiles = filled_tiles
		self.floor = floor
		if exits is None:
			exits = []
		exits.sort()
		self.exits = exits
		bodies = []
		for tile in filled_tiles:
			position = (tile[0] + 0.5, tile[1] + 0.5)
			shape = b2.polygonShape(box=(0.5, 0.5))
			bodies.append(self.world.world.CreateStaticBody(shapes=shape, position=position, userData=self))
		self.bodies = bodies
		#dictionary of {coords: object}
		if contents is None:
			contents = {}
		for i in contents.values():
			i.location = self
		self.contents = contents
예제 #13
0
    def _generate_terrain(self, hardcore):
        GRASS, STUMP, STAIRS, PIT, _STATES_ = range(5)
        state    = GRASS
        velocity = 0.0
        y        = TERRAIN_HEIGHT
        counter  = TERRAIN_STARTPAD
        oneshot  = False
        self.terrain   = []
        self.terrain_x = []
        self.terrain_y = []
        for i in range(TERRAIN_LENGTH):
            x = i*TERRAIN_STEP
            self.terrain_x.append(x)

            if state==GRASS and not oneshot:
                velocity = 0.8*velocity + 0.01*np.sign(TERRAIN_HEIGHT - y)
                if i > TERRAIN_STARTPAD: velocity=0 #+= self.np_random.uniform(-1, 1)/SCALE   #1
                y += velocity
                #y = TERRAIN_HEIGHT

            elif state==PIT and oneshot:
                counter = self.np_random.randint(3, 5)
                poly = [
                    (x,              y),
                    (x+TERRAIN_STEP, y),
                    (x+TERRAIN_STEP, y-4*TERRAIN_STEP),
                    (x,              y-4*TERRAIN_STEP),
                    ]
                t = self.world.CreateStaticBody(
                    fixtures = fixtureDef(
                        shape=polygonShape(vertices=poly),
                        friction = FRICTION
                    ))
                t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
                self.terrain.append(t)
                t = self.world.CreateStaticBody(
                    fixtures = fixtureDef(
                        shape=polygonShape(vertices=[(p[0]+TERRAIN_STEP*counter,p[1]) for p in poly]),
                        friction = FRICTION
                    ))
                t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
                self.terrain.append(t)
                counter += 2
                original_y = y

            elif state==PIT and not oneshot:
                y = original_y
                if counter > 1:
                    y -= 4*TERRAIN_STEP

            elif state==STUMP and oneshot:
                counter = self.np_random.randint(1, 3)
                poly = [
                    (x,                      y),
                    (x+counter*TERRAIN_STEP, y),
                    (x+counter*TERRAIN_STEP, y+counter*TERRAIN_STEP),
                    (x,                      y+counter*TERRAIN_STEP),
                    ]
                t = self.world.CreateStaticBody(
                    fixtures = fixtureDef(
                        shape=polygonShape(vertices=poly),
                        friction = FRICTION
                    ))
                t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
                self.terrain.append(t)

            elif state==STAIRS and oneshot:
                stair_height = +1 if self.np_random.rand() > 0.5 else -1
                stair_width = self.np_random.randint(4, 5)
                stair_steps = self.np_random.randint(3, 5)
                original_y = y
                for s in range(stair_steps):
                    poly = [
                        (x+(    s*stair_width)*TERRAIN_STEP, y+(   s*stair_height)*TERRAIN_STEP),
                        (x+((1+s)*stair_width)*TERRAIN_STEP, y+(   s*stair_height)*TERRAIN_STEP),
                        (x+((1+s)*stair_width)*TERRAIN_STEP, y+(-1+s*stair_height)*TERRAIN_STEP),
                        (x+(    s*stair_width)*TERRAIN_STEP, y+(-1+s*stair_height)*TERRAIN_STEP),
                        ]
                    t = self.world.CreateStaticBody(
                        fixtures = fixtureDef(
                            shape=polygonShape(vertices=poly),
                            friction = FRICTION
                        ))
                    t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
                    self.terrain.append(t)
                counter = stair_steps*stair_width

            elif state==STAIRS and not oneshot:
                s = stair_steps*stair_width - counter - stair_height
                n = s/stair_width
                y = original_y + (n*stair_height)*TERRAIN_STEP

            oneshot = False
            self.terrain_y.append(y)
            counter -= 1
            if counter==0:
                counter = self.np_random.randint(TERRAIN_GRASS/2, TERRAIN_GRASS)
                if state==GRASS and hardcore:
                    state = self.np_random.randint(1, _STATES_)
                    oneshot = True
                else:
                    state = GRASS
                    oneshot = True

        self.terrain_poly = []
        for i in range(TERRAIN_LENGTH-1):
            poly = [
                (self.terrain_x[i],   self.terrain_y[i]),
                (self.terrain_x[i+1], self.terrain_y[i+1])
                ]
            t = self.world.CreateStaticBody(
                fixtures = fixtureDef(
                    shape=edgeShape(vertices=poly),
                    friction = FRICTION,
                    categoryBits=0x0001,
                ))
            color = (0.3, 1.0 if i%2==0 else 0.8, 0.3)
            t.color1 = color
            t.color2 = color
            self.terrain.append(t)
            color = (0.4, 0.6, 0.3)
            poly += [ (poly[1][0], 0), (poly[0][0], 0) ]
            self.terrain_poly.append( (poly, color) )
        self.terrain.reverse()
    def reset(self):
        self._destroy()
        self.world.contactListener_keepref = ContactDetector(self)
        self.world.contactListener = self.world.contactListener_keepref
        self.game_over = False
        self.prev_shaping = None
        self.throttle = 0
        self.gimbal = 0.0
        self.landed_ticks = 0
        self.stepnumber = 0
        self.smoke = []
        self.episode_number += 1

        # self.terrainheigth = self.np_random.uniform(H / 20, H / 10)
        self.terrainheigth = H / 20
        self.shipheight = self.terrainheigth + SHIP_HEIGHT
        # ship_pos = self.np_random.uniform(0, SHIP_WIDTH / SCALE) + SHIP_WIDTH / SCALE
        ship_pos = W / 2
        self.helipad_x1 = ship_pos - SHIP_WIDTH / 2
        self.helipad_x2 = self.helipad_x1 + SHIP_WIDTH
        self.helipad_y = self.terrainheigth + SHIP_HEIGHT

        self.water = self.world.CreateStaticBody(
            fixtures=fixtureDef(
                shape=polygonShape(
                    vertices=(
                        (0, 0),
                        (W, 0),
                        (W, self.terrainheigth),
                        (0, self.terrainheigth),
                    )
                ),
                friction=0.1,
                restitution=0.0,
            )
        )
        self.water.color1 = rgb(70, 96, 176)

        self.ship = self.world.CreateStaticBody(
            fixtures=fixtureDef(
                shape=polygonShape(
                    vertices=(
                        (self.helipad_x1, self.terrainheigth),
                        (self.helipad_x2, self.terrainheigth),
                        (self.helipad_x2, self.terrainheigth + SHIP_HEIGHT),
                        (self.helipad_x1, self.terrainheigth + SHIP_HEIGHT),
                    )
                ),
                friction=0.5,
                restitution=0.0,
            )
        )

        self.containers = []
        for side in [-1, 1]:
            self.containers.append(
                self.world.CreateStaticBody(
                    fixtures=fixtureDef(
                        shape=polygonShape(
                            vertices=(
                                (
                                    ship_pos + side * 0.95 * SHIP_WIDTH / 2,
                                    self.helipad_y,
                                ),
                                (
                                    ship_pos + side * 0.95 * SHIP_WIDTH / 2,
                                    self.helipad_y + SHIP_HEIGHT,
                                ),
                                (
                                    ship_pos
                                    + side * 0.95 * SHIP_WIDTH / 2
                                    - side * SHIP_HEIGHT,
                                    self.helipad_y + SHIP_HEIGHT,
                                ),
                                (
                                    ship_pos
                                    + side * 0.95 * SHIP_WIDTH / 2
                                    - side * SHIP_HEIGHT,
                                    self.helipad_y,
                                ),
                            )
                        ),
                        friction=0.2,
                        restitution=0.0,
                    )
                )
            )
            self.containers[-1].color1 = rgb(206, 206, 2)

        self.ship.color1 = (0.2, 0.2, 0.2)

        def initial_rocket_pos(level):

            if level > 0:
                initial_x = W / 2 + W * np.random.uniform(-0.3, 0.3)
                initial_y = H * 0.95
            else:
                initial_x = W / 2 + W * np.random.uniform(-0.03, 0.03)
                initial_y = H * 0.95
            return initial_x, initial_y

        initial_x, initial_y = initial_rocket_pos(self.level_number)
        self.lander = self.world.CreateDynamicBody(
            position=(initial_x, initial_y),
            angle=0.0,
            fixtures=fixtureDef(
                shape=polygonShape(
                    vertices=(
                        (-ROCKET_WIDTH / 2, 0),
                        (+ROCKET_WIDTH / 2, 0),
                        (ROCKET_WIDTH / 2, +ROCKET_HEIGHT),
                        (-ROCKET_WIDTH / 2, +ROCKET_HEIGHT),
                    )
                ),
                density=1.0,
                friction=0.5,
                categoryBits=0x0010,
                maskBits=0x001,
                restitution=0.0,
            ),
        )

        self.lander.color1 = rgb(230, 230, 230)

        leg_length_modified = compute_leg_length(LEG_LENGTH, self.level_number)
        for i in [-1, +1]:
            leg = self.world.CreateDynamicBody(
                position=(initial_x - i * LEG_AWAY, initial_y + ROCKET_WIDTH * 0.2),
                angle=(i * BASE_ANGLE),
                fixtures=fixtureDef(
                    shape=polygonShape(
                        vertices=(
                            (0, 0),
                            (0, leg_length_modified / 25),
                            (i * leg_length_modified, 0),
                            (i * leg_length_modified, -leg_length_modified / 20),
                            (i * leg_length_modified / 3, -leg_length_modified / 7),
                        )
                    ),
                    density=1,
                    restitution=0.0,
                    friction=0.2,
                    categoryBits=0x0020,
                    maskBits=0x001,
                ),
            )
            leg.ground_contact = False
            leg.color1 = (0.25, 0.25, 0.25)
            rjd = revoluteJointDef(
                bodyA=self.lander,
                bodyB=leg,
                localAnchorA=(i * LEG_AWAY, ROCKET_WIDTH * 0.2),
                localAnchorB=(0, 0),
                enableLimit=True,
                maxMotorTorque=2500.0,
                motorSpeed=-0.05 * i,
                enableMotor=True,
            )
            djd = distanceJointDef(
                bodyA=self.lander,
                bodyB=leg,
                anchorA=(i * LEG_AWAY, ROCKET_HEIGHT / 8),
                anchorB=leg.fixtures[0].body.transform * (i * leg_length_modified, 0),
                collideConnected=False,
                frequencyHz=0.01,
                dampingRatio=0.9,
            )
            if i == 1:
                rjd.lowerAngle = -SPRING_ANGLE
                rjd.upperAngle = 0
            else:
                rjd.lowerAngle = 0
                rjd.upperAngle = +SPRING_ANGLE
            leg.joint = self.world.CreateJoint(rjd)
            leg.joint2 = self.world.CreateJoint(djd)

            self.legs.append(leg)

        def random_factor_velocity(INITIAL_RANDOM, level):
            if level > 1:
                return INITIAL_RANDOM + 0.2
            else:
                return INITIAL_RANDOM

        random_velocity_factor = random_factor_velocity(
            INITIAL_RANDOM, self.level_number
        )

        self.lander.linearVelocity = (
            -self.np_random.uniform(0, random_velocity_factor)
            * START_SPEED
            * (initial_x - W / 2)
            / W,
            -START_SPEED,
        )

        self.lander.angularVelocity = (1 + random_velocity_factor) * np.random.uniform(
            -1, 1
        )

        self.drawlist = (
            self.legs + [self.water] + [self.ship] + self.containers + [self.lander]
        )

        if self.continuous:
            return self.step([0, 0, 0])[0]
        else:
            return self.step(6)[0]
예제 #15
0
    def _create_track(self):

        CHECKPOINTS = 12  # 12 = nombre de virages (11) + le départ (1)

        # Create checkpoints
        checkpoints = []
        self.obstacles_positions = []
        for c in range(CHECKPOINTS):
            noise = self.np_random.uniform(0, 2 * 3.14159 * 1 / CHECKPOINTS)
            alpha = 2 * 3.14159 * c / CHECKPOINTS + noise
            rad = self.np_random.uniform(TRACK_RAD / 3, TRACK_RAD)

            if c == 0:
                alpha = 0
                rad = 1.5 * TRACK_RAD
            if c == CHECKPOINTS - 1:
                alpha = 2 * 3.14159 * c / CHECKPOINTS
                self.start_alpha = 2 * 3.14159 * (-0.5) / CHECKPOINTS
                rad = 1.5 * TRACK_RAD

            checkpoints.append(
                (alpha, rad * math.cos(alpha), rad * math.sin(alpha)))
        self.road = []

        # Go from one checkpoint to another to create track
        x, y, beta = 1.5 * TRACK_RAD, 0, 0
        dest_i = 0
        laps = 0
        track = []
        no_freeze = 2500
        visited_other_side = False
        while True:
            alpha = math.atan2(y, x)
            if visited_other_side and alpha > 0:
                laps += 1
                visited_other_side = False
            if alpha < 0:
                visited_other_side = True
                alpha += 2 * 3.14159

            while True:  # Find destination from checkpoints
                failed = True

                while True:
                    dest_alpha, dest_x, dest_y = checkpoints[dest_i %
                                                             len(checkpoints)]
                    if alpha <= dest_alpha:
                        failed = False
                        break
                    dest_i += 1
                    if dest_i % len(checkpoints) == 0:
                        break

                if not failed:
                    break

                alpha -= 2 * 3.14159
                continue

            r1x = math.cos(beta)
            r1y = math.sin(beta)
            p1x = -r1y
            p1y = r1x
            dest_dx = dest_x - x  # vector towards destination
            dest_dy = dest_y - y
            # destination vector projected on rad:
            proj = r1x * dest_dx + r1y * dest_dy
            while beta - alpha > 1.5 * 3.14159:
                beta -= 2 * 3.14159
            while beta - alpha < -1.5 * 3.14159:
                beta += 2 * 3.14159
            prev_beta = beta
            proj *= SCALE
            if proj > 0.3:
                beta -= min(TRACK_TURN_RATE, abs(0.001 * proj))
            if proj < -0.3:
                beta += min(TRACK_TURN_RATE, abs(0.001 * proj))
            x += p1x * TRACK_DETAIL_STEP
            y += p1y * TRACK_DETAIL_STEP
            track.append((alpha, prev_beta * 0.5 + beta * 0.5, x, y))
            if laps > 4:
                break
            no_freeze -= 1
            if no_freeze == 0:
                break

        # Find closed loop range i1..i2, first loop should be ignored, second is OK
        i1, i2 = -1, -1
        i = len(track)
        while True:
            i -= 1
            if i == 0:
                return False  # Failed
            pass_through_start = track[i][0] > self.start_alpha and track[
                i - 1][0] <= self.start_alpha
            if pass_through_start and i2 == -1:
                i2 = i
            elif pass_through_start and i1 == -1:
                i1 = i
                break
        if self.verbose == 1:
            print("Track generation: %i..%i -> %i-tiles track" %
                  (i1, i2, i2 - i1))
        assert i1 != -1
        assert i2 != -1

        track = track[i1:i2 - 1]

        first_beta = track[0][1]
        first_perp_x = math.cos(first_beta)
        first_perp_y = math.sin(first_beta)
        # Length of perpendicular jump to put together head and tail
        well_glued_together = np.sqrt(
            np.square(first_perp_x * (track[0][2] - track[-1][2])) +
            np.square(first_perp_y * (track[0][3] - track[-1][3])))
        if well_glued_together > TRACK_DETAIL_STEP:
            return False

        # Red-white border on hard turns
        border = [False] * len(track)
        for i in range(len(track)):
            good = True
            oneside = 0
            for neg in range(BORDER_MIN_COUNT):
                beta1 = track[i - neg - 0][1]
                beta2 = track[i - neg - 1][1]
                good &= abs(beta1 - beta2) > TRACK_TURN_RATE * 0.2
                oneside += np.sign(beta1 - beta2)
            good &= abs(oneside) == BORDER_MIN_COUNT
            border[i] = good
        for i in range(len(track)):
            for neg in range(BORDER_MIN_COUNT):
                border[i - neg] |= border[i]

        # Create tiles and obstacles
        last_obstacle = 15  # pour que le début de course se passe sans obstacle
        for i in range(len(track)):

            alpha1, beta1, x1, y1 = track[i]
            alpha2, beta2, x2, y2 = track[i - 1]
            road1_l = (x1 - TRACK_WIDTH * math.cos(beta1),
                       y1 - TRACK_WIDTH * math.sin(beta1))
            road1_r = (x1 + TRACK_WIDTH * math.cos(beta1),
                       y1 + TRACK_WIDTH * math.sin(beta1))
            road2_l = (x2 - TRACK_WIDTH * math.cos(beta2),
                       y2 - TRACK_WIDTH * math.sin(beta2))
            road2_r = (x2 + TRACK_WIDTH * math.cos(beta2),
                       y2 + TRACK_WIDTH * math.sin(beta2))
            vertices = [road1_l, road1_r, road2_r, road2_l]

            self.fd_tile.shape.vertices = vertices
            t = self.world.CreateStaticBody(fixtures=self.fd_tile)
            t.userData = t
            c = 0.01 * (i % 3)
            t.color = [0, 0.128, 0.624, 0.019]
            t.road_visited = False
            t.road_friction = 1.0
            t.fixtures[0].sensor = True
            self.road_poly.append(([road1_l, road1_r, road2_r,
                                    road2_l], t.color))
            self.road.append(t)
            if border[i]:
                side = np.sign(beta2 - beta1)
                b1_l = (x1 + side * TRACK_WIDTH * math.cos(beta1),
                        y1 + side * TRACK_WIDTH * math.sin(beta1))
                b1_r = (x1 + side * (TRACK_WIDTH + BORDER) * math.cos(beta1),
                        y1 + side * (TRACK_WIDTH + BORDER) * math.sin(beta1))
                b2_l = (x2 + side * TRACK_WIDTH * math.cos(beta2),
                        y2 + side * TRACK_WIDTH * math.sin(beta2))
                b2_r = (x2 + side * (TRACK_WIDTH + BORDER) * math.cos(beta2),
                        y2 + side * (TRACK_WIDTH + BORDER) * math.sin(beta2))
                self.road_poly.append(([b1_l, b1_r, b2_r,
                                        b2_l], (1, 1, 1) if i % 2 == 0 else
                                       (1, 0, 0)))

            self.random_obs += 1
            self.seed(self.random_obs)
            if (self.np_random.uniform(0, 1) < OBSTACLE_PROB) and (
                    last_obstacle <=
                    0):  # i > 15 pour que la course soit toujours faisable
                last_obstacle = 8

                deriv_left = self.np_random.uniform(TRACK_WIDTH)
                deriv_right = TRACK_WIDTH - deriv_left

                obs1_l = (x1 - (TRACK_WIDTH - deriv_left) * math.cos(beta1),
                          y1 - (TRACK_WIDTH - deriv_left) * math.sin(beta1))
                obs1_r = (x1 + (TRACK_WIDTH - deriv_right) * math.cos(beta1),
                          y1 + (TRACK_WIDTH - deriv_right) * math.sin(beta1))
                obs2_l = (x2 - (TRACK_WIDTH - deriv_left) * math.cos(beta2),
                          y2 - (TRACK_WIDTH - deriv_left) * math.sin(beta2))
                obs2_r = (x2 + (TRACK_WIDTH - deriv_right) * math.cos(beta2),
                          y2 + (TRACK_WIDTH - deriv_right) * math.sin(beta2))

                self.obstacles_positions.append(
                    (obs1_l, obs1_r, obs2_r, obs2_l))
                vertices = [obs1_l, obs1_r, obs2_r, obs2_l]
                obstacle = fixtureDef(shape=polygonShape(vertices=vertices))

                obstacle.userData = obstacle
                obstacle.color = [0.1568, 0.598, 0.2862, 0]

                self.road_poly.append(([obs1_l, obs1_r, obs2_r,
                                        obs2_l], obstacle.color))
            last_obstacle -= 1

        self.track = track
        return True
    def _create_track(self):
        CHECKPOINTS = 12

        # Create checkpoints
        checkpoints = []
        for c in range(CHECKPOINTS):
            alpha = 2 * math.pi * c / CHECKPOINTS + self.np_random.uniform(
                0, 2 * math.pi * 1 / CHECKPOINTS)
            rad = self.np_random.uniform(TRACK_RAD / 3, TRACK_RAD)
            if c == 0:
                alpha = 0
                rad = 1.5 * TRACK_RAD
            if c == CHECKPOINTS - 1:
                alpha = 2 * math.pi * c / CHECKPOINTS
                self.start_alpha = 2 * math.pi * (-0.5) / CHECKPOINTS
                rad = 1.5 * TRACK_RAD
            checkpoints.append(
                (alpha, rad * math.cos(alpha), rad * math.sin(alpha)))

        #print "\n".join(str(h) for h in checkpoints)
        # self.road_poly = [ (    # uncomment this to see checkpoints
        #    [ (tx,ty) for a,tx,ty in checkpoints ],
        #    (0.7,0.7,0.9) ) ]
        self.road = []

        # Go from one checkpoint to another to create track
        x, y, beta = 1.5 * TRACK_RAD, 0, 0
        dest_i = 0
        laps = 0
        track = []
        no_freeze = 2500
        visited_other_side = False
        while 1:
            alpha = math.atan2(y, x)
            if visited_other_side and alpha > 0:
                laps += 1
                visited_other_side = False
            if alpha < 0:
                visited_other_side = True
                alpha += 2 * math.pi
            while True:  # Find destination from checkpoints
                failed = True
                while True:
                    dest_alpha, dest_x, dest_y = checkpoints[dest_i %
                                                             len(checkpoints)]
                    if alpha <= dest_alpha:
                        failed = False
                        break
                    dest_i += 1
                    if dest_i % len(checkpoints) == 0: break
                if not failed: break
                alpha -= 2 * math.pi
                continue
            r1x = math.cos(beta)
            r1y = math.sin(beta)
            p1x = -r1y
            p1y = r1x
            dest_dx = dest_x - x  # vector towards destination
            dest_dy = dest_y - y
            proj = r1x * dest_dx + r1y * dest_dy  # destination vector projected on rad
            while beta - alpha > 1.5 * math.pi:
                beta -= 2 * math.pi
            while beta - alpha < -1.5 * math.pi:
                beta += 2 * math.pi
            prev_beta = beta
            proj *= SCALE
            if proj > 0.3: beta -= min(TRACK_TURN_RATE, abs(0.001 * proj))
            if proj < -0.3: beta += min(TRACK_TURN_RATE, abs(0.001 * proj))
            x += p1x * TRACK_DETAIL_STEP
            y += p1y * TRACK_DETAIL_STEP
            track.append((alpha, prev_beta * 0.5 + beta * 0.5, x, y))
            if laps > 4: break
            no_freeze -= 1
            if no_freeze == 0: break
        #print "\n".join([str(t) for t in enumerate(track)])

        # Find closed loop range i1..i2, first loop should be ignored, second is OK
        i1, i2 = -1, -1
        i = len(track)
        while True:
            i -= 1
            if i == 0: return False  # Failed
            pass_through_start = track[i][0] > self.start_alpha and track[
                i - 1][0] <= self.start_alpha
            if pass_through_start and i2 == -1:
                i2 = i
            elif pass_through_start and i1 == -1:
                i1 = i
                break
        print("Track generation: %i..%i -> %i-tiles track" % (i1, i2, i2 - i1))
        assert i1 != -1
        assert i2 != -1

        track = track[i1:i2 - 1]

        first_beta = track[0][1]
        first_perp_x = math.cos(first_beta)
        first_perp_y = math.sin(first_beta)
        # Length of perpendicular jump to put together head and tail
        well_glued_together = np.sqrt(
            np.square(first_perp_x * (track[0][2] - track[-1][2])) +
            np.square(first_perp_y * (track[0][3] - track[-1][3])))
        if well_glued_together > TRACK_DETAIL_STEP:
            return False

        # Red-white border on hard turns
        border = [False] * len(track)
        for i in range(len(track)):
            good = True
            oneside = 0
            for neg in range(BORDER_MIN_COUNT):
                beta1 = track[i - neg - 0][1]
                beta2 = track[i - neg - 1][1]
                good &= abs(beta1 - beta2) > TRACK_TURN_RATE * 0.2
                oneside += np.sign(beta1 - beta2)
            good &= abs(oneside) == BORDER_MIN_COUNT
            border[i] = good
        for i in range(len(track)):
            for neg in range(BORDER_MIN_COUNT):
                border[i - neg] |= border[i]

        # Create tiles
        for i in range(len(track)):
            alpha1, beta1, x1, y1 = track[i]
            alpha2, beta2, x2, y2 = track[i - 1]
            road1_l = (x1 - TRACK_WIDTH * math.cos(beta1),
                       y1 - TRACK_WIDTH * math.sin(beta1))
            road1_r = (x1 + TRACK_WIDTH * math.cos(beta1),
                       y1 + TRACK_WIDTH * math.sin(beta1))
            road2_l = (x2 - TRACK_WIDTH * math.cos(beta2),
                       y2 - TRACK_WIDTH * math.sin(beta2))
            road2_r = (x2 + TRACK_WIDTH * math.cos(beta2),
                       y2 + TRACK_WIDTH * math.sin(beta2))
            t = self.world.CreateStaticBody(fixtures=fixtureDef(
                shape=polygonShape(
                    vertices=[road1_l, road1_r, road2_r, road2_l])))
            t.userData = t
            c = 0.1 * (i % 3)
            t.color = [ROAD_COLOR[0] + c, ROAD_COLOR[1] + c, ROAD_COLOR[2] + c]
            t.road_visited = False
            t.road_friction = 1.0
            t.fixtures[0].sensor = True
            self.road_poly.append(([road1_l, road1_r, road2_r,
                                    road2_l], t.color))
            self.road.append(t)
            if border[i]:
                side = np.sign(beta2 - beta1)
                b1_l = (x1 + side * TRACK_WIDTH * math.cos(beta1),
                        y1 + side * TRACK_WIDTH * math.sin(beta1))
                b1_r = (x1 + side * (TRACK_WIDTH + BORDER) * math.cos(beta1),
                        y1 + side * (TRACK_WIDTH + BORDER) * math.sin(beta1))
                b2_l = (x2 + side * TRACK_WIDTH * math.cos(beta2),
                        y2 + side * TRACK_WIDTH * math.sin(beta2))
                b2_r = (x2 + side * (TRACK_WIDTH + BORDER) * math.cos(beta2),
                        y2 + side * (TRACK_WIDTH + BORDER) * math.sin(beta2))
                self.road_poly.append(([b1_l, b1_r, b2_r,
                                        b2_l], (1, 1, 1) if i % 2 == 0 else
                                       (1, 0, 0)))
        self.track = track
        return True
예제 #17
0
    def __init__(self, world, init_angle, init_x, init_y, userData, robot_id,
                 group, color):
        self.world = world
        self.hull = self.world.CreateDynamicBody(
            position=(init_x, init_y),
            angle=0,
            fixtures=[
                fixtureDef(
                    shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                                 for x, y in HULL_POLY]),
                    density=1.0,
                    restitution=1,
                    userData=userData,
                    friction=1),
            ])
        # self.hull.color = (0.8,0.0,0.0)
        self.hull.color = color
        self.hull.userData = userData
        self.wheels = []
        for wx, wy in WHEEL_POS:
            front_k = 1.0 if wy > 0 else 1.0
            w = self.world.CreateDynamicBody(
                position=(init_x + wx * SIZE, init_y + wy * SIZE),
                angle=0,
                fixtures=fixtureDef(
                    shape=polygonShape(vertices=[(x * front_k * SIZE,
                                                  y * front_k * SIZE)
                                                 for x, y in WHEEL_POLY]),
                    density=0.1,
                    restitution=1,
                    userData=userData + "_wheel",
                    friction=1))
            rjd = revoluteJointDef(
                bodyA=self.hull,
                bodyB=w,
                localAnchorA=(wx * SIZE, wy * SIZE),
                localAnchorB=(0, 0),
                enableMotor=False,
                enableLimit=True,
                lowerAngle=-0.0,
                upperAngle=+0.0,
            )
            w.joint = self.world.CreateJoint(rjd)
            w.color = WHEEL_COLOR
            w.userData = userData
            self.wheels.append(w)

        self.gun = self.world.CreateDynamicBody(
            position=(init_x, init_y),
            angle=0,
            fixtures=[
                fixtureDef(shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                                        for x, y in GUN_POLY]),
                           categoryBits=0x00,
                           density=1e-6,
                           userData=userData)
            ])
        self.gun_joint = self.world.CreateJoint(
            revoluteJointDef(
                bodyA=self.hull,
                bodyB=self.gun,
                localAnchorA=(0, 0),
                localAnchorB=(0, 0),
                enableMotor=True,
                enableLimit=False,
                maxMotorTorque=180 * 900 * SIZE * SIZE,
                motorSpeed=0.0,
                #lowerAngle = -math.pi,
                #upperAngle = +math.pi,
            ))
        self.gun.color = (0.1, 0.1, 0.1)
        self.gun.angle = init_angle

        self.hull.angle = init_angle * 1.04
        self.drawlist = self.wheels + [self.hull, self.gun]
        self.group = group
        self.robot_id = robot_id
        self.health = 1000.0
        self.buffLeftTime = 0
        self.command = {"ahead": 0, "rotate": 0, "transverse": 0}

        self.bullets_num = 40
        self.opportuniy_to_add_bullets = 2
    def draw(self, world, init_x, init_y, force_to_center):
        head = world.CreateDynamicBody(
            position=(init_x, init_y + self.BODY_HEIGHT / self.SCALE / 2 +
                      self.HEAD_HEIGHT / self.SCALE / 2 + 0.2),
            fixtures=fixtureDef(shape=polygonShape(
                vertices=[(x / self.SCALE, y / self.SCALE)
                          for x, y in [(-5, +10), (+5, +10), (+5,
                                                              -10), (-5,
                                                                     -10)]]),
                                density=5.0,
                                friction=0.1,
                                categoryBits=0x20,
                                maskBits=0x1))
        head.color1 = (0.5, 0.4, 0.9)
        head.color2 = (0.3, 0.3, 0.5)
        head.ApplyForceToCenter((force_to_center, 0), True)

        head.userData = CustomBodyUserData(True,
                                           is_contact_critical=True,
                                           name="head")
        self.body_parts.append(head)
        self.reference_head_object = head

        body = world.CreateDynamicBody(
            position=(init_x, init_y),
            fixtures=fixtureDef(
                shape=polygonShape(
                    vertices=[(x / self.SCALE, y / self.SCALE)
                              for x, y in [(-12, +25), (+12,
                                                        +25), (+8,
                                                               -20), (-8,
                                                                      -20)]]),
                density=5.0,
                friction=0.1,
                categoryBits=0x20,
                maskBits=0x1  # collide only with ground
            ))
        body.color1 = (0.5, 0.4, 0.9)
        body.color2 = (0.3, 0.3, 0.5)

        body.userData = CustomBodyUserData(True,
                                           is_contact_critical=True,
                                           name="body")
        self.body_parts.append(body)

        rjd = revoluteJointDef(
            bodyA=head,
            bodyB=body,
            anchor=(init_x, init_y + self.BODY_HEIGHT / self.SCALE / 2),
            enableMotor=False,
            enableLimit=True,
            lowerAngle=-0.1 * np.pi,
            upperAngle=0.1 * np.pi,
        )

        world.CreateJoint(rjd)

        UPPER_LIMB_FD = fixtureDef(shape=polygonShape(box=(self.LIMB_W / 2,
                                                           self.LIMB_H / 2)),
                                   density=1.0,
                                   restitution=0.0,
                                   categoryBits=0x20,
                                   maskBits=0x1)

        LOWER_LIMB_FD = fixtureDef(
            shape=polygonShape(box=(0.8 * self.LIMB_W / 2, self.LIMB_H / 2)),
            density=1.0,
            restitution=0.0,
            categoryBits=0x20,
            maskBits=0x1)

        HAND_PART_FD = fixtureDef(
            shape=polygonShape(box=(self.HAND_PART_W / 2,
                                    self.HAND_PART_H / 2)),
            density=1.0,
            restitution=0.0,
            categoryBits=0x20,
            maskBits=0x1)

        # ARMS
        for j in [-1, -1]:
            upper = world.CreateDynamicBody(
                position=(init_x, init_y + self.LIMB_H / 2 + self.ARM_UP),
                # angle=(i * 0.05),
                fixtures=UPPER_LIMB_FD)
            upper.color1 = (0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
            upper.color2 = (0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
            rjd = revoluteJointDef(
                bodyA=body,
                bodyB=upper,
                anchor=(init_x, init_y + self.ARM_UP),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=self.MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=-0.75 * 2 * np.pi,
                upperAngle=0,
            )

            upper.userData = CustomBodyUserData(False, name="upper_arm")
            self.body_parts.append(upper)

            joint_motor = world.CreateJoint(rjd)
            joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
            self.motors.append(joint_motor)

            lower = world.CreateDynamicBody(
                position=(init_x, init_y + self.LIMB_H * 3 / 2 + self.ARM_UP),
                # angle=(i * 0.05),
                fixtures=LOWER_LIMB_FD)
            lower.color1 = (0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
            lower.color2 = (0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
            rjd = revoluteJointDef(
                bodyA=upper,
                bodyB=lower,
                anchor=(init_x, init_y + self.LIMB_H + self.ARM_UP),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=self.MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=0,
                upperAngle=0.75 * np.pi,
            )

            lower.userData = CustomBodyUserData(False, name="lower_arm")
            self.body_parts.append(lower)

            joint_motor = world.CreateJoint(rjd)
            joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
            self.motors.append(joint_motor)

            # hand
            prev_part = lower
            initial_y = init_y + self.LIMB_H * 2 + self.ARM_UP
            angle_boundaries = [[-0.5, 0.5]]
            nb_hand_parts = 1
            for u in range(nb_hand_parts):
                hand_part = world.CreateDynamicBody(
                    position=(init_x, initial_y + self.HAND_PART_H / 2 +
                              self.HAND_PART_H * u),
                    fixtures=HAND_PART_FD)

                hand_part.color1 = (0.6 - j / 10., 0.3 - j / 10.,
                                    0.5 - j / 10.)
                hand_part.color2 = (0.4 - j / 10., 0.2 - j / 10.,
                                    0.3 - j / 10.)
                rjd = revoluteJointDef(
                    bodyA=prev_part,
                    bodyB=hand_part,
                    anchor=(init_x, initial_y + self.HAND_PART_H * u),
                    enableMotor=True,
                    enableLimit=True,
                    maxMotorTorque=self.MOTORS_TORQUE,
                    motorSpeed=1,
                    lowerAngle=angle_boundaries[u][0] * np.pi,
                    upperAngle=angle_boundaries[u][1] * np.pi,
                )

                hand_part.userData = CustomBodyUserData(True, name="hand")
                self.body_parts.append(hand_part)

                joint_motor = world.CreateJoint(rjd)
                joint_motor.userData = CustomMotorUserData(
                    SPEED_HAND, True, contact_body=hand_part)
                self.motors.append(joint_motor)

                prev_part = hand_part

            hand_sensor_position = (init_x, initial_y +
                                    self.HAND_PART_H * nb_hand_parts)
            hand_sensor_part = world.CreateDynamicBody(
                position=hand_sensor_position,
                fixtures=self.SENSOR_FD,
                userData=CustomBodySensorUserData(True, False, "hand_sensor"))
            hand_sensor_part.color1 = (
                1, 0, 0)  #(0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
            hand_sensor_part.color2 = (
                1, 0, 0)  #(0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)

            self.sensors.append(hand_sensor_part)
            world.CreateJoint(
                weldJointDef(bodyA=prev_part,
                             bodyB=hand_sensor_part,
                             anchor=hand_sensor_position))
예제 #19
0
def box2d_shapes_from_shapely(geometry):
    """Create Box2D shapes from shapely geometry"""
    return [b2.polygonShape(vertices=list(poly.exterior.coords))
            for poly in getattr(geometry, "geoms", [geometry])]
        pass

    def PreSolve(self, contact, oldManifold):
        pass

    def PostSolve(self, contact, impulse):
        pass


# construct world
world = world(gravity=(0, 0),
              doSleep=True,
              contactListener=myContactListener())

# add static body
env_shape = polygonShape(vertices=metadata)
# env_body = world.CreateStaticBody(position=(0,0), shapes=env_shape)

# add borders (for debugging)
border_shape_h = polygonShape(vertices=[(-0.75, 0), (-0.75,
                                                     0.01), (0.75,
                                                             0.01), (0.75, 0)])
border_shape_v = polygonShape(vertices=[(0, -0.6), (0.01, -0.6), (0.01,
                                                                  0.6), (0,
                                                                         0.6)])

# border_body1 = world.CreateStaticBody(position=(0,0.39), shapes=border_shape_h)

# border_body2 = world.CreateStaticBody(position=(0,-0.4), shapes=border_shape_h)

# border_body3 = world.CreateStaticBody(position=(0.59,0), shapes=border_shape_v)
예제 #21
0
SCREEN_WIDTH, SCREEN_HEIGHT=640,480

# --- pygame setup ---
screen=pygame.display.set_mode((SCREEN_WIDTH,SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption('Simple pygame example')
clock=pygame.time.Clock()


# --- pybox2d world setup ---
# Create the world
world=b2.world(gravity=(0,-0),doSleep=True)

# And a static body to hold the ground shape
ground_body=world.CreateStaticBody(
    position=(0,1),
    shapes=b2.polygonShape(box=(50,1)),
    )


# Create a dynamic body
box1=world.CreateDynamicBody(position=(10,3+5), angle=0.0)
box2=world.CreateDynamicBody(position=(14,5+5), angle=0.0)

rj=world.CreateRevoluteJoint(
    bodyA=box1, 
    bodyB=box2, 
    anchor=(12,4+5),
    lowerAngle = -.5 * b2.pi, # -90 degrees
    upperAngle = .5 * b2.pi, #  45 degrees
    enableLimit = True,
    maxMotorTorque = -200.0,
예제 #22
0
]
LEG_DOWN = -8 / SCALE
LEG_W, LEG_H = 8 / SCALE, 34 / SCALE

VIEWPORT_W = 600
VIEWPORT_H = 400

TERRAIN_STEP = 14 / SCALE
TERRAIN_LENGTH = 200     # in steps
TERRAIN_HEIGHT = VIEWPORT_H / SCALE / 4
TERRAIN_GRASS = 10    # low long are grass spots, in steps
TERRAIN_STARTPAD = 20    # in steps
FRICTION = 2.5

HULL_FD = fixtureDef(
    shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                 for x, y in HULL_POLY]),
    density=5.0,
    friction=0.1,
    categoryBits=0x0020,
    maskBits=0x001,  # collide only with ground
    restitution=0.0)  # 0.99 bouncy

LEG_FD = fixtureDef(
    shape=polygonShape(box=(LEG_W / 2, LEG_H / 2)),
    density=1.0,
    restitution=0.0,
    categoryBits=0x0020,
    maskBits=0x001)

LOWER_FD = fixtureDef(
    shape=polygonShape(box=(0.8 * LEG_W / 2, LEG_H / 2)),
PAD_RADIUS = 5

n_balls = -1
frames = 0

pygame.init()
font = pygame.font.SysFont("monospace", 15)

screen = pygame.display.set_mode((WIDTH, HEIGHT), 0, 32)
pygame.display.set_caption("SuperKoolt spel")
clock = pygame.time.Clock()

world = b2d.world(gravity=(0, -GRAVITY), doSleep=True)

world.CreateStaticBody(position=(0, 0),
                       shapes=b2d.polygonShape(box=(0.01, 100)))  #Left wall
world.CreateStaticBody(position=(float(WIDTH / PPM), 0),
                       shapes=b2d.polygonShape(box=(0.01, 100)))  #Right wall
world.CreateStaticBody(position=(0, float(HEIGHT / PPM)),
                       shapes=b2d.polygonShape(box=(100, 0.01)))  #Ceiling

pad_body = world.CreateKinematicBody(position=(WIDTH / (PPM * 2),
                                               0.3 - PAD_RADIUS),
                                     shapes=b2d.circleShape(radius=PAD_RADIUS))
pad_body.mass = 10000
pad_body.fixedRotation = True

pygame.key.set_repeat(10, 10)

balls = []
예제 #24
0
    def reset(self):
        if self.scores is None:
            self.scores = deque(maxlen=100)
        else:
            if self.achieve_goal:
                self.scores.append(1)
            else:
                self.scores.append(0)

        if self.verbose:
            self.verbosing()

        self.game_over = False
        self.prev_shaping = None
        self.achieve_goal = False
        self.strike_by_obstacle = False
        self.speed_table = np.zeros(len(OBSTACLE_POSITIONS))
        # timer
        self.energy = 1
        # clean up objects in the Box 2D world
        self._destroy()
        # create lidar objects
        self.lidar = [LidarCallback() for _ in range(self.num_beams)]
        self.world.contactListener_keepref = ContactDetector(self)
        self.world.contactListener = self.world.contactListener_keepref

        # create new world
        self.moon = self.world.CreateStaticBody(shapes=edgeShape(
            vertices=[(0, 0), (W, 0)]))
        p1 = (1, 1)
        p2 = (W - 1, 1)
        self.moon.CreateEdgeFixture(
            vertices=[p1, p2],
            density=100,
            friction=0,
            restitution=1.0,
        )
        self._build_wall()
        self.moon.color1 = (0.0, 0.0, 0.0)
        self.moon.color2 = (0.0, 0.0, 0.0)

        # create obstacles
        self._build_obstacles()

        # create controller object
        while True:
            drone_pos = (int(np.random.randint(1, 10)),
                         int(np.random.randint(1, 5)))
            self.drone = self.world.CreateDynamicBody(
                position=drone_pos,
                angle=0.0,
                fixtures=fixtureDef(
                    shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                                 for x, y in DRONE_POLY]),
                    density=5.0,
                    friction=0.1,
                    categoryBits=0x0010,
                    maskBits=0x003,  # collide all but obs range object
                    restitution=0.0)  # 0.99 bouncy
            )
            self.drone.color1 = (0.5, 0.4, 0.9)
            self.drone.color2 = (0.3, 0.3, 0.5)
            self.world.Step(1.0 / FPS, 6 * 30, 2 * 30)
            if self.game_over:
                self.world.DestroyBody(self.drone)
                self.game_over = False
            else:
                break
        # create goal
        goal_pos = GOAL_POS[np.random.randint(len(GOAL_POS))]
        self.goal = self.world.CreateStaticBody(
            position=goal_pos,
            angle=0.0,
            fixtures=fixtureDef(
                shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                             for x, y in DRONE_POLY]),
                density=5.0,
                friction=0.1,
                categoryBits=0x002,
                maskBits=0x0010,  # collide only with control device
                restitution=0.0)  # 0.99 bouncy
        )
        self.goal.color1 = (0., 0.5, 0)
        self.goal.color2 = (0., 0.5, 0)

        self.obs_range_plt = self.world.CreateKinematicBody(
            position=(self.drone.position[0], self.drone.position[1]),
            angle=0.0,
            fixtures=fixtureDef(
                shape=circleShape(radius=np.float64(self.max_obs_range),
                                  pos=(0, 0)),
                density=0,
                friction=0,
                categoryBits=0x0100,
                maskBits=0x000,  # collide with nothing
                restitution=0.3))
        self.obs_range_plt.color1 = (0.2, 0.2, 0.4)
        self.obs_range_plt.color2 = (0.6, 0.6, 0.6)
        self.drawlist = [self.obs_range_plt, self.drone, self.goal
                         ] + self.walls + self.obstacles
        self._observe_lidar(drone_pos)
        self.world.Step(1.0 / FPS, 6 * 30, 2 * 30)
        return np.copy(self.array_observation())
예제 #25
0
    def _reset(self):
        self._destroy()
        self.world.contactListener_bug_workaround = ContactDetector(self)
        self.world.contactListener = self.world.contactListener_bug_workaround
        self.game_over = False
        self.prev_shaping = None
        self.scroll = 0.0
        self.lidar_render = 0

        # Create world
        self._generate_terrain(hardcore=False)
        self._generate_clouds()

        # Create body
        init_x = TERRAIN_STEP * TERRAIN_STARTPAD / 2
        init_y = TERRAIN_HEIGHT + 2 * LEG_H
        self.hull = self.world.CreateDynamicBody(
            position=(init_x, init_y),
            fixtures=fixtureDef(
                shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                             for x, y in HULL_POLY]),
                density=1.0,
                friction=0.1,
                categoryBits=0x0020,
                maskBits=0x001,
                restitution=0.0))
        self.hull.color1 = (0.3, 0.4, 0.9)
        self.hull.color2 = (0.3, 0.4, 0.7)

        # Create legs
        self.legs = []
        self.joints = []
        for i in [-1, 1]:

            # Upper leg
            leg = self.world.CreateDynamicBody(
                position=(init_x, init_y - LEG_H / 2 - LEG_DOWN),
                angle=(i * 0.01),
                fixtures=fixtureDef(
                    shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                                 for x, y in LEG_POLY]),
                    density=1.0,
                    restitution=0.0,
                    categoryBits=0x0020,
                    maskBits=0x001))
            leg.color1 = (0.3 - i / 10, 0.4 - i / 10, 0.8 - i / 10)
            leg.color2 = (0.3, 0.4, 0.5)
            rjd = revoluteJointDef(
                bodyA=self.hull,
                bodyB=leg,
                localAnchorA=(0, LEG_DOWN),
                localAnchorB=(0, LEG_H / 2),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed=i,
                lowerAngle=-1.1,
                upperAngle=1.3,
            )
            self.legs.append(leg)
            self.joints.append(self.world.CreateJoint(rjd))

            # Lower leg
            lower = self.world.CreateDynamicBody(
                position=(init_x, init_y - LEG_H * 3 / 2 - LEG_DOWN),
                angle=(i * 0.01),
                fixtures=fixtureDef(
                    shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                                 for x, y in LOWERLEG_POLY]),
                    density=1.0,
                    restitution=0.00,
                    categoryBits=0x0020,
                    maskBits=0x001))
            lower.color1 = (0.3 - i / 10., 0.4 - i / 10., 0.8 - i / 10.)
            lower.color2 = (0.3 - i / 10., 0.4 - i / 10., 0.5 - i / 10.)
            rjd = revoluteJointDef(
                bodyA=leg,
                bodyB=lower,
                localAnchorA=(0, -LEG_H / 2),
                localAnchorB=(0, LEG_H / 2),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=-2.8,
                upperAngle=0.2,
            )
            lower.ground_contact = False
            self.legs.append(lower)
            self.joints.append(self.world.CreateJoint(rjd))

            # Feet
            foot = self.world.CreateDynamicBody(
                position=(init_x, init_y),
                angle=0,
                fixtures=fixtureDef(shape=polygonShape(box=(LEG_H, LEG_W)),
                                    density=1.0,
                                    restitution=0.05,
                                    categoryBits=0x0020,
                                    maskBits=0x001))
            foot.color1 = (0.3 - i / 10., 0.3 - i / 10., 0.8 - i / 10.)
            foot.color2 = (0.3 - i / 10., 0.3 - i / 10., 0.8 - i / 10.)
            rjd = revoluteJointDef(
                bodyA=lower,
                bodyB=foot,
                localAnchorA=(0, -LEG_H / 2),
                localAnchorB=(-0.05, 0),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=-1.0,
                upperAngle=1.0,
            )
            foot.ground_contact = False
            self.legs.append(foot)
            self.joints.append(self.world.CreateJoint(rjd))

        # We render this
        self.drawlist = self.terrain + self.legs + [self.hull]

        # Lidar
        class LidarCallback(Box2D.b2.rayCastCallback):
            def ReportFixture(self, fixture, point, normal, fraction):
                if (fixture.filterData.categoryBits & 1) == 0:
                    return 1
                self.p2 = point
                self.fraction = fraction
                return 0

        self.lidar = [LidarCallback() for _ in range(10)]

        # Start
        return self._step(np.array([0, 0, 0, 0, 0, 0]))[0]
    def _reset(self):
        self.trajectory = []
        self.actions = []
        self.curr_step = 0
        self.curr_obs = None
        self.fan = False
        self.fan_confs = []
        self.at_site = [False for _ in range(LANDED_SLACK)]
        self.grounded = [False for _ in range(LANDED_SLACK)]

        self._destroy()
        self.world.contactListener_keepref = ContactDetector(self)
        self.world.contactListener = self.world.contactListener_keepref
        self.game_over = False
        self.prev_shaping = None

        W = VIEWPORT_W / SCALE
        H = VIEWPORT_H / SCALE

        # terrain
        CHUNKS = 11
        height = self.np_random.uniform(0, H / 2, size=(CHUNKS + 1, ))
        chunk_x = [W / (CHUNKS - 1) * i for i in range(CHUNKS)]

        #helipad_chunk = CHUNKS // 2
        # randomize helipad x coord
        helipad_chunk = self.goal if self.goal is not None else np.random.choice(
            range(1, CHUNKS - 1))

        self.helipad_x1 = chunk_x[helipad_chunk - 1]
        self.helipad_x2 = chunk_x[helipad_chunk + 1]
        self.helipad_y = H / 4
        height[helipad_chunk - 2] = self.helipad_y
        height[helipad_chunk - 1] = self.helipad_y
        height[helipad_chunk + 0] = self.helipad_y
        height[helipad_chunk + 1] = self.helipad_y
        height[helipad_chunk + 2] = self.helipad_y
        smooth_y = [
            0.33 * (height[i - 1] + height[i + 0] + height[i + 1])
            for i in range(CHUNKS)
        ]

        self.moon = self.world.CreateStaticBody(shapes=edgeShape(
            vertices=[(0, 0), (W, 0)]))
        self.sky_polys = []
        for i in range(CHUNKS - 1):
            p1 = (chunk_x[i], smooth_y[i])
            p2 = (chunk_x[i + 1], smooth_y[i + 1])
            self.moon.CreateEdgeFixture(vertices=[p1, p2],
                                        density=0,
                                        friction=0.1)
            self.sky_polys.append([p1, p2, (p2[0], H), (p1[0], H)])

        self.moon.color1 = (0.0, 0.0, 0.0)
        self.moon.color2 = (0.0, 0.0, 0.0)

        initial_y = VIEWPORT_H / SCALE  #*0.75
        self.lander = self.world.CreateDynamicBody(
            position=(VIEWPORT_W / SCALE / 2, initial_y),
            angle=0.0,
            fixtures=fixtureDef(
                shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                             for x, y in LANDER_POLY]),
                density=5.0,
                friction=0.1,
                categoryBits=0x0010,
                maskBits=0x001,  # collide only with ground
                restitution=0.0)  # 0.99 bouncy
        )
        self.lander.color1 = (0.5, 0.4, 0.9)
        self.lander.color2 = (0.3, 0.3, 0.5)
        self.lander.ApplyForceToCenter(
            (self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM),
             self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM)), True)

        self.legs = []
        for i in [-1, +1]:
            leg = self.world.CreateDynamicBody(
                position=(VIEWPORT_W / SCALE / 2 - i * LEG_AWAY / SCALE,
                          initial_y),
                angle=(i * 0.05),
                fixtures=fixtureDef(shape=polygonShape(box=(LEG_W / SCALE,
                                                            LEG_H / SCALE)),
                                    density=1.0,
                                    restitution=0.0,
                                    categoryBits=0x0020,
                                    maskBits=0x001))
            leg.ground_contact = False
            leg.color1 = (0.5, 0.4, 0.9)
            leg.color2 = (0.3, 0.3, 0.5)
            rjd = revoluteJointDef(
                bodyA=self.lander,
                bodyB=leg,
                localAnchorA=(0, 0),
                localAnchorB=(i * LEG_AWAY / SCALE, LEG_DOWN / SCALE),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=LEG_SPRING_TORQUE,
                motorSpeed=+0.3 * i  # low enough not to jump back into the sky
            )
            if i == -1:
                rjd.lowerAngle = +0.9 - 0.5  # Yes, the most esoteric numbers here, angles legs have freedom to travel within
                rjd.upperAngle = +0.9
            else:
                rjd.lowerAngle = -0.9
                rjd.upperAngle = -0.9 + 0.5
            leg.joint = self.world.CreateJoint(rjd)
            self.legs.append(leg)

        self.drawlist = [self.lander] + self.legs

        return self._step(np.array([0, 0]) if self.continuous else 0)[0]
예제 #27
0
    def _reset(self):
        self._destroy()
        self.world.contactListener = ContactDetector(self)
        self.game_over = False
        self.prev_shaping = None
        self.scroll = 0.0
        self.lidar_render = 0

        W = VIEWPORT_W/SCALE
        H = VIEWPORT_H/SCALE

        self._generate_terrain(self.hardcore)
        self._generate_clouds()

        init_x = TERRAIN_STEP*TERRAIN_STARTPAD/2
        init_y = TERRAIN_HEIGHT+2*LEG_H
        self.hull = self.world.CreateDynamicBody(
            position = (init_x, init_y),
            fixtures = fixtureDef(
                shape=polygonShape(vertices=[ (x/SCALE,y/SCALE) for x,y in HULL_POLY ]),
                density=5.0,
                friction=0.1,
                categoryBits=0x0020,
                maskBits=0x001,  # collide only with ground
                restitution=0.0) # 0.99 bouncy
                )
        self.hull.color1 = (0.5,0.4,0.9)
        self.hull.color2 = (0.3,0.3,0.5)
        self.hull.ApplyForceToCenter((np.random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)

        self.legs = []
        self.joints = []
        for i in [-1,+1]:
            leg = self.world.CreateDynamicBody(
                position = (init_x, init_y - LEG_H/2 - LEG_DOWN),
                angle = (i*0.05),
                fixtures = fixtureDef(
                    shape=polygonShape(box=(LEG_W/2, LEG_H/2)),
                    density=1.0,
                    restitution=0.0,
                    categoryBits=0x0020,
                    maskBits=0x001)
                )
            leg.color1 = (0.6-i/10., 0.3-i/10., 0.5-i/10.)
            leg.color2 = (0.4-i/10., 0.2-i/10., 0.3-i/10.)
            rjd = revoluteJointDef(
                bodyA=self.hull,
                bodyB=leg,
                localAnchorA=(0, LEG_DOWN),
                localAnchorB=(0, LEG_H/2),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed = i,
                lowerAngle = -0.8,
                upperAngle = 1.1,
                )
            self.legs.append(leg)
            self.joints.append(self.world.CreateJoint(rjd))

            lower = self.world.CreateDynamicBody(
                position = (init_x, init_y - LEG_H*3/2 - LEG_DOWN),
                angle = (i*0.05),
                fixtures = fixtureDef(
                    shape=polygonShape(box=(0.8*LEG_W/2, LEG_H/2)),
                    density=1.0,
                    restitution=0.0,
                    categoryBits=0x0020,
                    maskBits=0x001)
                )
            lower.color1 = (0.6-i/10., 0.3-i/10., 0.5-i/10.)
            lower.color2 = (0.4-i/10., 0.2-i/10., 0.3-i/10.)
            rjd = revoluteJointDef(
                bodyA=leg,
                bodyB=lower,
                localAnchorA=(0, -LEG_H/2),
                localAnchorB=(0, LEG_H/2),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed = 1,
                lowerAngle = -1.6,
                upperAngle = -0.1,
                )
            lower.ground_contact = False
            self.legs.append(lower)
            self.joints.append(self.world.CreateJoint(rjd))

        self.drawlist = self.terrain + self.legs + [self.hull]

        class LidarCallback(Box2D.b2.rayCastCallback):
            def ReportFixture(self, fixture, point, normal, fraction):
                if (fixture.filterData.categoryBits & 1) == 0:
                    return 1
                self.p2 = point
                self.fraction = fraction
                return 0
        self.lidar = [LidarCallback() for _ in range(10)]

        return self._step(np.array([0,0,0,0]))[0]
예제 #28
0
    def _create_track(self):

        # Offset parameters to get the track correctly on the playfield
        self.off_params = (-WINDOW_H//4, -WINDOW_W//2.5)

        # Construct a lattice using a random node initialization followed
        # by a random edge deletion, ensuring everything is connected and
        # lattice constraints are met
        if "lattice" not in self.__dict__:
            self.lattice = construct_lattice(GRID_ROWS, GRID_COLS, PROB_EDGE,
                LATTICE_CONSTRAINTS, RANDOM_DELETIONS, self.pre_provided_lattice, self.delete_edges)

        # Create polygons for the lattice road pieces and lane separators
        # Also store directions for each of the road piece
        if "track" not in self.__dict__:
            self.normal_polygons, self.special_polygons, self.ls_polygons, self.directions, \
            self.relevant_nodes, self.special_relevant_nodes = \
                construct_grid(self.lattice, LANE_WIDTH, EDGE_WIDTH, self.off_params, LANE_SEP)
            self.track = self.normal_polygons+self.special_polygons
            self.directions += "n"*len(self.special_polygons) # Each special_polygon is junction
            self.relevant_nodes += self.special_relevant_nodes
            assert(len(self.track) == len(self.directions) == len(self.relevant_nodes))

        # Start with a blank list of road objects
        self.road = []

        # Draw track with varying shade so road pieces can be differentiated
        i = 0
        for polygon in self.track:

            # Create static body
            t = self.world.CreateStaticBody(fixtures = fixtureDef(
                shape=polygonShape(vertices=list(polygon))
                ))
            t.userData = t

            # Assign direction
            t.direction = self.directions[i]

            # Assign colors
            # USEFUL: Change colors when debugging directions!
            c = 0.01*(i%3)
            i += 1
            t.color = [ROAD_COLOR[0] + c, ROAD_COLOR[1] + c, ROAD_COLOR[2] + c]
            
            # Assign properties and set contact true
            t.boxtype = "road"
            t.road_friction = 1.0
            t.fixtures[0].sensor = True

            # Add to list for rendering, and keep track by adding to self.road
            self.road_poly.append(( list(polygon), t.color ))
            self.road.append(t)

        # Which vertices are in the lattice?
        h, w = self.lattice.shape[:2]
        self.which_points = []
        self.neighbors = []
        for i in range(h):
            for j in range(w):
                if self.lattice[i, j, 0]:
                    self.which_points += [(i, j)]
                    self.neighbors += [self.lattice[i, j, 1:]]

        # Construct traffic lights
        self.lights = []
        for neighbor in self.neighbors:
            self.lights += [construct_traffic_lights(neighbor, LANE_WIDTH, TRAFFIC_LIGHT_R, TRAFFIC_LIGHT_R2)]

        return True
예제 #29
0
# --- pybox2d world setup ---
# Create the world
world = b2.world(gravity=(0, -10), doSleep=True)

# --- pybox2d static body setup ---
bodyDef = Box2D.b2BodyDef()
bodyDef.position = (22.5, 12)  # the center of object
body = world.CreateBody(
    bodyDef)  # The default bodies are static. # The default density is zero.
groundBody = Box2D.b2PolygonShape(box=(1, 7))
groundBoxFixture = Box2D.b2FixtureDef(shape=groundBody)
body.CreateFixture(groundBoxFixture)
# above is equal in this:
ground_body = world.CreateStaticBody(
    position=(3, 1),
    shapes=b2.polygonShape(box=(10, 1)),
)
# circle shape
body1 = world.CreateStaticBody(position=(30, 12))
circle = body1.CreateCircleFixture(radius=0.1, density=1, friction=0.3)

# other shape:create by offering vertices
body = world.CreateStaticBody(position=(30, 12), angle=math.pi * 90 / 180)
box = body.CreatePolygonFixture(vertices=[(1, 1), (2, 3), (1, 0), (0, 2)],
                                density=1,
                                friction=0.3,
                                restitution=0.9)

# --- pygame draw function setup ---
colors = {
    b2.staticBody: (255, 255, 255),  # white
예제 #30
0
def box2d_triangles_from_shapely(geometry):
    """Create Box2D shapes by triangulating shapely geometry"""
    for face in triangulate(geometry):
        yield b2.polygonShape(vertices=list(face.exterior.coords))
예제 #31
0
    def _reset(self):
        self._destroy()

        init_x = self.init_x
        init_y = self.init_y
        self.hull = self.world.CreateDynamicBody(
            position=(init_x, init_y),
            fixtures=fixtureDef(
                shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                             for x, y in HULL_POLY]),
                density=5.0,
                friction=0.1,
                categoryBits=0x002,
                # maskBits=(0x001 & 0x002),  # collide only with ground
                restitution=0.0)  # 0.99 bouncy
        )
        self.hull.color1 = (0.5, 0.4, 0.9)
        self.hull.color2 = (0.3, 0.3, 0.5)
        self.hull.ApplyForceToCenter(
            (self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)

        self.legs = []
        self.joints = []
        for i in [-1, +1]:
            leg = self.world.CreateDynamicBody(
                position=(init_x, init_y - LEG_H / 2 - LEG_DOWN),
                angle=(i * 0.05),
                fixtures=fixtureDef(shape=polygonShape(box=(LEG_W / 2,
                                                            LEG_H / 2)),
                                    density=1.0,
                                    restitution=0.0,
                                    categoryBits=0x002,
                                    maskBits=0x001)  # collide with ground only
            )
            leg.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
            leg.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
            rjd = revoluteJointDef(
                bodyA=self.hull,
                bodyB=leg,
                localAnchorA=(0, LEG_DOWN),
                localAnchorB=(0, LEG_H / 2),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed=i,
                lowerAngle=-0.8,
                upperAngle=1.1,
            )
            self.legs.append(leg)
            self.joints.append(self.world.CreateJoint(rjd))

            lower = self.world.CreateDynamicBody(
                position=(init_x, init_y - LEG_H * 3 / 2 - LEG_DOWN),
                angle=(i * 0.05),
                fixtures=fixtureDef(shape=polygonShape(box=(0.8 * LEG_W / 2,
                                                            LEG_H / 2)),
                                    density=1.0,
                                    restitution=0.0,
                                    categoryBits=0x0020,
                                    maskBits=0x001))
            lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
            lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
            rjd = revoluteJointDef(
                bodyA=leg,
                bodyB=lower,
                localAnchorA=(0, -LEG_H / 2),
                localAnchorB=(0, LEG_H / 2),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=-1.6,
                upperAngle=-0.1,
            )
            lower.ground_contact = False
            self.legs.append(lower)
            self.joints.append(self.world.CreateJoint(rjd))

        self.drawlist = self.legs + [self.hull]

        class LidarCallback(Box2D.b2.rayCastCallback):
            def ReportFixture(self, fixture, point, normal, fraction):
                if (fixture.filterData.categoryBits & 1) == 0:
                    return 1
                self.p2 = point
                self.fraction = fraction
                return 0

        self.lidar = [LidarCallback() for _ in range(10)]
예제 #32
0
파일: path_02.py 프로젝트: sc15zs/project
class World():
    def create_world_grid():
        ##---Create dictionary to store world cells---##
        my_world = {}
        key = 0
        for x in range(600):
            my_world[key] = [-100, -100]
            key += 1
        ##---Store x and y coordinates for each cell---##
        key = 0
        for y in xrange(1, 40, 2):
            for x in xrange(1, 60, 2):
                my_world[key] = [x, y]
                key += 1
        return my_world

    def create_graph():
        ##---BUILD A FULLY CONNECTED GRAPH OF THE WORLD: AT THE BEGINNING EVERY CELL IS---##
        ##---CONSIDERED TO BE FREE---##
        my_graph = {}
        for key in range(600):
            my_graph[key] = {
                key + 30: 1,
                key + 31: 1.4,
                key + 1: 1,
                key - 29: 1.4,
                key - 30: 1,
                key - 31: 1.4,
                key - 1: 1,
                key + 29: 1.4
            }

        ##---Define list of cells for each side (corner cells not included!)---##
        left_side = [
            30, 60, 90, 120, 150, 180, 210, 240, 270, 300, 330, 360, 390, 420,
            450, 480, 510, 540
        ]
        right_side = [
            59, 89, 119, 149, 179, 209, 239, 269, 299, 329, 359, 389, 419, 449,
            479, 509, 539, 569
        ]
        bottom_side = [
            1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19,
            20, 21, 22, 23, 24, 25, 26, 27, 28
        ]
        top_side = [
            571, 572, 573, 574, 575, 576, 577, 578, 579, 580, 581, 582, 583,
            584, 585, 586, 587, 588, 589, 590, 591, 592, 593, 594, 595, 596,
            597, 598
        ]
        for key in range(600):
            ##---Set connected cells of side cells (corner cells not included)---##
            if (key in left_side):
                my_graph[key] = {
                    key + 30: 1,
                    key + 31: 1.4,
                    key + 1: 1,
                    key - 29: 1.4,
                    key - 30: 1
                }
            if (key in right_side):
                my_graph[key] = {
                    key + 30: 1,
                    key - 30: 1,
                    key - 31: 1.4,
                    key - 1: 1,
                    key + 29: 1.4
                }
            if (key in bottom_side):
                my_graph[key] = {
                    key + 30: 1,
                    key + 31: 1.4,
                    key + 1: 1,
                    key - 1: 1,
                    key + 29: 1.4
                }
            if (key in top_side):
                my_graph[key] = {
                    key + 1: 1,
                    key - 29: 1.4,
                    key - 30: 1,
                    key - 31: 1.4,
                    key - 1: 1
                }
            ##---Set connections of corner cells---##
            if (key == 0):
                my_graph[key] = {key + 30: 1, key + 31: 1.4, key + 1: 1}
            if (key == 29):
                my_graph[key] = {key + 30: 1, key - 1: 1, key + 29: 1.4}
            if (key == 570):
                my_graph[key] = {key + 1: 1, key - 29: 1.4, key - 30: 1}
            if (key == 599):
                my_graph[key] = {key - 30: 1, key - 31: 1.4, key - 1: 1}
        return my_graph

    ##---Constants---##
    PPM = 20.0  # pixels per meter
    TARGET_FPS = 60
    TIME_STEP = 1.0 / TARGET_FPS
    vel_iters, pos_iters = 6, 2
    SCREEN_WIDTH, SCREEN_HEIGHT = 1200, 800

    ##---pygame setup---##
    screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
    pygame.display.set_caption('Path finding: robot body of any shape')
    clock = pygame.time.Clock()
    start_time = datetime.now()

    ##---Create the world---##
    my_world = world(gravity=(0, 0), doSleep=True)

    # ##---Static obstacles of the environment---##
    # wall_bottom = my_world.CreateStaticBody(position=(30, 0.1), shapes=polygonShape(box=(30, 0.1)), )
    # wall_top = my_world.CreateStaticBody(position=(30, 40), shapes=polygonShape(box=(30, 0.1)), )
    # wall_left = my_world.CreateStaticBody(position=(0, 20), shapes=polygonShape(box=(20, 0.1)), angle=math.pi/2)
    # wall_right = my_world.CreateStaticBody(position=(60, 20), shapes=polygonShape(box=(20, 0.1)), angle=math.pi/2)
    # wall_bottom.userData = {'color': 'obstacle'}
    # wall_top.userData = {'color': 'obstacle'}
    # wall_left.userData = {'color': 'obstacle'}
    # wall_right.userData = {'color': 'obstacle'}

    obstacle_1 = my_world.CreateStaticBody(position=(5, 12.5),
                                           shapes=polygonShape(box=(5, 2.5)))
    obstacle_2 = my_world.CreateStaticBody(position=(40, 25),
                                           shapes=polygonShape(box=(6, 2)),
                                           angle=math.pi / 4)
    obstacle_3 = my_world.CreateStaticBody(position=(30, 25),
                                           shapes=polygonShape(box=(6, 2)),
                                           angle=-math.pi / 4)
    obstacle_1.userData = {'color': 'obstacle'}
    obstacle_2.userData = {'color': 'obstacle'}
    obstacle_3.userData = {'color': 'obstacle'}

    # ##---TO BE USED TO TEST AND COMPARE DIJKSTRA'S AND A* ALGORITHMS---##
    # obstacle_1 = my_world.CreateStaticBody(position=(12, 12), shapes=polygonShape(box=(8, 0.5)), angle=-math.pi/4)
    # obstacle_1.userData = {'color': 'obstacle'}
    # obstacle_2 = my_world.CreateStaticBody(position=(24, 8), shapes=polygonShape(box=(8, 0.5)), angle=-math.pi/4)
    # obstacle_2.userData = {'color': 'obstacle'}
    # obstacle_3 = my_world.CreateStaticBody(position=(18, 20), shapes=polygonShape(box=(8, 0.5)), angle=-math.pi/4)
    # obstacle_3.userData = {'color': 'obstacle'}
    # obstacle_4 = my_world.CreateStaticBody(position=(22, 31), shapes=polygonShape(box=(14, 1.0)), angle=-math.pi/4)
    # obstacle_4.userData = {'color': 'obstacle'}
    # obstacle_5 = my_world.CreateStaticBody(position=(37, 25), shapes=polygonShape(box=(12, 1.0)), angle=-math.pi/2)
    # obstacle_5.userData = {'color': 'obstacle'}
    # obstacle_6 = my_world.CreateStaticBody(position=(44, 14), shapes=polygonShape(box=(6, 3.0)), angle=-math.pi/4)
    # obstacle_6.userData = {'color': 'obstacle'}
    # obstacle_7 = my_world.CreateStaticBody(position=(54, 26), shapes=polygonShape(box=(10, 1.0)))
    # obstacle_7.userData = {'color': 'obstacle'}

    ##---Data structures---##
    my_grid = create_world_grid()
    my_graph = create_graph()

    ##---Check if cells are free or occupied---##
    key = 0
    occupied_counter = 0
    occupied_list = []
    free_cell_list = []
    for y in xrange(1, 40, 2):
        for x in xrange(1, 60, 2):
            start_position = (x, y)
            ##---Create robot---##
            body = my_world.CreateDynamicBody(position=start_position)
            box = body.CreatePolygonFixture(box=(0.98, 1.98),
                                            density=1,
                                            friction=0.3)
            free_cell_list.append(key)
            for i in range(3):
                ##---Instruct the world to perform a single step of simulation---##
                my_world.Step(TIME_STEP, vel_iters, pos_iters)
            ##---If the position of the robot changes, the cell must be occupied---##
            if (start_position != body.position):
                occupied_counter += 1
                occupied_list.append(key)
                ##---Add some fictional high values for occupied cell x and y coordinates---##
                my_grid[key] = [1000, 1000]
                del my_graph[key]
                ##---Destroy robot body---##
                free_cell_list.remove(key)

            my_world.DestroyBody(body)
            key += 1

    print("occupied_counter: %d" % occupied_counter)

    ##---count number of cells in the graph---##
    counter = 0
    for key, value in my_graph.items():
        counter += 1
    print("Number of free cells in graph: %d" % counter)
    print("occupied cells: %s" % occupied_list)

    ##---Count edges in graph so far---##
    edge_count = 0
    for key, value in my_graph.items():
        for x in value:
            edge_count += 1
    print("edgecount: %d" % edge_count)

    ##---Delete edges which are connected to occupied cells---##
    for number in occupied_list:
        for key, value in my_graph.items():
            for x, y in value.items():
                if (x == number):
                    del value[x]

    ##---Delete diagonal edges at the corners of occupied cells---##
    ##---Those edges which connect two free cell diagonally but there is an occupied cells next to them---##
    for number in occupied_list:
        ##---Delete edges at the top-right corner of the occupied cell---##
        if (number + 1 in my_graph.keys() and number + 30 in my_graph.keys()):
            if (number + 30 in my_graph[number + 1].keys()):
                del my_graph[number + 1][number + 30]
            if (number + 1 in my_graph[number + 30].keys()):
                del my_graph[number + 30][number + 1]
        ##---Delete edges at the top-left corner of the occupied cell---##
        if (number - 1 in my_graph.keys() and number + 30 in my_graph.keys()):
            if (number + 30 in my_graph[number - 1].keys()):
                del my_graph[number - 1][number + 30]
            if (number - 1 in my_graph[number + 30].keys()):
                del my_graph[number + 30][number - 1]
        ##---Delete edges at the bottom-left corner of the occupied cell---##
        if (number - 1 in my_graph.keys() and number - 30 in my_graph.keys()):
            if (number - 30 in my_graph[number - 1].keys()):
                del my_graph[number - 1][number - 30]
            if (number - 1 in my_graph[number - 30].keys()):
                del my_graph[number - 30][number - 1]
        ##---Delete edges at the bottom-right corner of the occupied cell---##
        if (number + 1 in my_graph.keys() and number - 30 in my_graph.keys()):
            if (number - 30 in my_graph[number + 1].keys()):
                del my_graph[number + 1][number - 30]
            if (number + 1 in my_graph[number - 30].keys()):
                del my_graph[number - 30][number + 1]

    ##---Turn the robot counterclockwise and check for collision at every 9 degrees---##
    start_key = 0
    for y in xrange(1, 40, 2):
        for x in xrange(1, 60, 2):
            start_position3 = (x, y)
            ##---40 iterations, 9 degrees/iteration = 360 degrees checked---##
            for i in xrange(0, 39, 1):
                body3 = my_world.CreateDynamicBody(
                    position=start_position3,
                    angle=i * math.pi / 20)  ##Step size: 9 degrees (180/20)
                box = body3.CreatePolygonFixture(box=(0.98, 1.98),
                                                 density=1,
                                                 friction=0.3)
                for j in range(3):
                    ##---Instruct the world to perform a single step of simulation---##
                    my_world.Step(TIME_STEP, vel_iters, pos_iters)
                ##---If the robot body was in collision with any object of the environment---##
                if (start_position3 != body3.position):
                    ##---Check how much the robot was rotated when the collision happened---##
                    ##---And delete all the edges which come after this rotation---##
                    if (i < 35):  # 35*9=315 degrees
                        for key, value in my_graph.items():
                            if (key == start_key):
                                for inner_key, inner_value in value.items():
                                    if (key + 31 == inner_key):
                                        del value[key + 31]
                    if (i < 30):  # 30*9=270 degrees
                        for key, value in my_graph.items():
                            if (key == start_key):
                                for inner_key, inner_value in value.items():
                                    if (key + 1 == inner_key):
                                        del value[key + 1]
                    if (i < 25):  # 25*9=225 degrees
                        for key, value in my_graph.items():
                            if (key == start_key):
                                for inner_key, inner_value in value.items():
                                    if (key - 29 == inner_key):
                                        del value[key - 29]
                    if (i < 20):  # 20*9=180 degrees
                        for key, value in my_graph.items():
                            if (key == start_key):
                                for inner_key, inner_value in value.items():
                                    if (key - 30 == inner_key):
                                        del value[key - 30]
                    if (i < 15):  # 15*9=135 degrees
                        for key, value in my_graph.items():
                            if (key == start_key):
                                for inner_key, inner_value in value.items():
                                    if (key - 31 == inner_key):
                                        del value[key - 31]
                    if (i < 10):  # 10*9=00 degrees
                        for key, value in my_graph.items():
                            if (key == start_key):
                                for inner_key, inner_value in value.items():
                                    if (key - 1 == inner_key):
                                        del value[key - 1]
                    if (i < 5):  # 5*9=45 degrees
                        for key, value in my_graph.items():
                            if (key == start_key):
                                for inner_key, inner_value in value.items():
                                    if (key + 29 == inner_key):
                                        del value[key + 29]
                my_world.DestroyBody(body3)
            start_key += 1

    ##---count edges again---##
    new_edge_count = 0
    for key, value in my_graph.items():
        for x in value:
            new_edge_count += 1
    print("new edgecount: %d" % new_edge_count)

    ##---Set start and goal node numbers---##
    start_number = 31
    goal_number = 437

    ##---Use Dijkstra's algorithm to find shortest path---##
    start_time_dijkstra = datetime.now()
    shortest_path_1, cost_1, counter_1 = dijkstra(my_graph, start_number,
                                                  goal_number, 0)
    shortest_path_1.reverse()
    print("Shortest path using Dijkstra's algorithm: ")
    print(shortest_path_1)
    print("cost: %f" % cost_1)
    print("Number of nodes expanded: %d" % counter_1)
    end_time_dijkstra = datetime.now()
    time_needed_dijkstra = end_time_dijkstra - start_time_dijkstra
    print("Time needed for Dijkstra's algorithm: %s s." %
          str(time_needed_dijkstra))

    ##---Use A* algorithm to find shortest path---##
    start_time_A_star = datetime.now()
    shortest_path_2, cost_2, counter_2 = A_star(my_grid, my_graph,
                                                start_number, goal_number, 0)
    shortest_path_2.reverse()
    print("Shortest path using A* algorithm: ")
    print(shortest_path_2)
    print("cost: %f" % cost_2)
    print("Number of nodes expanded: %d" % counter_2)
    end_time_A_star = datetime.now()
    time_needed_A_star = end_time_A_star - start_time_A_star
    print("Time needed for A* algorithm: %s s." % str(time_needed_A_star))

    ##---Create static bodies to mark the nodes in the tree with small "dots"---##
    for key, value in my_grid.items():
        x = value[0]
        y = value[1]
        body3 = my_world.CreateStaticBody(position=(x, y))
        box3 = body3.CreatePolygonFixture(box=(0.098, 0.098), density=1)
        box3.sensor = True
        body3.userData = {'color': 'node_marker'}

    ##---Draw small 'dots' along the shortest path---##
    for number in shortest_path_1:  ##---To be used with Dijkstra's algorithm---##
        #for number in shortest_path_2:          ##---To be used with the A* algorithm---##
        x = my_grid[number][0]
        y = my_grid[number][1]
        body = my_world.CreateKinematicBody(position=(x, y))
        box = body.CreatePolygonFixture(box=(0.098, 0.098),
                                        density=1,
                                        friction=0.3)
        box.sensor = True
        body.userData = {'color': 'shortest_path'}

    ##---Define vertices for start and goal positions and for the robot---##
    vertices2 = [(-0.5, -0.2), (-0.2, -0.5), (0.2, -0.5), (0.5, -0.2),
                 (0.5, 0.2), (0.2, 0.5), (-0.2, 0.5), (-0.5, 0.2)]
    ##---Create static body to mark the start position---##
    body1 = my_world.CreateStaticBody(position=(my_grid[start_number][0],
                                                my_grid[start_number][1]))
    box1 = body1.CreatePolygonFixture(vertices=vertices2, density=1)
    box1.sensor = True
    body1.userData = {'color': 'start_position'}

    ##---Create static body to mark the goal position---##
    body2 = my_world.CreateStaticBody(position=(my_grid[goal_number][0],
                                                my_grid[goal_number][1]))
    box2 = body2.CreatePolygonFixture(vertices=vertices2, density=1)
    box2.sensor = True
    body2.userData = {'color': 'goal_position'}

    # Create a dynamic body for the robot
    dynamic_body = my_world.CreateDynamicBody(
        position=(my_grid[start_number][0], my_grid[start_number][1]),
        angle=0 * math.pi / 2)
    box = dynamic_body.CreatePolygonFixture(box=(0.5, 0.8),
                                            density=1,
                                            friction=0.3)
    dynamic_body.userData = {'color': 'robot'}

    ##---Define colours for robot, obstacles, start- and goal positions, node position markers---##
    colors = {
        'obstacle': (115, 115, 115),
        'robot': (165, 0, 0),
        'moveable_obstacle': (77, 166, 255),
        'start_position': (255, 140, 26),
        'goal_position': (204, 255, 153),
        'node_marker': (0, 77, 0),
        'shortest_path': (255, 128, 223),
    }

    ##---Calculate and print runtime---##
    end_time = datetime.now()
    total_runtime = end_time - start_time
    print("total_runtime: %s" % (total_runtime))
예제 #33
0
    def draw(self, world, init_x, init_y, force_to_center):
        MAIN_BODY_FIXTURES = [
            fixtureDef(shape=polygonShape(vertices=[(x / self.SCALE,
                                                     y / self.SCALE)
                                                    for x, y in polygon]),
                       density=5.0,
                       friction=0.1,
                       categoryBits=0x20,
                       maskBits=0x000F) for polygon in MAIN_BODY_POLYGONS
        ]

        LEG_FD = fixtureDef(shape=polygonShape(box=(self.LEG_W / 2,
                                                    self.LEG_H / 2)),
                            density=1.0,
                            restitution=0.0,
                            categoryBits=0x20,
                            maskBits=0x000F)

        LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * self.LEG_W / 2,
                                                      self.LEG_H / 2)),
                              density=1.0,
                              restitution=0.0,
                              categoryBits=0x20,
                              maskBits=0x000F)

        init_x = init_x - MAIN_BODY_BOTTOM_WIDTH / self.SCALE * self.nb_of_bodies / 2  # initial init_x is the middle of the whole body
        previous_main_body = None
        for j in range(self.nb_of_bodies):
            main_body_x = init_x + j * (MAIN_BODY_BOTTOM_WIDTH / self.SCALE)
            main_body = world.CreateDynamicBody(position=(main_body_x, init_y),
                                                fixtures=MAIN_BODY_FIXTURES)
            main_body.color1 = (0.5, 0.4, 0.9)
            main_body.color2 = (0.3, 0.3, 0.5)
            main_body.ApplyForceToCenter((force_to_center, 0), True)

            # self.body_parts.append(BodyPart(main_body, True, True))
            main_body.userData = CustomBodyUserData(True,
                                                    is_contact_critical=False,
                                                    name="body")
            self.body_parts.append(main_body)

            for i in [-1, +1]:
                leg = world.CreateDynamicBody(
                    position=(main_body_x,
                              init_y - self.LEG_H / 2 - self.LEG_DOWN),
                    #angle=(i * 0.05),
                    fixtures=LEG_FD)
                leg.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
                leg.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
                rjd = revoluteJointDef(
                    bodyA=main_body,
                    bodyB=leg,
                    anchor=(main_body_x, init_y - self.LEG_DOWN),
                    enableMotor=True,
                    enableLimit=True,
                    maxMotorTorque=self.MOTORS_TORQUE,
                    motorSpeed=i,
                    lowerAngle=-0.8,
                    upperAngle=1.1,
                )

                leg.userData = CustomBodyUserData(False, name="leg")
                self.body_parts.append(leg)

                joint_motor = world.CreateJoint(rjd)
                joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
                self.motors.append(joint_motor)

                lower = world.CreateDynamicBody(
                    position=(main_body_x,
                              init_y - self.LEG_H * 3 / 2 - self.LEG_DOWN),
                    #angle=(i * 0.05),
                    fixtures=LOWER_FD)
                lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
                lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
                rjd = revoluteJointDef(
                    bodyA=leg,
                    bodyB=lower,
                    anchor=(main_body_x, init_y - self.LEG_DOWN - self.LEG_H),
                    enableMotor=True,
                    enableLimit=True,
                    maxMotorTorque=self.MOTORS_TORQUE,
                    motorSpeed=1,
                    lowerAngle=-1.6,
                    upperAngle=-0.1,
                )

                lower.userData = CustomBodyUserData(True, name="lower")
                self.body_parts.append(lower)

                joint_motor = world.CreateJoint(rjd)
                joint_motor.userData = CustomMotorUserData(
                    SPEED_KNEE, True, contact_body=lower, angle_correction=1.0)
                self.motors.append(joint_motor)

            if previous_main_body is not None:
                rjd = revoluteJointDef(
                    bodyA=previous_main_body,
                    bodyB=main_body,
                    anchor=(main_body_x -
                            MAIN_BODY_BOTTOM_WIDTH / self.SCALE / 2, init_y),
                    enableMotor=False,
                    enableLimit=True,
                    lowerAngle=-0.05 * np.pi,
                    upperAngle=0.05 * np.pi,
                )

                world.CreateJoint(rjd)
            previous_main_body = main_body

        self.reference_head_object = previous_main_body  # set as head the rightmost body
        self.reference_head_object.is_contact_critical = self.reference_head_object
예제 #34
0
    def _create_rocket(self, initial_coordinates=(W / 2, H / 1.2)):
        body_color = (1, 1, 1)
        # ----------------------------------------------------------------------------------------
        # LANDER

        initial_x, initial_y = initial_coordinates
        self.lander = self.world.CreateDynamicBody(
            position=(initial_x, initial_y),
            angle=0,
            fixtures=fixtureDef(
                shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                             for x, y in LANDER_POLY]),
                density=5.0,
                friction=0.1,
                categoryBits=0x0010,
                maskBits=0x001,  # collide only with ground
                restitution=0.0)  # 0.99 bouncy
        )
        self.lander.color1 = body_color
        self.lander.color2 = (0, 0, 0)

        if isinstance(self.settings['Initial Force'], str):
            self.lander.ApplyForceToCenter(
                (self.np_random.uniform(-INITIAL_RANDOM * 0.3,
                                        INITIAL_RANDOM * 0.3),
                 self.np_random.uniform(-1.3 * INITIAL_RANDOM,
                                        -INITIAL_RANDOM)), True)
        else:
            self.lander.ApplyForceToCenter(self.settings['Initial Force'],
                                           True)

        # COG is set in the middle of the polygon by default. x = 0 = middle.
        # self.lander.mass = 25
        # self.lander.localCenter = (0, 3) # COG
        # ----------------------------------------------------------------------------------------
        # LEGS
        self.legs = []
        for i in [-1, +1]:
            leg = self.world.CreateDynamicBody(
                position=(initial_x - i * LEG_AWAY / SCALE, initial_y),
                angle=(i * 0.05),
                fixtures=fixtureDef(shape=polygonShape(box=(LEG_W / SCALE,
                                                            LEG_H / SCALE)),
                                    density=5.0,
                                    restitution=0.0,
                                    categoryBits=0x0020,
                                    maskBits=0x005))
            leg.ground_contact = False
            leg.color1 = body_color
            leg.color2 = (0, 0, 0)
            rjd = revoluteJointDef(
                bodyA=self.lander,
                bodyB=leg,
                localAnchorA=(-i * 0.3 / LANDER_CONSTANT, 0),
                localAnchorB=(i * 0.5 / LANDER_CONSTANT, LEG_DOWN),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=LEG_SPRING_TORQUE,
                motorSpeed=+0.3 * i  # low enough not to jump back into the sky
            )
            if i == -1:
                rjd.lowerAngle = 40 * DEGTORAD
                rjd.upperAngle = 45 * DEGTORAD
            else:
                rjd.lowerAngle = -45 * DEGTORAD
                rjd.upperAngle = -40 * DEGTORAD
            leg.joint = self.world.CreateJoint(rjd)
            self.legs.append(leg)
        # ----------------------------------------------------------------------------------------
        # NOZZLE
        self.nozzle = self.world.CreateDynamicBody(
            position=(initial_x, initial_y),
            angle=0.0,
            fixtures=fixtureDef(
                shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                             for x, y in NOZZLE_POLY]),
                density=5.0,
                friction=0.1,
                categoryBits=0x0040,
                maskBits=0x003,  # collide only with ground
                restitution=0.0)  # 0.99 bouncy
        )
        self.nozzle.color1 = (0, 0, 0)
        self.nozzle.color2 = (0, 0, 0)
        rjd = revoluteJointDef(
            bodyA=self.lander,
            bodyB=self.nozzle,
            localAnchorA=(0, 0),
            localAnchorB=(0, 0.2),
            enableMotor=True,
            enableLimit=True,
            maxMotorTorque=NOZZLE_TORQUE,
            motorSpeed=0,
            referenceAngle=0,
            lowerAngle=-13 *
            DEGTORAD,  # +- 15 degrees limit applied in practice
            upperAngle=13 * DEGTORAD)
        # The default behaviour of a revolute joint is to rotate without resistance.
        self.nozzle.joint = self.world.CreateJoint(rjd)
        # ----------------------------------------------------------------------------------------
        # self.drawlist = [self.nozzle] + [self.lander] + self.legs
        self.drawlist = self.legs + [self.nozzle] + [self.lander]
        self.initial_mass = self.lander.mass
        self.remaining_fuel = INITIAL_FUEL_MASS_PERCENTAGE * self.initial_mass
        return
예제 #35
0
    ]
LEG_DOWN = -8/SCALE
LEG_W, LEG_H = 8/SCALE, 34/SCALE

VIEWPORT_W = 600
VIEWPORT_H = 400

TERRAIN_STEP   = 14/SCALE
TERRAIN_LENGTH = 200     # in steps
TERRAIN_HEIGHT = VIEWPORT_H/SCALE/4
TERRAIN_GRASS    = 10    # low long are grass spots, in steps
TERRAIN_STARTPAD = 20    # in steps
FRICTION = 2.5

HULL_FD = fixtureDef(
    shape=polygonShape(box=(28/SCALE/2, 28/SCALE/2)),
    density=5.0,
    friction=0.1,
    categoryBits=0x0020,
    maskBits=0x001,  # collide only with ground
    restitution=0.0) # 0.99 bouncy

NECK1_FD = fixtureDef(
    shape=polygonShape(box=(20/SCALE/2, 20/SCALE/2)),
    density=5.0,
    friction=0.1,
    categoryBits=0x0020,
    maskBits=0x001, # collide only with ground
    restitution=0.0) # 0.99 bouncy
NECK2_FD = fixtureDef(
    shape=polygonShape(box=(10/SCALE/2, 15/SCALE/2)),
예제 #36
0
    def _reset(self):
        self._destroy()
        #        self.world.contactListener_bug_workaround = ContactDetector(self)
        #        self.world.contactListener = self.world.contactListener_bug_workaround
        self.world.contactListener_keepref = ContactDetector(self)
        self.world.contactListener = self.world.contactListener_keepref
        self.game_over = False
        self.prev_shaping = None
        self.scroll = 0.0
        self.lidar_render = 0

        W = VIEWPORT_W / SCALE
        H = VIEWPORT_H / SCALE

        self._generate_terrain(self.hardcore)
        self._generate_clouds()

        init_x = TERRAIN_STEP * TERRAIN_STARTPAD / 2
        init_y = TERRAIN_HEIGHT + 2 * LEG_H
        self.hull = self.world.CreateDynamicBody(
            position=(init_x, init_y),
            fixtures=fixtureDef(
                shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                             for x, y in HULL_POLY]),
                density=5.0,
                friction=0.1,
                categoryBits=0x0020,
                maskBits=0x001,  # collide only with ground
                restitution=0.0)  # 0.99 bouncy
        )
        self.hull.color1 = (0.5, 0.4, 0.9)
        self.hull.color2 = (0.3, 0.3, 0.5)
        self.hull.ApplyForceToCenter(
            (self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)

        self.head = self.world.CreateDynamicBody(
            position=(init_x - 0 / SCALE, init_y + (HULL_H + 10) / SCALE),
            fixtures=fixtureDef(
                shape=circleShape(pos=[0, 0], radius=7.0 / SCALE),
                density=5.0,
                friction=0.1,
                categoryBits=0x0020,
                maskBits=0x001,  # collide only with ground
                restitution=0.0)  # 0.99 bouncy
        )
        self.head.color1 = (1.0, 0.4, 0.9)
        self.head.color2 = (0.3, 1.0, 0.5)
        #self.head.ApplyForceToCenter((self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)

        self.legs = []
        self.arms = []
        self.joints = []

        rjd1 = revoluteJointDef(
            bodyA=self.head,
            bodyB=self.hull,
            localAnchorA=(0, 0),
            localAnchorB=(0, +(HULL_H) / SCALE),
            #            enableMotor=True,
            #            enableLimit=True,
            #            maxMotorTorque=MOTORS_TORQUE,
            #            motorSpeed = -1,
            #            lowerAngle = -0.8,
            #            upperAngle = 1.1,
        )

        for i in [-1, +1]:
            leg = self.world.CreateDynamicBody(
                position=(init_x, init_y - LEG_H / 2 - LEG_DOWN),
                angle=(i * 0.05),
                fixtures=fixtureDef(shape=polygonShape(box=(LEG_W / 2,
                                                            LEG_H / 2)),
                                    density=1.0,
                                    restitution=0.0,
                                    categoryBits=0x0020,
                                    maskBits=0x001))
            leg.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
            leg.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
            rjd = revoluteJointDef(
                bodyA=self.hull,
                bodyB=leg,
                localAnchorA=(0, LEG_DOWN),
                localAnchorB=(0, LEG_H / 2),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed=i,
                lowerAngle=-0.8,
                upperAngle=1.1,
            )
            self.legs.append(leg)
            self.joints.append(self.world.CreateJoint(rjd))

            lower = self.world.CreateDynamicBody(
                position=(init_x, init_y - LEG_H * 3 / 2 - LEG_DOWN),
                angle=(i * 0.05),
                fixtures=fixtureDef(shape=polygonShape(box=(0.8 * LEG_W / 2,
                                                            LEG_H / 2)),
                                    density=1.0,
                                    restitution=0.0,
                                    categoryBits=0x0020,
                                    maskBits=0x001))
            lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
            lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
            rjd = revoluteJointDef(
                bodyA=leg,
                bodyB=lower,
                localAnchorA=(0, -LEG_H / 2),
                localAnchorB=(0, LEG_H / 2),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=-1.6,
                upperAngle=-0.1,
            )
            lower.ground_contact = False
            self.legs.append(lower)
            self.joints.append(self.world.CreateJoint(rjd))

        for i in [-1, +1]:
            arm = self.world.CreateDynamicBody(
                position=(init_x, init_y - arm_H / 2 + (HULL_H) / SCALE),
                angle=(i * 0.05),
                fixtures=fixtureDef(shape=polygonShape(box=(arm_W / 2,
                                                            arm_H / 2)),
                                    density=1.0,
                                    restitution=0.0,
                                    categoryBits=0x0020,
                                    maskBits=0x001))
            arm.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
            arm.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
            rjd = revoluteJointDef(
                bodyA=self.hull,
                bodyB=arm,
                localAnchorA=(0, (HULL_H) / SCALE),
                localAnchorB=(0, arm_H / 2),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed=-i,
                lowerAngle=-0.8,
                upperAngle=0.8,
            )
            self.arms.append(arm)
            self.joints.append(self.world.CreateJoint(rjd))

#            lower_arm = self.world.CreateDynamicBody(
#                position = (init_x, init_y - arm_H*3/2 + (HULL_H)/SCALE),
#                angle = (i*0.05),
#                fixtures = fixtureDef(
#                    shape=polygonShape(box=(0.8*arm_W/2, arm_H/2)),
#                    density=1.0,
#                    restitution=0.0,
#                    categoryBits=0x0020,
#                    maskBits=0x001)
#                )
#            lower_arm.color1 = (0.6-i/10., 0.3-i/10., 0.5-i/10.)
#            lower_arm.color2 = (0.4-i/10., 0.2-i/10., 0.3-i/10.)
#            rjd = revoluteJointDef(
#                bodyA=arm,
#                bodyB=lower_arm,
#                localAnchorA=(0, -arm_H/2),
#                localAnchorB=(0, arm_H/2),
#                enableMotor=True,
#                enableLimit=True,
#                maxMotorTorque=MOTORS_TORQUE,
#                motorSpeed = 1,
#                lowerAngle = -1.6,
#                upperAngle = -0.1,
#                )
#            lower_arm.ground_contact = False
#            self.arms.append(lower_arm)
#            self.joints.append(self.world.CreateJoint(rjd))

        self.joints.append(self.world.CreateJoint(rjd1))
        self.drawlist = self.terrain + self.legs + self.arms + [self.hull] + [
            self.head
        ]

        class LidarCallback(Box2D.b2.rayCastCallback):
            def ReportFixture(self, fixture, point, normal, fraction):
                if (fixture.filterData.categoryBits & 1) == 0:
                    return 1
                self.p2 = point
                self.fraction = fraction
                return 0

        self.lidar = [LidarCallback() for _ in range(10)]

        self.a = np.array([0, 0, 0, 0])
        return self._step(0)
예제 #37
0
    def draw(self, world, init_x, init_y, force_to_center):
        HULL_FIXTURES = [
            fixtureDef(shape=polygonShape(vertices=[(x / self.SCALE,
                                                     y / self.SCALE)
                                                    for x, y in polygon]),
                       density=self.DENSITY * 1.25,
                       friction=0.1,
                       categoryBits=0x20,
                       maskBits=0x000F)  # 0.99 bouncy
            for polygon in HULL_POLYGONS
        ]

        LEG_FD = fixtureDef(shape=polygonShape(box=(self.LEG_W / 2,
                                                    self.LEG_H / 2)),
                            density=self.DENSITY * 0.25,
                            restitution=0.0,
                            categoryBits=0x20,
                            maskBits=0x000F)

        LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * self.LEG_W / 2,
                                                      self.LEG_H / 2)),
                              density=self.DENSITY * 0.25,
                              restitution=0.0,
                              categoryBits=0x20,
                              maskBits=0x000F)

        hull = world.CreateDynamicBody(position=(init_x, init_y),
                                       fixtures=HULL_FIXTURES)
        hull.color1 = (0.44, 0.81, 0.14)
        hull.color2 = (0.36, 0.66, 0.11)
        hull.ApplyForceToCenter((force_to_center, 0), True)

        hull.userData = CustomBodyUserData(True,
                                           is_contact_critical=False,
                                           name="hull")
        self.body_parts.append(hull)
        self.reference_head_object = hull

        for i in [-1, +1]:
            leg = world.CreateDynamicBody(
                position=(init_x, init_y - self.LEG_H / 2 - self.LEG_DOWN),
                #angle=(i * 0.05),#2°
                fixtures=LEG_FD)
            leg.color1 = (0.44, 0.81, 0.14)
            leg.color2 = (0.36, 0.66, 0.11)
            rjd = revoluteJointDef(
                bodyA=hull,
                bodyB=leg,
                anchor=(init_x, init_y - self.LEG_DOWN),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=self.MOTORS_TORQUE,
                motorSpeed=i,
                lowerAngle=-0.8,
                upperAngle=1.1,
            )

            leg.userData = CustomBodyUserData(False, name="leg")
            self.body_parts.append(leg)

            joint_motor = world.CreateJoint(rjd)
            joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
            self.motors.append(joint_motor)

            lower = world.CreateDynamicBody(
                position=(init_x, init_y - self.LEG_H * 3 / 2 - self.LEG_DOWN),
                #angle=(i * 0.05), #2°
                fixtures=LOWER_FD)
            lower.color1 = (1.0, 0.25, 0.04)
            lower.color2 = (0.86, 0.29, 0.12)
            rjd = revoluteJointDef(
                bodyA=leg,
                bodyB=lower,
                anchor=(init_x, init_y - self.LEG_DOWN - self.LEG_H),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=self.MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=-1.6,
                upperAngle=-0.1,
            )

            lower.userData = CustomBodyUserData(True, name="lower")
            self.body_parts.append(lower)

            joint_motor = world.CreateJoint(rjd)
            joint_motor.userData = CustomMotorUserData(SPEED_KNEE,
                                                       True,
                                                       contact_body=lower,
                                                       angle_correction=1.0)
            self.motors.append(joint_motor)
예제 #38
0
파일: engine.py 프로젝트: JonComo/stumbly
 def add_static_body(self, p, size):
     return self.world.CreateStaticBody(position=self.to_pybox2d(p), \
         shapes=polygonShape(box=self.size_to_pybox2d(size)))
예제 #39
0
        def _create_decoration():
            objs = []
            objs.append(
                self.world.CreateStaticBody(
                    position=(W / 2, H / 2),
                    angle=0.0,
                    fixtures=fixtureDef(shape=circleShape(radius=100 / SCALE,
                                                          pos=(0, 0)),
                                        categoryBits=0x0,
                                        maskBits=0x0)))
            objs[-1].color1 = (0.8, 0.8, 0.8)
            objs[-1].color2 = (0.8, 0.8, 0.8)

            objs.append(
                self.world.CreateStaticBody(
                    position=(W / 2, H / 2),
                    angle=0.0,
                    fixtures=fixtureDef(shape=circleShape(radius=100 / SCALE,
                                                          pos=(0, 0)),
                                        categoryBits=0x0,
                                        maskBits=0x0)))
            objs[-1].color1 = (0.8, 0.8, 0.8)
            objs[-1].color2 = (0.8, 0.8, 0.8)

            objs.append(
                self.world.CreateStaticBody(
                    position=(W / 2 - 250 / SCALE, H / 2),
                    angle=0.0,
                    fixtures=fixtureDef(shape=circleShape(radius=70 / SCALE,
                                                          pos=(0, 0)),
                                        categoryBits=0x0,
                                        maskBits=0x0)))
            objs[-1].color1 = (255. / 255, 204. / 255, 191. / 255)
            objs[-1].color2 = (255. / 255, 204. / 255, 191. / 255)

            poly = [(0, 100), (100, 100), (100, -100), (0, -100)]
            objs.append(
                self.world.CreateStaticBody(
                    position=(W / 2 - 240 / SCALE, H / 2),
                    angle=0.0,
                    fixtures=fixtureDef(
                        shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                                     for x, y in poly]),
                        categoryBits=0x0,
                        maskBits=0x0)))
            objs[-1].color1 = (1, 1, 1)
            objs[-1].color2 = (1, 1, 1)

            objs.append(
                self.world.CreateStaticBody(
                    position=(W / 2 + 250 / SCALE, H / 2),
                    angle=0.0,
                    fixtures=fixtureDef(shape=circleShape(radius=70 / SCALE,
                                                          pos=(0, 0)),
                                        categoryBits=0x0,
                                        maskBits=0x0)))
            objs[-1].color1 = (255. / 255, 204. / 255, 191. / 255)
            objs[-1].color2 = (255. / 255, 204. / 255, 191. / 255)

            poly = [(100, 100), (0, 100), (0, -100), (100, -100)]
            objs.append(
                self.world.CreateStaticBody(
                    position=(W / 2 + 140 / SCALE, H / 2),
                    angle=0.0,
                    fixtures=fixtureDef(
                        shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                                     for x, y in poly]),
                        categoryBits=0x0,
                        maskBits=0x0)))
            objs[-1].color1 = (1, 1, 1)
            objs[-1].color2 = (1, 1, 1)

            return objs
예제 #40
0
    def draw(self, world, init_x, init_y, force_to_center):
        head = world.CreateDynamicBody(
            position=(init_x, init_y + self.BODY_HEIGHT / self.SCALE / 2 + self.HEAD_HEIGHT / self.SCALE / 2 + 0.2),
            fixtures=fixtureDef(
                shape=polygonShape(vertices=[(x / self.SCALE, y / self.SCALE) for x, y in [
                    (-5, +10), (+5, +10),
                    (+5, -10), (-5, -10)]]),
                density=5.0,
                friction=0.1,
                categoryBits=0x20,
                maskBits=0x1
            )
        )
        head.color1 = (0.5, 0.4, 0.9)
        head.color2 = (0.3, 0.3, 0.5)
        head.ApplyForceToCenter((force_to_center, 0), True)

        head.userData = CustomBodyUserData(True, is_contact_critical=self.reset_on_hull_critical_contact, name="head")
        self.body_parts.append(head)
        self.reference_head_object = head

        body = world.CreateDynamicBody(
            position=(init_x, init_y),
            fixtures=fixtureDef(
                shape=polygonShape(vertices=[(x / self.SCALE, y / self.SCALE) for x, y in [
                    (-8, +25), (+8, +25),
                    (+8, -20), (-8, -20)]]),
                density=5.0,
                friction=0.1,
                categoryBits=0x20,
                maskBits=0x1  # collide only with ground
            )
        )
        body.color1 = (0.5, 0.4, 0.9)
        body.color2 = (0.3, 0.3, 0.5)

        body.userData = CustomBodyUserData(True, is_contact_critical=True, name="body")
        self.body_parts.append(body)
        
        rjd = revoluteJointDef(
            bodyA=head,
            bodyB=body,
            anchor=(init_x, init_y + self.BODY_HEIGHT / self.SCALE / 2),
            enableMotor=False,
            enableLimit=True,
            lowerAngle=-0.1 * np.pi,
            upperAngle=0.1 * np.pi,
        )

        world.CreateJoint(rjd)

        UPPER_LIMB_FD = fixtureDef(
            shape=polygonShape(box=(self.LIMB_W / 2, self.LIMB_H / 2)),
            density=1.0,
            restitution=0.0,
            categoryBits=0x20,
            maskBits=0x1
        )

        LOWER_LIMB_FD = fixtureDef(
            shape=polygonShape(box=(0.8 * self.LIMB_W / 2, self.LIMB_H / 2)),
            density=1.0,
            restitution=0.0,
            categoryBits=0x20,
            maskBits=0x1
        )

        # LEGS
        for i in [+1, +1]:
                upper = world.CreateDynamicBody(
                    position=(init_x, init_y - self.LIMB_H / 2 - self.LEG_DOWN),
                    # angle=(i * 0.05),
                    fixtures=UPPER_LIMB_FD
                )
                upper.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
                upper.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
                rjd = revoluteJointDef(
                    bodyA=body,
                    bodyB=upper,
                    anchor=(init_x, init_y - self.LEG_DOWN),
                    enableMotor=True,
                    enableLimit=True,
                    maxMotorTorque=self.MOTORS_TORQUE,
                    motorSpeed=1,
                    lowerAngle=-0.3 * np.pi,
                    upperAngle=0.6 * np.pi,
                )

                upper.userData = CustomBodyUserData(False, name="upper_leg")
                self.body_parts.append(upper)

                joint_motor = world.CreateJoint(rjd)
                joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
                self.motors.append(joint_motor)

                lower = world.CreateDynamicBody(
                    position=(init_x, init_y - self.LIMB_H * 3 / 2 - self.LEG_DOWN),
                    # angle=(i * 0.05),
                    fixtures=LOWER_LIMB_FD
                )
                lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
                lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
                rjd = revoluteJointDef(
                    bodyA=upper,
                    bodyB=lower,
                    anchor=(init_x, init_y - self.LIMB_H - self.LEG_DOWN),
                    enableMotor=True,
                    enableLimit=True,
                    maxMotorTorque=self.MOTORS_TORQUE,
                    motorSpeed=1,
                    lowerAngle=-0.75 * np.pi,
                    upperAngle=-0.1,
                )

                lower.userData = CustomBodyUserData(True, name="lower_leg")
                self.body_parts.append(lower)

                joint_motor = world.CreateJoint(rjd)
                joint_motor.userData = CustomMotorUserData(SPEED_KNEE, True, angle_correction=1.0, contact_body=lower)
                self.motors.append(joint_motor)

        # ARMS
        for j in [-1, -1]:
            upper = world.CreateDynamicBody(
                position=(init_x, init_y - self.LIMB_H / 2 + self.ARM_UP),
                # angle=(i * 0.05),
                fixtures=UPPER_LIMB_FD
            )
            upper.color1 = (0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
            upper.color2 = (0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
            rjd = revoluteJointDef(
                bodyA=body,
                bodyB=upper,
                anchor=(init_x, init_y + self.ARM_UP),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=self.MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=-0.5 * np.pi,
                upperAngle=0.8 * np.pi,
            )

            upper.userData = CustomBodyUserData(False, name="upper_arm")
            self.body_parts.append(upper)

            joint_motor = world.CreateJoint(rjd)
            joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
            self.motors.append(joint_motor)

            lower = world.CreateDynamicBody(
                position=(init_x, init_y - self.LIMB_H * 3 / 2 + self.ARM_UP),
                # angle=(i * 0.05),
                fixtures=LOWER_LIMB_FD
            )
            lower.color1 = (0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
            lower.color2 = (0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
            rjd = revoluteJointDef(
                bodyA=upper,
                bodyB=lower,
                anchor=(init_x, init_y - self.LIMB_H + self.ARM_UP),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=self.MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=0,
                upperAngle=0.75 * np.pi,
            )

            lower.userData = CustomBodyUserData(True, name="lowar_arm")
            self.body_parts.append(lower)

            joint_motor = world.CreateJoint(rjd)
            joint_motor.userData = CustomMotorUserData(SPEED_KNEE, True, contact_body=lower)
            self.motors.append(joint_motor)
예제 #41
0
    def reset(self):
        self.obj_type = 5
        self.timer = 0
        self.trigger = 'stop'
        self.curr_grab = 1000
        self.curr_right = 0
        self.consecutive_wait = 0
        self.dist_from_obj = 0
        self.dist_from_pos = 0
        self.colour_list = []
        self.obj_type = random.randint(1, 5)
        self.lift_reward = [0.1] * 10
        self.max_height = 200
        for i_body in range(len(self.body_list)):
            if i_body != 0:
                self.world.DestroyBody(self.body_list[i_body])
        self.body_list = []
        self.ground_body = self.world.CreateStaticBody(
            position=(160, 0), shapes=polygonShape(box=(160, 7.5)), angle=0)
        self.wall_body1 = self.world.CreateStaticBody(
            position=(320, 80), shapes=polygonShape(box=(7.5, 320)), angle=0)
        self.wall_body2 = self.world.CreateStaticBody(
            position=(0, 80), shapes=polygonShape(box=(7.5, 320)), angle=0)
        self.wall_body3 = self.world.CreateStaticBody(
            position=(100000, 0), shapes=polygonShape(box=(5, 70)), angle=0)
        self.roof = self.world.CreateStaticBody(position=(160, 320),
                                                shapes=polygonShape(box=(160,
                                                                         7.5)),
                                                angle=0)
        pos1 = np.random.uniform(15, 130)
        pos2 = np.random.uniform(280, 310)
        self.arm = self.world.CreateDynamicBody(position=(pos1, 315),
                                                shapes=polygonShape(box=(6,
                                                                         12)),
                                                angle=0)
        self.grab = self.world.CreateDynamicBody(
            position=(pos1, pos2),
            shapes=polygonShape(box=(6, 6)),
            angle=0,
            fixedRotation=True,
        )
        self.grab.CreatePolygonFixture(box=(6, 6), density=0.1, friction=0)
        self.stick = self.world.CreateDynamicBody(
            position=(pos1, pos2),
            shapes=polygonShape(box=(40, 6)),
            angle=0,
        )
        self.stick.CreatePolygonFixture(box=(40, 6), density=0.1, friction=0)
        offset = 2.5
        DENSITY_2 = 0.1
        FRICTION = 10
        if self.obj_type in [2, 3, 5]:
            OBJ_2 = [(-40, 30), (-10, 30), (-10, 20), (-40, 20)]
            OBJ_3 = [(-40, -20), (-10, -20), (-10, -30), (-40, -30)]
        else:
            OBJ_2 = [(-40, 30), (-10, 30), (-10, 10), (-40, 10)]
            OBJ_3 = [(-40, -10), (-10, -10), (-10, -30), (-40, -30)]
        OBJ_1 = [(-10, 30), (10, 30), (10, -30), (-10, -30)]
        OBJ_4 = [(10, 10), (40, 10), (40, -10), (10, -10)]
        OBJ_5 = [(-40, -5), (-10, -5), (-10, 5), (-40, 5)]
        definedFixturesObj_1 = fixtureDef(
            shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                         for x, y in OBJ_1]),
            density=DENSITY_2,
            friction=FRICTION,
            restitution=0.0)

        definedFixturesObj_2 = fixtureDef(
            shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                         for x, y in OBJ_2]),
            density=DENSITY_2,
            friction=FRICTION,
            restitution=0.0)

        definedFixturesObj_3 = fixtureDef(
            shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                         for x, y in OBJ_3]),
            density=DENSITY_2,
            friction=FRICTION,
            restitution=0.0)

        definedFixturesObj_4 = fixtureDef(
            shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                         for x, y in OBJ_4]),
            density=DENSITY_2,
            friction=FRICTION,
            restitution=0.0)
        definedFixturesObj_5 = fixtureDef(
            shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                         for x, y in OBJ_5]),
            density=DENSITY_2,
            friction=FRICTION,
            restitution=0.0)
        if self.obj_type == 1:
            fixturesListObj = [
                definedFixturesObj_1, definedFixturesObj_2,
                definedFixturesObj_3, definedFixturesObj_4
            ]
        elif self.obj_type == 2:
            fixturesListObj = [
                definedFixturesObj_1, definedFixturesObj_3,
                definedFixturesObj_5
            ]
        elif self.obj_type == 3:
            fixturesListObj = [
                definedFixturesObj_1, definedFixturesObj_2,
                definedFixturesObj_5
            ]
        elif self.obj_type == 4:
            fixturesListObj = [
                definedFixturesObj_1, definedFixturesObj_2,
                definedFixturesObj_3
            ]
        elif self.obj_type == 5:
            fixturesListObj = [
                definedFixturesObj_1, definedFixturesObj_2,
                definedFixturesObj_3, definedFixturesObj_5
            ]

        self.object = self.world.CreateDynamicBody(position=(random.randint(
            140, 280), 40),
                                                   angle=0,
                                                   angularDamping=10,
                                                   linearDamping=1,
                                                   fixtures=fixturesListObj)

        self.object.mass = 1000

        self.prev_grab_pos = self.grab.position.copy()

        self.h_pj = self.world.CreatePrismaticJoint(
            bodyA=self.wall_body2,
            bodyB=self.arm,
            anchor=(self.wall_body2.worldCenter[0],
                    self.wall_body2.worldCenter[1]),
            axis=(1, 0),
            lowerTranslation=-100000,
            upperTranslation=1000000,
            enableLimit=True,
            motorSpeed=0,
            maxMotorForce=5000000000000000000000,
            enableMotor=True,
        )
        self.v_pj = self.world.CreatePrismaticJoint(
            bodyA=self.arm,
            bodyB=self.grab,
            anchor=(self.grab.worldCenter[0], self.grab.worldCenter[1]),
            axis=(0, 1),
            lowerTranslation=-100000,
            upperTranslation=0,
            enableLimit=True,
            motorSpeed=0,
            maxMotorForce=5000000000000000000000,
            enableMotor=True,
        )
        self.a_rj = self.world.CreateRevoluteJoint(
            bodyA=self.grab,
            bodyB=self.stick,
            anchor=self.stick.worldCenter,
            maxMotorTorque=100000000,
            motorSpeed=0,
            enableMotor=True,
            collideConnected=False,
        )
        self.colour_list.append((255, 255, 255, 255))
        self.colour_list.append((255, 255, 255, 255))
        self.colour_list.append((255, 255, 255, 255))
        self.colour_list.append((255, 255, 255, 255))
        self.colour_list.append((255, 255, 255, 255))
        self.colour_list.append((150, 200, 150, 200))
        self.colour_list.append((200, 150, 200, 150))
        self.colour_list.append((200, 150, 200, 150))
        self.body_list = [
            self.ground_body, self.wall_body1, self.wall_body2, self.roof,
            self.arm, self.grab, self.object, self.stick
        ]

        self.reward_list = [1] * len(self.body_list)

        self.update_screen()
        self.state1 = get_game_screen(screen)
        return self.state1
예제 #42
0
    def _reset(self):
        self._destroy()
        self.world.contactListener_bug_workaround = ContactDetector(self)
        self.world.contactListener = self.world.contactListener_bug_workaround
        self.game_over = False
        self.prev_shaping = None
        self.scroll = 0.0
        self.lidar_render = 0

        W = VIEWPORT_W/SCALE
        H = VIEWPORT_H/SCALE

        self._generate_terrain(self.hardcore)
        self._generate_clouds()

        self.hull = self.world.CreateDynamicBody(
            position = (hull_x, hull_y),
            fixtures = fixtureDef(
                shape=polygonShape(vertices=[ (x/SCALE,y/SCALE) for x,y in HULL_POLY ]),
                density=5.0,
                friction=0.1,
                categoryBits=0x0020,
                maskBits=0x001,  # collide only with ground
                restitution=0.0) # 0.99 bouncy
                )
        self.hull.color1 = (0.5,0.4,0.9)
        self.hull.color2 = (0.3,0.3,0.5)
        #self.hull.ApplyForceToCenter((self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)

        #self.hull.ApplyForceToCenter((0, 10*10.0*5.0*(20/SCALE)**2), False)

        self.legs = []
        self.joints = []
        for i in [-1,+1]:
            leg = self.world.CreateDynamicBody(
                position = (leg_x, leg_y),
                angle = gamma_i,
                fixtures = fixtureDef(
                    shape=polygonShape(box=(LEG_W/2, LEG_H/2)),
                    density=1.0,
                    restitution=0.0,
                    friction=0.1,
                    categoryBits=0x0020,
                    maskBits=0x001)
                )

            leg.color1 = (0.6-i/10., 0.3-i/10., 0.5-i/10.)
            leg.color2 = (0.4-i/10., 0.2-i/10., 0.3-i/10.)
            rjd = revoluteJointDef(
                bodyA=self.hull,
                bodyB=leg,
                localAnchorA=(0, 0),
                localAnchorB=(0, LEG_H/2),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed = 0.0,
                referenceAngle = 0.0,
                lowerAngle = -1.4,      # -80 degree
                upperAngle = 1.4,       # 80 degree
                )

            self.legs.append(leg)
            self.joints.append(self.world.CreateJoint(rjd))

            lower = self.world.CreateDynamicBody(
                position = (lower_x, lower_y),
                angle = (-math.pi/2+a_i),
                fixtures = fixtureDef(
                    shape=polygonShape(box=(LEG_W/2, LEG_H/2)),
                    density=1.0,
                    friction=0.1,
                    restitution=0.0,
                    categoryBits=0x0020,
                    maskBits=0x001)
                )
            lower.color1 = (0.6-i/10., 0.3-i/10., 0.5-i/10.)
            lower.color2 = (0.4-i/10., 0.2-i/10., 0.3-i/10.)
            rjd = revoluteJointDef(
                bodyA=leg,
                bodyB=lower,
                localAnchorA=(0, -LEG_H/2),
                localAnchorB=(0, LEG_H/2),  
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=MOTORS_TORQUE,
                motorSpeed = 0.0,
                referenceAngle = -1*math.pi,
                lowerAngle = 0.35,    # 20 degree
                upperAngle = 3.14    # 180 degree
                )

            lower.ground_contact = False
            # foot.ground_contact = False

            self.legs.append(lower)
            # self.legs.append(foot)
            self.joints.append(self.world.CreateJoint(rjd))
            # self.joints.append(self.world.CreateJoint(foot_jd))

        self.drawlist = self.terrain + self.legs + [self.hull]

        class LidarCallback(Box2D.b2.rayCastCallback):
            def ReportFixture(self, fixture, point, normal, fraction):
                if (fixture.filterData.categoryBits & 1) == 0:
                    return 1
                self.p2 = point
                self.fraction = fraction
                return 0
        self.lidar = [LidarCallback() for _ in range(10)]

        #self.hull.ApplyForceToCenter((0, 10.0 * (5.0 * (20 / SCALE) ** 2)), True)
        #self.legs[0].ApplyForceToCenter((0, 10.0 * 1.0 * LEG_H * LEG_W), True)
        #self.legs[1].ApplyForceToCenter((0, 10.0 * 1.0 * LEG_H * LEG_W), True)
        #self.legs[2].ApplyForceToCenter((0, 10.0 * 1.0 * LEG_H * LEG_W), True)
        #self.legs[3].ApplyForceToCenter((0, 10.0 * 1.0 * LEG_H * LEG_W), True)

        return self._step(np.array([0.0,0.0,0.0,0.0]))[0]
예제 #43
0
# --- constants ---
# Box2D deals with meters, but we want to display pixels, 
# so define a conversion factor:
PPM=20.0 # pixels per meter
TARGET_FPS=30
TIME_STEP=1.0/TARGET_FPS


# --- pybox2d world setup ---
# Create the world
world=world(gravity=(0,-10),doSleep=True)

# And a static body to hold the ground shape
ground_body=world.CreateStaticBody(
    position=(0,0),
    shapes=polygonShape(box=(70,1)),
    )

for idx, poly in enumerate(polys):
    print idx
    vecs = [vec2(p[0], HT-p[1]) / PPM for p in poly]
    mn = sum(vecs, vec2(0,0)) / len(vecs)
    if mn[1] > 17:
        continue
    try:
        ps = polygonShape(vertices=[1.02 * (v-mn) for v in vecs])
    except:
        vecs.reverse()
        try:
            ps = polygonShape(vertices=[v-mn for v in vecs])
        except:
예제 #44
0
TIME_STEP = 1.0 / TARGET_FPS
SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480

# --- pygame setup ---
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption('Simple pygame example')
clock = pygame.time.Clock()

# --- pybox2d world setup ---
# Create the world
world = world(gravity=(0, -10), doSleep=True)

# And a static body to hold the ground shape
ground_body = world.CreateStaticBody(
    position=(0, 0),
    shapes=polygonShape(box=(50, 1)),
)

# Create a couple dynamic bodies

body = world.CreateDynamicBody(position=(20, 15))
circle = body.CreateCircleFixture(radius=0.5, density=1, friction=0.3)

colors = {
    staticBody: (255, 255, 255, 255),
    dynamicBody: (127, 127, 127, 255),
}

# Let's play with extending the shape classes to draw for us.

예제 #45
0
def simulate(cmd, trj):
    import Box2D  # The main library
    # Box2D.b2 maps Box2D.b2Vec2 to vec2 (and so on)
    from Box2D.b2 import (world, polygonShape, circleShape, staticBody, dynamicBody, vec2)

    # --- constants ---
    # Box2D deals with meters, but we want to display pixels,
    # so define a conversion factor:
    PPM = 20.0  # pixels per meter
    TIME_STEP = 1.0 / TARGET_FPS
    SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480

    # --- pygame setup ---
    screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
    pygame.display.set_caption('Simple pygame example')
    clock = pygame.time.Clock()

    # --- pybox2d world setup ---
    # Create the world
    world = world(gravity=(0, 0), doSleep=True)

    # Add a static body to hold the ground shape
    ground_body = world.CreateStaticBody(
        position=(0, 1),
        shapes=polygonShape(box=(50, 1)),
    )

    # Create dynamic bodies
    des_body = world.CreateDynamicBody(position=(15, 12), angle=0, 
        linearDamping = 0.5*4*9.8, angularDamping = 0.3*1/12*9.8)

    obs1_body = world.CreateDynamicBody(position=(18, 12), angle=0,
        linearDamping = 0.5*16*9.8, angularDamping = 0.3*4/12*9.8)
    obs2_body = world.CreateDynamicBody(position=(11,11), angle=0,
        linearDamping = 0.5*36*9.8, angularDamping = 0.3*4/12*9.8)

    # Create fingers as kinematic bodies (infinite masses and directly controls velocity)
    width = 2.5
    fng1 = world.CreateKinematicBody(position=(16, 5), angle = 0)
    fng2 = world.CreateKinematicBody(position=(16+width,5), angle = 0)

        # And add box fixtures onto it (with a nonzero density, so it will move)
    des_box  = des_body.CreatePolygonFixture(box=(1, 1), density=1, friction=0.3, restitution = 0.8)
    obs1_box = obs1_body.CreatePolygonFixture(box=(2, 2), density=1, friction=0.3, restitution = 0.8)
    obs2_box = obs2_body.CreatePolygonFixture(box=(3, 3), density=1, friction=0.3, restitution = 0.8)

    # Add sensors for the contact points
    # print vec2(-1,0)
    cnt1 = des_body.CreatePolygonFixture(box=(0.05, 0.05, vec2(-1,0), 0), density=0, isSensor = True)
    cnt2 = des_body.CreatePolygonFixture(box=(0.05, 0.05, vec2( 1,0), 0), density=0, isSensor = True)
    printflag = True

    # Model fingers as small circular cross sections
    # circle = circleShape(radius=0.1)
    fng1_cir = fng1.CreatePolygonFixture(box = (0.1, 0.1), density = 5, friction = 0.3)
    fng2_cir = fng2.CreatePolygonFixture(box = (0.1, 0.1), density = 5, friction = 0.3)

    # Mass and Moment of Inertia data
    # print "des_body: " + str(des_body.mass) + " kg , " + str(des_body.inertia) + " kg*m^2"
    # print "obs_body1: " + str(obs_body1.mass) + " kg , " + str(obs_body1.inertia) + " kg*m^2"
    # print fng1.linearVelocity

    colors = [(255, 255, 255, 255), (255, 50, 50, 255), (124,252,0), (124,252,0),
         (50, 50, 255, 255), (50, 50, 255, 255), (255, 255, 255, 255), (255, 255, 255, 255)]
    bodies = [ground_body, des_body, obs1_body, obs2_body, fng1, fng2]

    # LOAD DESIRED VELOCITY PROFILE
    if cmd == 'naive':
        v_prof = np.loadtxt('examples/vel_prof1.txt', delimiter=';')
        v_x = v_prof[:,0]
        v_y = v_prof[:,1]
        psi_prof = np.loadtxt('examples/pos_prof1.txt', delimiter=';')
        xfng = np.reshape(np.matrix(psi_prof[:,0]),(N,1))
        yfng = np.reshape(np.matrix(psi_prof[:,1]),(N,1))
        # print xfng
        # print v_y/TARGET_FPS

    else:
        v_prof = np.gradient(trj, axis=0)*TARGET_FPS
        v_x = v_prof[:,0]
        v_y = v_prof[:,1]
        xfng = trj[:,0]
        yfng = trj[:,1]
        # print 'something else', v_x

    # GATHER ACTUAL FNG POSITIONS
    # xfng = np.zeros((N, 1))
    # yfng = np.zeros((N, 1))

    # INTIALIZE THE COST FUNCTION
    fx = 0
    fy = 0
    LQ = 1
    xdes = np.zeros((N,1))
    ydes = np.zeros((N,1))

    hits  = 0
    cnt_reward = 0

    # --- main game loop ---
    # while True:
    for k in range(N):
        # Check the event queue
        for event in pygame.event.get():
            if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE):
                # The user closed the window or pressed escape
                running = False

        screen.fill((0, 0, 0, 0))
        # Draw the world
        i = 0
        # Cast a ray from the center of the fingers to the desired object
        x_ray = fng1.worldCenter[0] + width/2
        y_ray = fng1.worldCenter[1]
        x_db  = des_body.worldCenter[0]
        y_db  = des_body.worldCenter[1]

        input = rayCastInput(p1=(x_ray,y_ray), p2=(x_db,y_db), maxFraction=1)
        output = rayCastOutput()
        for body in bodies:  # or: world.bodies
            # The body gives us the position and angle of its shapes
            for fixture in body.fixtures:
                # The fixture holds information like density and friction,
                # and also the shape.
                shape = fixture.shape

                # Naively assume that this is a polygon shape. (not good normally!)
                # We take the body's transform and multiply it with each
                # vertex, and then convert from meters to pixels with the scale
                # factor.
                vertices = [(body.transform * v) * PPM for v in shape.vertices]

                # But wait! It's upside-down! Pygame and Box2D orient their
                # axes in different ways. Box2D is just like how you learned
                # in high school, with positive x and y directions going
                # right and up. Pygame, on the other hand, increases in the
                # right and downward directions. This means we must flip
                # the y components.
                vertices = [(v[0], SCREEN_HEIGHT - v[1]) for v in vertices]

                if body != des_body:
                    hit = shape.RayCast(output, input, body.transform, 0)
                    if hit:
                        hits = 1 + hits

                pygame.draw.polygon(screen, colors[i], vertices)
                i = i + 1
                # print i
            vd = vec2((float)(v_x[k]), (float)(v_y[k]))
            fng1.linearVelocity = vd
            fng2.linearVelocity = vd
            # print fng1.linearVelocity
            # print fng2.linearVelocity    

        if hits > 0:
            cnt_reward = 0
        else:
            cnt_reward = 1*LQ
        print cnt_reward

        # Collect data from these bodies
        KEx_des, KEy_des = KE(des_body)
        KEx_obs1, KEy_obs1 = KE(obs1_body)
        KEx_obs2, KEy_obs2 = KE(obs2_body)
        KEx_obs = KEx_obs1 + KEx_obs2
        KEy_obs = KEy_obs1 + KEy_obs2

        psi_des = des_body.GetWorldPoint(des_body.localCenter + [-width/2,0])
        xdes[k] = psi_des[0]
        ydes[k] = psi_des[1]
        # xdes[k] = 13.75 # (CONSTANT)
        # ydes[k] = 12 # (CONSTANT)12

        # Collect data from the fingers
        KEx_fng1, KEy_fng1 = KE_fng(fng1, mfng)
        KEx_fng2, KEy_fng2 = KE_fng(fng2, mfng)
        # xfng[k] = fng1.worldCenter[0]
        # yfng[k] = fng1.worldCenter[1]

        # # Check contacts
        # cnt_reward = 0
        # for c in des_body.contacts:
        #     # if printflag:
        #     #     print c.contact.touching
        #     #     printflag = False
        #     if c.contact.touching :
        #         # print "sensor triggered"
        #         cnt_reward = 0
        #     else:
        #         # print "contacts points are free"
        #         cnt_reward = 1*LQ

        # Integrate the Cost function
        # print cnt_reward
        fx = C1*KEx_fng1+ C2*mfng*np.abs(xfng[k]-xdes[k])**2 + C3*KEx_des + C4*KEx_obs - C5*cnt_reward + fx
        fy = C1*KEy_fng1+ C2*mfng*np.abs(yfng[k]-ydes[k])**2 + C3*KEy_des + C4*KEy_obs - C5*cnt_reward + fy
        # print "KEx: " + str(KEx_fng1) + ", KEy: " + str(KEy_fng1)

        # Make Box2D simulate the physics of our world for one step.
        # Instruct the world to perform a single step of simulation. It is
        # generally best to keep the time step and iterations fixed.
        # See the manual (Section "Simulating the World") for further discussion
        # on these parameters and their implications.
        world.Step(TIME_STEP, 10, 10)
        # Flip the screen and try to keep at the target FPS
        if cmd == "naive" or cmd == "show":
            pygame.display.flip()
            clock.tick(TARGET_FPS)
        else:
            pass

    pygame.quit()
    print('Simulation Done!')
    # print 'xdes', xdes
    # print 'xfng', xfng
    return xfng, yfng, xdes, ydes, fx, fy
예제 #46
0
파일: world.py 프로젝트: ctoth/shooter
	def create_wall_tile(self, position, size=(0.5, 0.5), **kwargs):
		shape = b2.polygonShape(box=size)
		self.bodies.append(self.world.CreateStaticBody(shapes=shape, position=position, userData=self, **kwargs))
예제 #47
0
    def draw(self, world, init_x, init_y, force_to_center):
        ''' Circular body
        MAIN_BODY_FIXTURES = fixtureDef(
            shape=circleShape(radius=0.4),
            density=1.0,
            restitution=0.0,
            categoryBits=0x20,
            maskBits=0x000F
        )

        '''
        MAIN_BODY_FIXTURES = [
            fixtureDef(shape=polygonShape(vertices=[(x / self.SCALE,
                                                     y / self.SCALE)
                                                    for x, y in polygon]),
                       density=5.0,
                       friction=0.1,
                       categoryBits=0x20,
                       maskBits=0x000F) for polygon in MAIN_BODY_POLYGONS
        ]

        LEG_FD = fixtureDef(shape=polygonShape(box=(self.LEG_W / 2,
                                                    self.LEG_H / 2)),
                            density=1.0,
                            restitution=0.0,
                            categoryBits=0x20,
                            maskBits=0x000F)

        LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * self.LEG_W / 2,
                                                      self.LEG_H / 2)),
                              density=1.0,
                              restitution=0.0,
                              categoryBits=0x20,
                              maskBits=0x000F)

        main_body = world.CreateDynamicBody(position=(init_x, init_y),
                                            fixtures=MAIN_BODY_FIXTURES)

        basis_color1 = (0.6, 0.3, 0.5)
        basis_color2 = (0.4, 0.2, 0.3)
        main_body.color1 = tuple([c - 0.1 for c in basis_color1])
        main_body.color2 = tuple([c - 0.1 for c in basis_color2])
        leg_color1 = tuple([c + 0.1 for c in basis_color1])
        leg_color2 = tuple([c + 0.1 for c in basis_color2])

        main_body.ApplyForceToCenter((force_to_center, 0), True)

        main_body.userData = CustomBodyUserData(True,
                                                is_contact_critical=True,
                                                name="main_body")
        self.body_parts.append(main_body)
        self.reference_head_object = main_body

        for i in [+1, -1] * self.nb_pairs_of_legs:
            ##### First part of the leg #####
            upper_leg_angle = 0.25 * np.pi * i
            upper_leg_x_distance = np.sin(upper_leg_angle) * self.LEG_H / 2
            upper_leg_y_distance = np.cos(upper_leg_angle) * self.LEG_H / 2
            upper_leg_x = init_x - i * MAIN_BODY_BOTTOM_WIDTH / self.SCALE / 2 - upper_leg_x_distance
            upper_leg_y = init_y + upper_leg_y_distance - self.LEG_DOWN

            upper_leg = world.CreateDynamicBody(position=(upper_leg_x,
                                                          upper_leg_y),
                                                angle=upper_leg_angle,
                                                fixtures=LEG_FD)
            upper_leg.color1 = leg_color1
            upper_leg.color2 = leg_color2

            rjd = revoluteJointDef(
                bodyA=main_body,
                bodyB=upper_leg,
                anchor=(init_x - i * MAIN_BODY_BOTTOM_WIDTH / self.SCALE / 2,
                        init_y - self.LEG_DOWN),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=self.MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=-0.1 * np.pi,
                upperAngle=0.1 * np.pi,
            )

            upper_leg.userData = CustomBodyUserData(False, name="upper_leg")
            self.body_parts.append(upper_leg)

            joint_motor = world.CreateJoint(rjd)
            joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
            self.motors.append(joint_motor)

            ##### Second part of the leg #####
            middle_leg_angle = 0.7 * np.pi * i
            middle_leg_x_distance = np.sin(middle_leg_angle) * self.LEG_H / 2
            middle_leg_y_distance = -np.cos(middle_leg_angle) * self.LEG_H / 2
            middle_leg_x = upper_leg_x - upper_leg_x_distance - middle_leg_x_distance
            middle_leg_y = upper_leg_y + upper_leg_y_distance - middle_leg_y_distance
            middle_leg = world.CreateDynamicBody(position=(middle_leg_x,
                                                           middle_leg_y),
                                                 angle=middle_leg_angle,
                                                 fixtures=LEG_FD)
            middle_leg.color1 = leg_color1
            middle_leg.color2 = leg_color2

            rjd = revoluteJointDef(
                bodyA=upper_leg,
                bodyB=middle_leg,
                anchor=(upper_leg_x - upper_leg_x_distance,
                        upper_leg_y + upper_leg_y_distance),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=self.MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=-0.15 * np.pi,
                upperAngle=0.15 * np.pi,
            )

            middle_leg.userData = CustomBodyUserData(False, name="middle_leg")
            self.body_parts.append(middle_leg)

            joint_motor = world.CreateJoint(rjd)
            joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
            self.motors.append(joint_motor)

            ##### Third part of the leg #####
            lower_leg_angle = 0.9 * np.pi * i
            lower_leg_x_distance = np.sin(lower_leg_angle) * self.LEG_H / 2
            lower_leg_y_distance = -np.cos(lower_leg_angle) * self.LEG_H / 2
            lower_leg_x = middle_leg_x - middle_leg_x_distance - lower_leg_x_distance
            lower_leg_y = middle_leg_y - middle_leg_y_distance - lower_leg_y_distance
            lower_leg = world.CreateDynamicBody(position=(lower_leg_x,
                                                          lower_leg_y),
                                                angle=lower_leg_angle,
                                                fixtures=LOWER_FD)
            lower_leg.color1 = leg_color1
            lower_leg.color2 = leg_color2

            rjd = revoluteJointDef(
                bodyA=middle_leg,
                bodyB=lower_leg,
                anchor=(middle_leg_x - middle_leg_x_distance,
                        middle_leg_y - middle_leg_y_distance),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=self.MOTORS_TORQUE,
                motorSpeed=1,
                lowerAngle=-0.2 * np.pi,
                upperAngle=0.2 * np.pi,
            )

            lower_leg.userData = CustomBodyUserData(True, name="lower_leg")
            self.body_parts.append(lower_leg)

            joint_motor = world.CreateJoint(rjd)
            joint_motor.userData = CustomMotorUserData(SPEED_KNEE,
                                                       True,
                                                       contact_body=lower_leg)
            self.motors.append(joint_motor)
예제 #48
0
    def _reset(self):
        self._destroy()
        self.game_over = False
        self.prev_shaping = None

        W = VIEWPORT_W/SCALE
        H = VIEWPORT_H/SCALE

        # terrain
        CHUNKS = 11
        height = np.random.uniform(0, H/2, size=(CHUNKS+1,) )
        chunk_x  = [W/(CHUNKS-1)*i for i in xrange(CHUNKS)]
        self.helipad_x1 = chunk_x[CHUNKS//2-1]
        self.helipad_x2 = chunk_x[CHUNKS//2+1]
        self.helipad_y  = H/4
        height[CHUNKS//2-2] = self.helipad_y
        height[CHUNKS//2-1] = self.helipad_y
        height[CHUNKS//2+0] = self.helipad_y
        height[CHUNKS//2+1] = self.helipad_y
        height[CHUNKS//2+2] = self.helipad_y
        smooth_y = [0.33*(height[i-1] + height[i+0] + height[i+1]) for i in xrange(CHUNKS)]

        self.moon = self.world.CreateStaticBody( shapes=edgeShape(vertices=[(0, 0), (W, 0)]) )
        self.sky_polys = []
        for i in xrange(CHUNKS-1):
            p1 = (chunk_x[i],   smooth_y[i])
            p2 = (chunk_x[i+1], smooth_y[i+1])
            self.moon.CreateEdgeFixture(
                vertices=[p1,p2],
                density=0,
                friction=0.1)
            self.sky_polys.append( [p1, p2, (p2[0],H), (p1[0],H)] )

        self.moon.color1 = (0.0,0.0,0.0)
        self.moon.color2 = (0.0,0.0,0.0)

        initial_y = VIEWPORT_H/SCALE
        self.lander = self.world.CreateDynamicBody(
            position = (VIEWPORT_W/SCALE/2, initial_y),
            angle=0.0,
            fixtures = fixtureDef(
                shape=polygonShape(vertices=[ (x/SCALE,y/SCALE) for x,y in LANDER_POLY ]),
                density=5.0,
                friction=0.1,
                categoryBits=0x0010,
                maskBits=0x001,  # collide only with ground
                restitution=0.0) # 0.99 bouncy
                )
        self.lander.color1 = (0.5,0.4,0.9)
        self.lander.color2 = (0.3,0.3,0.5)
        self.lander.ApplyForceToCenter( (
            np.random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM),
            np.random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM)
            ), True)

        self.legs = []
        for i in [-1,+1]:
            leg = self.world.CreateDynamicBody(
                position = (VIEWPORT_W/SCALE/2 - i*LEG_AWAY/SCALE, initial_y),
                angle = (i*0.05),
                fixtures = fixtureDef(
                    shape=polygonShape(box=(LEG_W/SCALE, LEG_H/SCALE)),
                    density=1.0,
                    restitution=0.0,
                    categoryBits=0x0020,
                    maskBits=0x001)
                )
            leg.color1 = (0.5,0.4,0.9)
            leg.color2 = (0.3,0.3,0.5)
            rjd = revoluteJointDef(
                bodyA=self.lander,
                bodyB=leg,
                localAnchorA=(0, 0),
                localAnchorB=(i*LEG_AWAY/SCALE, LEG_DOWN/SCALE),
                enableMotor=True,
                enableLimit=True,
                maxMotorTorque=LEG_SPRING_TORQUE,
                motorSpeed=+0.3*i  # low enough not to jump back into the sky
                )
            if i==-1:
                rjd.lowerAngle = +0.9 - 0.5  # Yes, the most esoteric numbers here, angles legs have freedom to travel within
                rjd.upperAngle = +0.9
            else:
                rjd.lowerAngle = -0.9
                rjd.upperAngle = -0.9 + 0.5
            leg.joint = self.world.CreateJoint(rjd)
            self.legs.append(leg)

        self.drawlist = [self.lander] + self.legs

        return self._step(0)[0]
예제 #49
0
 def __init__(self, world, init_angle, init_x, init_y):
     self.world = world
     self.hull = self.world.CreateDynamicBody(
         position=(init_x, init_y),
         angle=init_angle,
         fixtures=[
             fixtureDef(
                 shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                              for x, y in HULL_POLY1]),
                 density=1.0,
             ),
             fixtureDef(
                 shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                              for x, y in HULL_POLY2]),
                 density=1.0,
             ),
             fixtureDef(
                 shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                              for x, y in HULL_POLY3]),
                 density=1.0,
             ),
             fixtureDef(
                 shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                              for x, y in HULL_POLY4]),
                 density=1.0,
             ),
         ],
     )
     self.hull.color = (0.8, 0.0, 0.0)
     self.wheels = []
     self.fuel_spent = 0.0
     WHEEL_POLY = [
         (-WHEEL_W, +WHEEL_R),
         (+WHEEL_W, +WHEEL_R),
         (+WHEEL_W, -WHEEL_R),
         (-WHEEL_W, -WHEEL_R),
     ]
     for wx, wy in WHEELPOS:
         front_k = 1.0 if wy > 0 else 1.0
         w = self.world.CreateDynamicBody(
             position=(init_x + wx * SIZE, init_y + wy * SIZE),
             angle=init_angle,
             fixtures=fixtureDef(
                 shape=polygonShape(vertices=[(x * front_k * SIZE,
                                               y * front_k * SIZE)
                                              for x, y in WHEEL_POLY]),
                 density=0.1,
                 categoryBits=0x0020,
                 maskBits=0x001,
                 restitution=0.0,
             ),
         )
         w.wheel_rad = front_k * WHEEL_R * SIZE
         w.color = WHEEL_COLOR
         w.gas = 0.0
         w.brake = 0.0
         w.steer = 0.0
         w.phase = 0.0  # wheel angle
         w.omega = 0.0  # angular velocity
         w.skid_start = None
         w.skid_particle = None
         rjd = revoluteJointDef(
             bodyA=self.hull,
             bodyB=w,
             localAnchorA=(wx * SIZE, wy * SIZE),
             localAnchorB=(0, 0),
             enableMotor=True,
             enableLimit=True,
             maxMotorTorque=180 * 900 * SIZE * SIZE,
             motorSpeed=0,
             lowerAngle=-0.4,
             upperAngle=+0.4,
         )
         w.joint = self.world.CreateJoint(rjd)
         w.tiles = set()
         w.userData = w
         self.wheels.append(w)
     self.drawlist = self.wheels + [self.hull]
     self.particles = []
예제 #50
0
def convex_box2d_shape_from_shapely(geometry):
    """Create a Box2D shape from the convex hull of shapely geometry"""
    return b2.polygonShape(
        vertices=list(geometry.convex_hull.exterior.coords))
예제 #51
0
VIEWPORT_W = 600
VIEWPORT_H = 400

TERRAIN_STEP = 14 / SCALE
TERRAIN_LENGTH = 200  # in steps
TERRAIN_HEIGHT = VIEWPORT_H / SCALE / 4
TERRAIN_GRASS = 10  # low long are grass spots, in steps
TERRAIN_STARTPAD = 20  # in steps
FRICTION = 2.5

BIPED_LIMIT = 1600
BIPED_HARDCORE_LIMIT = 2000

HULL_FD = fixtureDef(
    shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
                                 for x, y in HULL_POLY]),
    density=5.0,
    friction=0.1,
    categoryBits=0x0020,
    maskBits=0x001,  # collide only with ground
    restitution=0.0)  # 0.99 bouncy

LEG_FD = fixtureDef(shape=polygonShape(box=(LEG_W / 2, LEG_H / 2)),
                    density=1.0,
                    restitution=0.0,
                    categoryBits=0x0020,
                    maskBits=0x001)

LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * LEG_W / 2, LEG_H / 2)),
                      density=1.0,
                      restitution=0.0,
예제 #52
0
    def _generate_terrain(self, hardcore):
        GRASS, STUMP, STAIRS, PIT, _STATES_ = range(5)
        state    = GRASS
        velocity = 0.0
        y        = TERRAIN_HEIGHT
        counter  = TERRAIN_STARTPAD
        oneshot  = False
        self.terrain   = []
        self.terrain_x = []
        self.terrain_y = []
        for i in range(TERRAIN_LENGTH):
            x = i*TERRAIN_STEP
            self.terrain_x.append(x)

            if state==GRASS and not oneshot:
                velocity = 0.8*velocity + 0.01*np.sign(TERRAIN_HEIGHT - y)
                if i > TERRAIN_STARTPAD: velocity += np.random.uniform(-1, 1)/SCALE   #1
                y += velocity

            elif state==PIT and oneshot:
                counter = np.random.randint(3, 5)
                poly = [
                    (x,              y),
                    (x+TERRAIN_STEP, y),
                    (x+TERRAIN_STEP, y-4*TERRAIN_STEP),
                    (x,              y-4*TERRAIN_STEP),
                    ]
                t = self.world.CreateStaticBody(
                    fixtures = fixtureDef(
                        shape=polygonShape(vertices=poly),
                        friction = FRICTION
                    ))
                t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
                self.terrain.append(t)
                t = self.world.CreateStaticBody(
                    fixtures = fixtureDef(
                        shape=polygonShape(vertices=[(p[0]+TERRAIN_STEP*counter,p[1]) for p in poly]),
                        friction = FRICTION
                    ))
                t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
                self.terrain.append(t)
                counter += 2
                original_y = y

            elif state==PIT and not oneshot:
                y = original_y
                if counter > 1:
                    y -= 4*TERRAIN_STEP

            elif state==STUMP and oneshot:
                counter = np.random.randint(1, 3)
                poly = [
                    (x,                      y),
                    (x+counter*TERRAIN_STEP, y),
                    (x+counter*TERRAIN_STEP, y+counter*TERRAIN_STEP),
                    (x,                      y+counter*TERRAIN_STEP),
                    ]
                t = self.world.CreateStaticBody(
                    fixtures = fixtureDef(
                        shape=polygonShape(vertices=poly),
                        friction = FRICTION
                    ))
                t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
                self.terrain.append(t)

            elif state==STAIRS and oneshot:
                stair_height = +1 if np.random.ranf() > 0.5 else -1
                stair_width = np.random.randint(4, 5)
                stair_steps = np.random.randint(3, 5)
                original_y = y
                for s in range(stair_steps):
                    poly = [
                        (x+(    s*stair_width)*TERRAIN_STEP, y+(   s*stair_height)*TERRAIN_STEP),
                        (x+((1+s)*stair_width)*TERRAIN_STEP, y+(   s*stair_height)*TERRAIN_STEP),
                        (x+((1+s)*stair_width)*TERRAIN_STEP, y+(-1+s*stair_height)*TERRAIN_STEP),
                        (x+(    s*stair_width)*TERRAIN_STEP, y+(-1+s*stair_height)*TERRAIN_STEP),
                        ]
                    t = self.world.CreateStaticBody(
                        fixtures = fixtureDef(
                            shape=polygonShape(vertices=poly),
                            friction = FRICTION
                        ))
                    t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
                    self.terrain.append(t)
                counter = stair_steps*stair_width

            elif state==STAIRS and not oneshot:
                s = stair_steps*stair_width - counter - stair_height
                n = s/stair_width
                y = original_y + (n*stair_height)*TERRAIN_STEP

            oneshot = False
            self.terrain_y.append(y)
            counter -= 1
            if counter==0:
                counter = np.random.randint(TERRAIN_GRASS/2, TERRAIN_GRASS)
                if state==GRASS and hardcore:
                    state = np.random.randint(1, _STATES_)
                    oneshot = True
                else:
                    state = GRASS
                    oneshot = True

        self.terrain_poly = []
        for i in range(TERRAIN_LENGTH-1):
            poly = [
                (self.terrain_x[i],   self.terrain_y[i]),
                (self.terrain_x[i+1], self.terrain_y[i+1])
                ]
            t = self.world.CreateStaticBody(
                fixtures = fixtureDef(
                    shape=edgeShape(vertices=poly),
                    friction = FRICTION,
                    categoryBits=0x0001,
                ))
            color = (0.3, 1.0 if i%2==0 else 0.8, 0.3)
            t.color1 = color
            t.color2 = color
            self.terrain.append(t)
            color = (0.4, 0.6, 0.3)
            poly += [ (poly[1][0], 0), (poly[0][0], 0) ]
            self.terrain_poly.append( (poly, color) )
        self.terrain.reverse()
예제 #53
0
    def __init__(self,
                 world,
                 init_angle,
                 init_x,
                 init_y,
                 variable_speed=False):
        self.world = world
        self.grass_friction = 1.0
        self.variable_speed = variable_speed

        # Create car hull
        self.hull = self.world.CreateDynamicBody(
            position=(init_x, init_y),
            angle=(init_angle),
            fixtures=[
                fixtureDef(
                    shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                                 for x, y in HULL_POLY1]),
                    density=1.0),
                fixtureDef(
                    shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                                 for x, y in HULL_POLY2]),
                    density=1.0),
                fixtureDef(
                    shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                                 for x, y in HULL_POLY3]),
                    density=1.0),
                fixtureDef(
                    shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                                 for x, y in HULL_POLY4]),
                    density=1.0)
            ])
        self.hull.color = (0.8, 0, 0)

        # Create wheels
        self.wheels = []
        for wx, wy in WHEELPOS:
            # First two wheels are front wheels
            # Next two are rear wheels
            w = self.world.CreateDynamicBody(
                position=(init_x + wx * SIZE, init_y + wy * SIZE),
                angle=init_angle,
                fixtures=fixtureDef(
                    shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
                                                 for x, y in WHEEL_POLY]),
                    density=0.1,
                    categoryBits=0x0020,
                    maskBits=0x001,
                    restitution=0.0))
            w.front = True if wy > 0 else False
            w.wheel_rad = WHEEL_R * SIZE
            w.color = WHEEL_COLOR
            w.speed = 0.0  # linear speed
            w.steering_angle = 0.0  # steering angle
            w.omega = 0.0  # angular speed
            # Skid
            w.skid_particle = None
            w.skid_start = None
            # Revolute joint to model steering
            rjd = revoluteJointDef(bodyA=self.hull,
                                   bodyB=w,
                                   localAnchorA=(wx * SIZE, wy * SIZE),
                                   localAnchorB=(0, 0),
                                   enableMotor=True,
                                   enableLimit=True,
                                   maxMotorTorque=180 * 900 * SIZE**2,
                                   motorSpeed=0,
                                   lowerAngle=-0.4,
                                   upperAngle=+0.4)
            w.joint = self.world.CreateJoint(rjd)
            w.tiles = set()
            w.userData = w
            self.wheels.append(w)

        self.drawlist = self.wheels + [self.hull]
        self.particles = []
예제 #54
0
파일: car_racing.py 프로젝트: jiapei100/gym
    def _create_track(self):
        CHECKPOINTS = 12

        # Create checkpoints
        checkpoints = []
        for c in range(CHECKPOINTS):
            alpha = 2*math.pi*c/CHECKPOINTS + self.np_random.uniform(0, 2*math.pi*1/CHECKPOINTS)
            rad = self.np_random.uniform(TRACK_RAD/3, TRACK_RAD)
            if c==0:
                alpha = 0
                rad = 1.5*TRACK_RAD
            if c==CHECKPOINTS-1:
                alpha = 2*math.pi*c/CHECKPOINTS
                self.start_alpha = 2*math.pi*(-0.5)/CHECKPOINTS
                rad = 1.5*TRACK_RAD
            checkpoints.append( (alpha, rad*math.cos(alpha), rad*math.sin(alpha)) )

        #print "\n".join(str(h) for h in checkpoints)
        #self.road_poly = [ (    # uncomment this to see checkpoints
        #    [ (tx,ty) for a,tx,ty in checkpoints ],
        #    (0.7,0.7,0.9) ) ]
        self.road = []

        # Go from one checkpoint to another to create track
        x, y, beta = 1.5*TRACK_RAD, 0, 0
        dest_i = 0
        laps = 0
        track = []
        no_freeze = 2500
        visited_other_side = False
        while 1:
            alpha = math.atan2(y, x)
            if visited_other_side and alpha > 0:
                laps += 1
                visited_other_side = False
            if alpha < 0:
                visited_other_side = True
                alpha += 2*math.pi
            while True: # Find destination from checkpoints
                failed = True
                while True:
                    dest_alpha, dest_x, dest_y = checkpoints[dest_i % len(checkpoints)]
                    if alpha <= dest_alpha:
                        failed = False
                        break
                    dest_i += 1
                    if dest_i % len(checkpoints) == 0: break
                if not failed: break
                alpha -= 2*math.pi
                continue
            r1x = math.cos(beta)
            r1y = math.sin(beta)
            p1x = -r1y
            p1y = r1x
            dest_dx = dest_x - x  # vector towards destination
            dest_dy = dest_y - y
            proj = r1x*dest_dx + r1y*dest_dy  # destination vector projected on rad
            while beta - alpha >  1.5*math.pi: beta -= 2*math.pi
            while beta - alpha < -1.5*math.pi: beta += 2*math.pi
            prev_beta = beta
            proj *= SCALE
            if proj >  0.3: beta -= min(TRACK_TURN_RATE, abs(0.001*proj))
            if proj < -0.3: beta += min(TRACK_TURN_RATE, abs(0.001*proj))
            x += p1x*TRACK_DETAIL_STEP
            y += p1y*TRACK_DETAIL_STEP
            track.append( (alpha,prev_beta*0.5 + beta*0.5,x,y) )
            if laps > 4: break
            no_freeze -= 1
            if no_freeze==0: break
        #print "\n".join([str(t) for t in enumerate(track)])

        # Find closed loop range i1..i2, first loop should be ignored, second is OK
        i1, i2 = -1, -1
        i = len(track)
        while True:
            i -= 1
            if i==0: return False  # Failed
            pass_through_start = track[i][0] > self.start_alpha and track[i-1][0] <= self.start_alpha
            if pass_through_start and i2==-1:
                i2 = i
            elif pass_through_start and i1==-1:
                i1 = i
                break
        if self.verbose == 1:
            print("Track generation: %i..%i -> %i-tiles track" % (i1, i2, i2-i1))
        assert i1!=-1
        assert i2!=-1

        track = track[i1:i2-1]

        first_beta = track[0][1]
        first_perp_x = math.cos(first_beta)
        first_perp_y = math.sin(first_beta)
        # Length of perpendicular jump to put together head and tail
        well_glued_together = np.sqrt(
            np.square( first_perp_x*(track[0][2] - track[-1][2]) ) +
            np.square( first_perp_y*(track[0][3] - track[-1][3]) ))
        if well_glued_together > TRACK_DETAIL_STEP:
            return False

        # Red-white border on hard turns
        border = [False]*len(track)
        for i in range(len(track)):
            good = True
            oneside = 0
            for neg in range(BORDER_MIN_COUNT):
                beta1 = track[i-neg-0][1]
                beta2 = track[i-neg-1][1]
                good &= abs(beta1 - beta2) > TRACK_TURN_RATE*0.2
                oneside += np.sign(beta1 - beta2)
            good &= abs(oneside) == BORDER_MIN_COUNT
            border[i] = good
        for i in range(len(track)):
            for neg in range(BORDER_MIN_COUNT):
                border[i-neg] |= border[i]

        # Create tiles
        for i in range(len(track)):
            alpha1, beta1, x1, y1 = track[i]
            alpha2, beta2, x2, y2 = track[i-1]
            road1_l = (x1 - TRACK_WIDTH*math.cos(beta1), y1 - TRACK_WIDTH*math.sin(beta1))
            road1_r = (x1 + TRACK_WIDTH*math.cos(beta1), y1 + TRACK_WIDTH*math.sin(beta1))
            road2_l = (x2 - TRACK_WIDTH*math.cos(beta2), y2 - TRACK_WIDTH*math.sin(beta2))
            road2_r = (x2 + TRACK_WIDTH*math.cos(beta2), y2 + TRACK_WIDTH*math.sin(beta2))
            t = self.world.CreateStaticBody( fixtures = fixtureDef(
                shape=polygonShape(vertices=[road1_l, road1_r, road2_r, road2_l])
                ))
            t.userData = t
            c = 0.01*(i%3)
            t.color = [ROAD_COLOR[0] + c, ROAD_COLOR[1] + c, ROAD_COLOR[2] + c]
            t.road_visited = False
            t.road_friction = 1.0
            t.fixtures[0].sensor = True
            self.road_poly.append(( [road1_l, road1_r, road2_r, road2_l], t.color ))
            self.road.append(t)
            if border[i]:
                side = np.sign(beta2 - beta1)
                b1_l = (x1 + side* TRACK_WIDTH        *math.cos(beta1), y1 + side* TRACK_WIDTH        *math.sin(beta1))
                b1_r = (x1 + side*(TRACK_WIDTH+BORDER)*math.cos(beta1), y1 + side*(TRACK_WIDTH+BORDER)*math.sin(beta1))
                b2_l = (x2 + side* TRACK_WIDTH        *math.cos(beta2), y2 + side* TRACK_WIDTH        *math.sin(beta2))
                b2_r = (x2 + side*(TRACK_WIDTH+BORDER)*math.cos(beta2), y2 + side*(TRACK_WIDTH+BORDER)*math.sin(beta2))
                self.road_poly.append(( [b1_l, b1_r, b2_r, b2_l], (1,1,1) if i%2==0 else (1,0,0) ))
        self.track = track
        return True
예제 #55
0
    ]
LEG_DOWN = -8/SCALE
LEG_W, LEG_H = 8/SCALE, 34/SCALE

VIEWPORT_W = 600
VIEWPORT_H = 400

TERRAIN_STEP   = 14/SCALE
TERRAIN_LENGTH = 200     # in steps
TERRAIN_HEIGHT = VIEWPORT_H/SCALE/4
TERRAIN_GRASS    = 10    # low long are grass spots, in steps
TERRAIN_STARTPAD = 20    # in steps
FRICTION = 2.5

HULL_FD = fixtureDef(
                shape=polygonShape(vertices=[ (x/SCALE,y/SCALE) for x,y in HULL_POLY ]),
                density=5.0,
                friction=0.1,
                categoryBits=0x0020,
                maskBits=0x001,  # collide only with ground
                restitution=0.0) # 0.99 bouncy

LEG_FD = fixtureDef(
                    shape=polygonShape(box=(LEG_W/2, LEG_H/2)),
                    density=1.0,
                    restitution=0.0,
                    categoryBits=0x0020,
                    maskBits=0x001)

LOWER_FD = fixtureDef(
                    shape=polygonShape(box=(0.8*LEG_W/2, LEG_H/2)),