def __init__(self, world, img, pos, rotation, shape, static=False, scale=1): img = pygame.transform.rotozoom(img, 0, scale) self.img = img angle = rotation * (pi / 180) self.shape = shape if static: self.body = world.CreateStaticBody(position=(pos), angle=angle) else: self.body = world.CreateDynamicBody(position=(pos), fixedRotation=False, angle=angle) if shape == CIRCLE: radius = self.img.get_rect().width * .8 / 2 / PPM self.fix = self.body.CreateCircleFixture(radius=radius, density=5, friction=0.3, restitution=.5) self.radius = radius elif shape == BOX: dimensions = (self.img.get_rect().width / (2 * PPM), self.img.get_rect().height / (2 * PPM)) self.fix = self.body.CreatePolygonFixture(box=dimensions, density=4, friction=0.3, restitution=.5)
def __init__(self, *group): super().__init__(*group) self.radius = radius self.rect = self.image.get_rect() body = world.CreateDynamicBody(position=(20, 20)) self.circle = body.CreateCircleFixture(radius=1, density=1, friction=0.3) self.rect = pygame.Rect(20, 20, 260, 260)
def __init__(self, world, x=10, y=15, color=None): if color is None: color = (255, 255, 0, 255) self.body = world.CreateDynamicBody(position=(x, y), linearDamping=1) self.fixture = self.body.CreateCircleFixture(radius=0.35, density=1, friction=0.3) self.circle = self.fixture.shape self.base_color = color self.color = color self.prev_force = None
def __init__(self, all_sprites, y, post): super().__init__(all_sprites) self.post1 = post self.post = world.CreateDynamicBody(position=(33, 40 - y - 5), shapes=polygonShape(box=(2, 2)), linearVelocity=(-5, 0), gravityScale=0) self.image = load_image("plume.png", -1) self.rect = pygame.Rect(630, (y + 3) * PPM, 40, 40) self.mask = pygame.mask.from_surface(self.image)
def load_chain(world, data, dynamic): chain = [] for ind in range(len(data)): row = data[ind] circle = circleShape(pos=(0,0), radius=0.001) if dynamic: new_body = world.CreateDynamicBody(position=row, userData=ind) new_body.CreateFixture(fixtureDef(shape=circle, density=1, friction=0.3)) # if ind > 0: # world.CreateRevoluteJoint(bodyA=chain[ind-1], bodyB=new_body, anchor=chain[ind-1].worldCenter) else: new_body = world.CreateStaticBody(position=row, shapes=[circle]) chain.append(new_body) return chain
def __init__(self, world, showcase, pos, size, angle=0, static=False): if static: self.body = world.CreateStaticBody( position=pos, shapes=polygonShape(box=size), ) else: self.body = world.CreateDynamicBody(position=pos, angle=angle) self.body.CreatePolygonFixture(box=size, density=1, friction=0.3) points = self.body.fixtures[0].shape.vertices index = (0, 1, 3, 1, 2, 3) self.figure = graphicsHelper.createMesh(points, index) showcase.add(self.figure)
def __init__(self, all_sprites, y): """Инициализация объекта""" super().__init__(all_sprites) self.y = y self.post = world.CreateDynamicBody(position=(33, 40), shapes=polygonShape(box=(3, y)), linearVelocity=(-5, 0), gravityScale=0) # self.image = pygame.Surface((60, y * PPM), pygame.SRCALPHA, 32) # pygame.draw.rect(self.image, pygame.Color(0, 255, 0), (0, 0, 60, y * PPM)) self.image = load_image("postdown.png", -1).subsurface( pygame.Rect(0, 800 - y * PPM, 60, y * PPM)) self.image.set_colorkey("BLACK") self.rect = pygame.Rect(630, 0, 60, y * PPM) self.mask = pygame.mask.from_surface(self.image) self.wascount = False
def __init__(self, img, pos, rotation, shape, static=False, scale=1, density=1): img = pygame.transform.rotozoom(img, 0, scale) self.img = img angle = rotation * (pi / 180) self.shape = shape self.contact_impulse = 0 self.min_impulse = 2.0 if static: self.body = world.CreateStaticBody(position=(pos), angle=angle, userData=self) else: self.body = world.CreateDynamicBody(position=(pos), fixedRotation=False, angle=angle, userData=self) if shape == CIRCLE: radius = self.img.get_rect().width * .6 / 2 / PPM self.fix = self.body.CreateCircleFixture(radius=radius, density=density, friction=0.3, restitution=.5) self.radius = radius elif shape == BOX: dimensions = (self.img.get_rect().width / (2 * PPM), self.img.get_rect().height / (2 * PPM)) self.fix = self.body.CreatePolygonFixture(box=dimensions, density=density, friction=0.3, restitution=.5) elif shape == RIGHT_TRIANGLE: rect = self.img.get_rect() width = rect.width / PPM height = rect.height / PPM vertices = [(-width / 2, -height / 2), (width / 2, -height / 2), (-width / 2, height / 2)] self.fix = self.body.CreatePolygonFixture(density=density, vertices=vertices, friction=0.3, restitution=.5)
def __init__(self, all_sprites): """Инициализация объекта""" super().__init__(all_sprites) self.body = world.CreateDynamicBody(position=(5, 20)) circle = self.body.CreateCircleFixture(radius=1, density=1, friction=0, categoryBits=3, maskBits=2) self.radius = 20 # self.image = pygame.Surface((62, 48), pygame.SRCALPHA, 32) self.sprites = [ load_image("bird1.png", -1), load_image("bird2.png", -1), load_image("bird3.png", -1), load_image("bird2.png", -1) ] self.an = 0 self.image = self.sprites[self.an] # pygame.draw.circle(self.image, pygame.Color("red"), (20, 20), 20) self.rect = pygame.Rect(80, 380, 61, 44) self.mask = pygame.mask.from_surface(self.image)
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 * 1 * 9.8, angularDamping=0.3 * 1 / 12 * 9.8) obs_body = world.CreateDynamicBody(position=(18, 12), angle=0, linearDamping=0.5 * 4 * 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) obs_box = obs_body.CreatePolygonFixture(box=(2, 2), 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_body: " + str(obs_body.mass) + " kg , " + str(obs_body.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), (255, 255, 255, 255), (255, 255, 255, 255)] bodies = [ground_body, des_body, obs_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)) # --- 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 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] 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 # Collect data from these bodies KEx_des, KEy_des = KE(des_body) KEx_obs, KEy_obs = KE(obs_body) 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) # 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
# And a static body to hold the ground shape ground_body = world.CreateStaticBody( position=(0, 0), shapes=polygonShape(box=(50, 1)), ) BODY_DIM = (2, 1) BODY_POS = (10, 11) LEG_INNER = 0.25 LEG_DIM = (0.25, 2) LEGA_POS = (BODY_POS[0]-BODY_DIM[0]+LEG_DIM[0], BODY_POS[1]-BODY_DIM[1]+LEG_INNER) LEGB_POS = (BODY_POS[0]+BODY_DIM[0]-LEG_DIM[0], BODY_POS[1]-BODY_DIM[1]+LEG_INNER) boxbody = world.CreateDynamicBody(position=BODY_POS, angle=0) box = boxbody.CreatePolygonFixture(box=BODY_DIM, density=1, friction=0.3) legabody = world.CreateDynamicBody(position=LEGA_POS, angle=(-45/2)*DEGTORAD) lega = legabody.CreatePolygonFixture(box=LEG_DIM, density=1, friction=0.3) lowerlegabody = world.CreateDynamicBody(position=(LEGA_POS[0], LEGA_POS[1]-LEG_DIM[1])) lowerlega = lowerlegabody.CreatePolygonFixture(box=LEG_DIM, density=1, friction=0.3) legbbody = world.CreateDynamicBody(position=LEGB_POS, angle=(+45/2)*DEGTORAD) legb = legbbody.CreatePolygonFixture(box=LEG_DIM, density=1, friction=0.3) lowerlegbbody = world.CreateDynamicBody(position=(LEGB_POS[0], LEGB_POS[1]-LEG_DIM[1])) lowerlegb = lowerlegbbody.CreatePolygonFixture(box=LEG_DIM, density=1, friction=0.3) # legalowerbody = world.CreateDynamicBody(position=(11.5, 8)) # legalower = legalowerbody.CreatePolygonFixture(box=(0.25, 2), density=1, friction=0.3)
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 --- world = world(gravity=(0, 0), doSleep=True) bodies = [] #track setup outer_track = world.CreateBody(shapes=chainShape(vertices=[(3,3), (int(SCREEN_WIDTH/PPM)-3, 3),(int(SCREEN_WIDTH/PPM)-3,int(SCREEN_HEIGHT/PPM)-3),(3,int(SCREEN_HEIGHT/PPM)-3)])) bodies.append([outer_track,'white']) inner_track = world.CreateBody(shapes=chainShape(vertices=[(17,int(SCREEN_HEIGHT/PPM)-17),(int(SCREEN_WIDTH/PPM)-17,int(SCREEN_HEIGHT/PPM)-17),(int(SCREEN_WIDTH/PPM)-17, 17),(17,17)])) bodies.append([inner_track,'gray']) # Create car car = world.CreateDynamicBody(position=(10, 15), angle= -np.pi/2, linearDamping=.4, angularDamping = 6) bodies.append([car,'blue']) box = car.CreatePolygonFixture(box=(1, .5), density=4, friction=0.2) box.massData.center = vec2(0,-10) #set center of mass pressed_keys = [] time_pressed = { 'w' : 0.0, 'a' : 0.0, 's' : 0.0, 'd' : 0.0 } #for whiskers # angles, lengths, intercept, normals rays = [[-np.pi/2, 10, 0, 0],
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, 1), shapes=polygonShape(box=(50, 5)), ) # Create a dynamic body dynamic_body = world.CreateDynamicBody(position=(10, 15), angle=15) # And add a box fixture onto it (with a nonzero density, so it will move) box = dynamic_body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3) colors = { staticBody: (255, 255, 255, 255), dynamicBody: (127, 127, 127, 255), } # --- main game loop --- running = True while running: # Check the event queue for event in pygame.event.get(): if event.type == QUIT or (event.type == KEYDOWN
x0 = 10 y0 = 10 s = 1 total_weight = 10 masses = [] joints = [] min_dists = [] with open("bodies/quad.obj") as f: for line in f: if line.startswith("v "): x, y, _ = [float(loc) for loc in line[2:].split(" ")] body = world.CreateDynamicBody( position=(x0 + s*x, y0 + s*y), fixedRotation=True) circle = body.CreateCircleFixture( radius=0.3, density=1, friction=0.4) masses.append(body) min_dists.append(1) elif line.startswith("f "): indices = [int(idx)-1 for idx in line[2:].split(" ")] for i in range(3): a = masses[indices[i]] b = masses[indices[(i+1) % 3]] l = math.hypot(a.position[0]-b.position[0], a.position[1]-b.position[1]) min_dists[indices[i]] = min(l, min_dists[indices[i]]) min_dists[indices[(i+1) % 3]] = min(l, min_dists[indices[(i+1) % 3]])
# And a static body to hold the ground shape ground_body = world.CreateStaticBody( position=(0, 1), shapes=polygonShape(box=(50, 5)), ) ''' # Create a dynamic body dynamic_body = world.CreateDynamicBody(position=(10, 15), angle=15) # And add a box fixture onto it (with a nonzero density, so it will move) box = dynamic_body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3) ''' #body = world.CreateDynamicBody(position=(20, 45)) #circle = body.CreateCircleFixture(radius=0.5, density=1, friction=0.3) body = world.CreateDynamicBody(position=(20, 35)) circle = body.CreatePolygonFixture(box=(3, 1), density=1, friction=0.3) for x in range(0, 10): body = world.CreateDynamicBody(position=(30 - x * 2, 35 + x * 10), angle=15) box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3) body = world.CreateDynamicBody(position=(30, 35), angle=15) box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3) body = world.CreateDynamicBody(position=(30, 25), angle=15) box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3) body = world.CreateDynamicBody(position=(10, 15), angle=45) box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
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, 45)) circle = body.CreateCircleFixture(radius=0.5, density=1, friction=0.3) body = world.CreateDynamicBody(position=(30, 45), angle=15) box = body.CreatePolygonFixture(box=(2, 1), 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. def my_draw_polygon(polygon, body, fixture): vertices = [(body.transform * v) * PPM for v in polygon.vertices]
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=(20, 5), angle=0) fng2 = world.CreateKinematicBody(position=(20 + width, 5), angle=0)
def create_circle(): for i in range(5, 40, 5): """Создаем круги""" body = world.CreateDynamicBody(position=(i, 20)) circle = body.CreateCircleFixture(radius=1, density=1, friction=0.3)
def build(self, world, x0, y0): width = self.genome['width'] height = self.genome['height'] length1 = self.genome['right_leg'] length2 = self.genome['left_leg'] """ construct body, head, and neck """ body = world.CreateDynamicBody(position=(x0, y0)) body.fixedRotation = True body.CreatePolygonFixture(box=(width, height), density=1, friction=0.3) head = world.CreateDynamicBody(position=(x0, y0 + 4)) head.fixedRotation = True head.CreateCircleFixture(radius=1.8, density=1, friction=0.3) neck = world.CreateDynamicBody(position=(x0, y0 + 2.5)) neck.fixedRotation = True neck.CreatePolygonFixture(box=(0.5, 1), density=1, friction=WHEEL_FRICTION) # # for comments, " for docstrings # Connect the body with the joints, but rotation is not allowed. world.CreateRevoluteJoint(bodyA=head, bodyB=neck, anchor=head.worldCenter, enableMotor=False) world.CreateRevoluteJoint(bodyA=neck, bodyB=body, anchor=body.worldCenter, enableMotor=False) # Construct the leg and connect it with the body right_leg = world.CreateDynamicBody(position=(x0 + 1, y0 - 2)) right_leg.CreatePolygonFixture(box=(0.5, 3), density=1, friction=WHEEL_FRICTION) world.CreateRevoluteJoint(bodyA=body, bodyB=right_leg, anchor=body.worldCenter + (1, -2), lowerAngle=-0.3 * pi, upperAngle=0.3 * pi, enableLimit=True, maxMotorTorque=MAX_TORQUE, motorSpeed=cos(SPEED), enableMotor=True) left_leg = world.CreateDynamicBody(position=(x0 - 1, y0 - 2)) left_leg.CreatePolygonFixture(box=(0.5, 3), density=1, friction=WHEEL_FRICTION) world.CreateRevoluteJoint(bodyA=body, bodyB=left_leg, anchor=body.worldCenter + (-1, -2), lowerAngle=-0.3 * pi, upperAngle=0.3 * pi, enableLimit=True, maxMotorTorque=MAX_TORQUE, motorSpeed=sin(SPEED), enableMotor=True) print(width, height) self.bodies = [body, head, neck, right_leg, left_leg] self.tracker = body.worldCenter # return the body object for tracking position return body
# border_body3 = world.CreateStaticBody(position=(0.59,0), shapes=border_shape_v) # border_body4 = world.CreateStaticBody(position=(-0.6,0), shapes=border_shape_v) # env = [env_body, border_body1, border_body2, border_body3, border_body4] # env = [env_body] env = [] # create dynamic bodies snowflakes = [] for ind in range(len(data)): row = data[ind] circle = circleShape(pos=(0, 0), radius=SNOWBALL_RADIUS) snowflake_body = world.CreateDynamicBody(position=row, userData=ind) fixture = snowflake_body.CreateFixture( fixtureDef(shape=circle, density=1, friction=0.3)) snowflakes.append(snowflake_body) force_magnitude = 1 force_decrement = force_magnitude / (end_frame - start_frame - 1) # force_decrement = 0 force_vec = vec2(0.0001, 0) # main game loop running = True frame = start_frame while running: # Check the event queue for event in pygame.event.get():
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=(3.2,1.0), shapes=polygonShape(box=(3.0, .2)), ) # Create a dynamic body dynamic_body = world.CreateDynamicBody(position=(3.0,3.0), angle=45) # And add a box fixture onto it (with a nonzero density, so it will move) box = dynamic_body.CreatePolygonFixture(box=(.1, .1), density=9, friction=0.7, restitution=0.89) colors = { staticBody: (255, 255, 255, 255), dynamicBody: (127, 127, 127, 255), } # --- main game loop --- running = True while running: # Check the event queue for event in pygame.event.get(): if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE):
i = 0 while callback.hit == False: world.RayCast(callback, (i - 10, -10), (i - 10, SCREEN_HEIGHT / PPM)) i += 3 if callback.fixture.userData != 'outer': print('clockwise track detected. Initiating corrections') temp = outer_track outer_track = inner_track inner_track = temp tracks.append([outer_track, 'white']) tracks.append([inner_track, 'lightblue']) #make a box for the mouse and move it around car = world.CreateDynamicBody(position=(200 / PPM, 200 / PPM), angle=0, linearDamping=.3, angularDamping=6) car_color = 'blue' box = car.CreatePolygonFixture(box=(1, .5), density=1, friction=0.002) pygame.init() screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32) pygame.display.set_caption('Drifter Evolver') clock = pygame.time.Clock() def step(): world.Step(1.0 / 60.0, 10, 10) for event in pygame.event.get(): if event.type == QUIT:
(0 , 0)]) ground_body = world.CreateStaticBody( position=(0, 0), shapes=(polygonShape(box=(100, 5))), ) quakeFloor = world.CreateKinematicBody(position=(1 , 6)) quakeFloor.CreateFixture(shape=Box2D.b2PolygonShape(vertices=[(0 , 0), (0 , 1), (40 , 1), (40 , 0)]) ,friction=4) dynamic_body = world.CreateDynamicBody(position=(10, 15), angle=math.radians(90)) box = dynamic_body.CreatePolygonFixture(box=blockSize, density=1, friction=0.3) colors = { staticBody: (255, 255, 255, 255), dynamicBody: (127, 127, 127, 255), kinematicBody: (250 , 128 , 0 , 255) } maxForces = {} font = pygame.font.Font(None , 24) fontLarge = pygame.font.Font(None , 80) ###
def simulate(cmd, trj, na, color): # Create dynamic bodies des_body = world.CreateDynamicBody(position=(db_init[0], db_init[1]), angle=db_init[2], linearDamping=0.5 * 1 * 9.8, angularDamping=0.3 * 1 / 12 * 9.8) obs1_body = world.CreateDynamicBody(position=(o1_init[0], o1_init[1]), angle=o1_init[2], linearDamping=0.5 * 18 * 9.8, angularDamping=0.3 * 4 / 12 * 9.8) obs2_body = world.CreateDynamicBody(position=(o2_init[0], o2_init[1]), angle=o2_init[2], linearDamping=0.5 * 24 * 9.8, angularDamping=0.3 * 4 / 12 * 9.8) # Create Gripper gripper = world.CreateKinematicBody(position=(18, 5), angle=0) # Add polygon fixtures for objects des_fixt = des_body.CreatePolygonFixture(box=(1, 1), density=1, friction=0.3, restitution=0.8) obs1_box = obs1_body.CreatePolygonFixture(box=(1.5, 3), density=1, friction=0.3, restitution=0.8) obs2_box = obs2_body.CreatePolygonFixture(box=(3, 2), 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) # gripperbase = polygonShape(vertices=[(-w/2,0), (-w/2,l), (-ow/2,l), # (-ow/2,l+lf), (-iw/2,l+lf), (-iw/2,l+lf-lt), (iw/2,l+lf-lt), (iw/2,l+lf), (ow/2,l+lf), # (ow/2,l), (w/2,l), (w/2,0)]) gripperbase = gripper.CreatePolygonFixture(box=(w / 2, l / 2), density=1, friction=0.3) gripperpalm = gripper.CreatePolygonFixture(box=(ow / 2, lt / 2, vec2(0, l / 2 + lt / 2), 0), density=1, friction=0.3) gripperfngL = gripper.CreatePolygonFixture( box=(wfng / 2, lfng / 2, vec2(-ow / 2 + wfng / 2, l / 2 + lt + lfng / 2), 0), density=1, friction=0.3) gripperfngR = gripper.CreatePolygonFixture( box=(wfng / 2, lfng / 2, vec2(ow / 2 - wfng / 2, l / 2 + lt + lfng / 2), 0), density=1, 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_body: " + str(obs_body.mass) + " kg , " + str(obs_body.inertia) + " kg*m^2" # white = (255, 255, 255, 255) # print des_body.fixtures colors = [(255, 50, 50, 255), (124, 252, 0, 0), (124, 252, 0, 0), (50, 50, 255, 255), (50, 50, 255, 255), (255, 255, 255, 255), (255, 255, 255, 255), (255, 255, 255, 255), (255, 255, 255, 255)] bodies = [des_body, obs1_body, obs2_body, gripper] if cmd != "fwd": for i in range(color): colors.insert(0, (124, 252, 0, 0)) if color == 1: bodies.insert(0, na) else: for b in na: bodies.insert(0, b) print colors print bodies LQ = 1 # --- main game loop --- 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 hits = 0 cnt_reward = 0 # Cast a ray from the center of the fingers to the desired object psi_ray = gripper.GetWorldPoint(gripper.localCenter + [0, l / 2 + lt]) x_ray = psi_ray[0] y_ray = psi_ray[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() # Check contact points on the gripper for ce in gripper.contacts: # print 'contact: ',ce.contact if ce.contact.touching: cntbody = ce.contact.fixtureA.body if moveList.count(cntbody) == 0: moveList.append(cntbody) elif moveList_prev.count(cntbody) != 0: if avoidList.count(cntbody) == 0: avoidList.append(cntbody) # raw_input() 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] # print "vertices",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 gripper.linearVelocity = (trj[k, 0], trj[k, 1]) gripper.angularVelocity = trj[k, 2] if hits > 0: cnt_reward = 0 else: cnt_reward = 1 * LQ # print cnt_reward # 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) pygame.display.flip() clock.tick(TARGET_FPS) db = [des_body.worldCenter[0], des_body.worldCenter[1], des_body.angle] o1 = [obs1_body.worldCenter[0], obs1_body.worldCenter[1], obs1_body.angle] o2 = [obs2_body.worldCenter[0], obs2_body.worldCenter[1], obs2_body.angle] for body in bodies: world.DestroyBody(body) return db, o1, o2
def createBlock(pos): global blockRotation dynamic_body = world.CreateDynamicBody(position=pos, angle=math.radians(blockRotation)) block = dynamic_body.CreatePolygonFixture(box=blockSize, density=1, friction=0.3) return
# --- pybox2d world setup --- # Create the world world = world(gravity=(0, -10), doSleep=True, CollideConnected=False) # And a static body to hold the ground shape ground_body = world.CreateStaticBody(position=(0, 0), shapes=polygonShape(box=(50, 1))) ground_body = world.CreateStaticBody(position=(0, 24), shapes=polygonShape(box=(1, 24))) ground_body = world.CreateStaticBody(position=(1, 25), shapes=polygonShape(box=(3, 1))) # graund_body = world.CreateDynamicBody(position = (0,0),angle = 0) # grauund = graund_body.CreatePolygonFixture(box = (50,1),density = 1, friction = 0.3) # Create a couple dynamic bodies body1 = world.CreateDynamicBody(position=(20, 1), linearVelocity=(0, 0)) # body.linearVelocity(-10) circle = body1.CreateCircleFixture(radius=1.5, density=9999999, friction=0.3) body2 = world.CreateDynamicBody(position=(20, 25), linearVelocity=(0, 5)) circle2 = body2.CreateCircleFixture(radius=0.5, density=1, friction=0.3) # joint = world.CreateJoint(bodyA = body1,bodyB = body2, collideConnected = True, type = b2RevoluteJoint) body = world.CreateDynamicBody(position=(20, 25), angle=0, linearVelocity=(0, 0)) box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3) body = world.CreateDynamicBody(position=(60, 20), angle=0,
TARGET_FPS = 60 PPM = 20 TIMESTEP = 1.0 / TARGET_FPS SCREEN_WIDTH, SCREEN_HEIGHT = 640, 400 screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32) pygame.display.set_caption("aaaa") clock = pygame.time.Clock() world = world(gravity=(0, -10)) ground_body = world.CreateStaticBody(position=(0, 0), shapes=polygonShape(box=(50, 1))) for x in range(5, 30, 5): body = world.CreateDynamicBody(position=(x, 20)) circle = body.CreateCircleFixture(radius=1, density=1, friction=0.3) for x in range(5, 40, 4): body = world.CreateDynamicBody(position=(x, 30), angle=x) box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3) colors = {staticBody: (255, 255, 255, 255), dynamicBody: (127, 127, 127, 255)} def my_draw_polygon(polygon, body, fixture): vertices = [(body.transform * v) * PPM for v in polygon.vertices] vertices = [(v[0], SCREEN_HEIGHT - v[1]) for v in vertices] pygame.draw.polygon(screen, colors[body.type], vertices) polygonShape.draw = my_draw_polygon
def simulate(cmd, trj): import pygame from pygame.locals import (QUIT, KEYDOWN, K_ESCAPE) 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) import numpy as np import copy # --- constants --- # Box2D deals with meters, but we want to display pixels, # so define a conversion factor: PPM = 20.0 # pixels per meter TARGET_FPS = 60 TIME_STEP = 1.0 / TARGET_FPS SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480 # --- cost function constants --- C_fng = 1 C_des = 1 C_obs = 1 C_cnt = 1 # --- 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) # And 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 * 1 * 9.8, angularDamping=0.3 * 1 / 12 * 9.8) obs_body = world.CreateDynamicBody(position=(18, 12), angle=0, linearDamping=0.5 * 4 * 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) obs_box = obs_body.CreatePolygonFixture(box=(2, 2), 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_body: " + str(obs_body.mass) + " kg , " + str(obs_body.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), (255, 255, 255, 255), (255, 255, 255, 255)] bodies = [ground_body, des_body, obs_body, fng1, fng2] # LOAD NAIVE VELOCITY PROFILE: generated in matlab if cmd == "naive": v_prof = np.loadtxt('examples/vel_prof1.txt', delimiter=";") v_x = v_prof[:, 0] v_y = v_prof[:, 1] # print v_prof # v_prof = np.array(v_prof, dtype='f') else: v_x = np.gradient(trj[:, 0]) / TARGET_FPS v_y = np.gradient(trj[:, 1]) / TARGET_FPS # INITIALIZE THE COST f_x = 0 pos_des_prev = copy.copy(des_body.worldCenter) pos_obs_prev = copy.copy(obs_body.worldCenter) LQ = 1 # print "initial pos: (" + str(pos_des_prev[0]) + ", " + str(pos_des_prev[1]) + ")" # --- main game loop --- # while True: for k in range(0, 181): # 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 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] pygame.draw.polygon(screen, colors[i], vertices) i = i + 1 # print i vd = vec2(v_x[k], v_y[k]) fng1.linearVelocity = vd fng2.linearVelocity = vd # print fng1.linearVelocity # print fng2.linearVelocity # Collect data from these bodies pos_des = des_body.worldCenter pos_obs = obs_body.worldCenter # Calculate the difference d_des = np.linalg.norm(pos_des - pos_des_prev) d_obs = np.linalg.norm(pos_obs - pos_obs_prev) # print d_des KE_des = KE(des_body) KE_obs = KE(obs_body) # print KE_des # print KE_obs # 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 # Determine the kinetic energy of the fingers KE_fng1 = KE_fng(fng1) KE_fng2 = KE_fng(fng2) # print KE_fng1 + KE_fng2 # Integrate the Cost function f_x = C_fng * KE_fng1 + C_fng * KE_fng2 + C_des * KE_des + C_obs * KE_obs - C_cnt * cnt_reward + f_x # print f_x # Update the previous position pos_des_prev = copy.copy(pos_des) pos_obs_prev = copy.copy(pos_obs) # 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!') # RETURN THINGS # x_k = v_prof[:,0] # y_k = v_prof[:,1] # return x_k # the velocity trajectory x_k # return y_k # the velocity trajectory y_k return (v_x, v_y, f_x) # the velocity profile, the total cost
def create_polygons(): for x in range(5, 40, 4): """Создаем прямоугольники""" body = world.CreateDynamicBody(position=(x, 30), angle=x) box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
def PostSolve(self, contact, impulse): pass # construct world world = world(gravity=(0, 0), doSleep=True, contactListener=myContactListener()) # add balloons balloon1_shape = polygonShape(vertices=balloon1) balloon1_knot_shape = polygonShape(vertices=balloon1_knot) balloon2_shape = polygonShape(vertices=balloon2) balloon2_knot_shape = polygonShape(vertices=balloon2_knot) balloon1_body = world.CreateDynamicBody(position=(0, 0)) fixture = balloon1_body.CreateFixture( fixtureDef(shape=balloon1_shape, density=1, friction=0.3)) balloon1_knot_body = world.CreateDynamicBody(position=(0, 0)) fixture = balloon1_knot_body.CreateFixture( fixtureDef(shape=balloon1_knot_shape, density=1, friction=0.3)) balloon2_body = world.CreateDynamicBody(position=(0, 0)) fixture = balloon2_body.CreateFixture( fixtureDef(shape=balloon2_shape, density=1, friction=0.3)) balloon2_knot_body = world.CreateDynamicBody(position=(0, 0)) fixture = balloon2_knot_body.CreateFixture( fixtureDef(shape=balloon2_knot_shape, density=1, friction=0.3)) # weld joints world.CreateWeldJoint(bodyA=balloon1_body,