def __init__(self, img, pos, world, scale=1): img = pygame.transform.rotozoom(img, 0, scale) self.img = img self.rect = img.get_rect(center=(pos[0] * PPM, 600 - pos[1] * PPM)) anchora = ((self.rect.topleft[0] + 10) / PPM, (600 - self.rect.topleft[1] - 10) / PPM) anchorb = ((self.rect.topright[0] - 10) / PPM, (600 - self.rect.topright[1] - 10) / PPM) self.anchora = world.CreateStaticBody(position=(anchora), angle=0) self.anchorb = world.CreateStaticBody(position=(anchorb), angle=0)
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, all_sprites): """Инициализация объекта""" super().__init__(all_sprites) ground_body = world.CreateStaticBody(position=(0, 0), shapes=polygonShape(box=(30, 5))) self.image = load_image("ground.jpg") self.rect = pygame.Rect(0, 700, 600, 100) self.mask = pygame.mask.from_surface(self.image)
def PG(ph, a, d): theta = ph[2] phi = math.pi / 2 - theta neg_area = world.CreateStaticBody(position=(ph[0] + 0.5 * d * cos(theta), ph[1] + 0.5 * d * sin(theta)), angle=theta) na_fixt = neg_area.CreatePolygonFixture(box=(d, a), density=1, friction=1) shape = na_fixt.shape vertices = [(neg_area.transform * v) * PPM for v in shape.vertices] return vertices
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, 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, vertices, inner_corners, color, position, world): # use box2d edge self.uuid = uuid1() self.vertices = vertices self.color = color self.position = position self.world = world self.edge_fixtures = [] self.edge_bodies = [] for index, corner in enumerate(inner_corners): next_corner = inner_corners[(index + 1) % len(inner_corners)] fixture = edgeShape(vertices=[corner, next_corner * 2 - corner]) self.edge_fixtures.append(fixture) body = world.CreateStaticBody(shapes=fixture, position=corner) body.userData = self self.edge_bodies.append(body)
def __init__(self, color, vertices, world_map=None, mass=1.0, density=0.050, friction=0.3, position=(0.0, 0.0), angle=0.0, momentum_vector=np.array([0, 0], dtype=float), angular_velocity=0.0, dynamic=True): self.uuid = uuid1() self.color = color self.vertices = [np.array(vertex, dtype=float) for vertex in vertices] self.edges = [] for index, vertex in enumerate(self.vertices): self.edges.append([ vertex, self.vertices[(index + 1) % len(self.vertices)] - vertex ]) self.momentum_vector = momentum_vector # x,y, direction self.world_map = world_map self.world = world_map.world self.body = None self.fixture = None self.mass = mass self.friction = friction self.position = position if world_map is not None: if dynamic is True: self.body = self.world.CreateDynamicBody(position=position, angle=angle) self.fixture = self.body.CreatePolygonFixture( vertices=vertices, density=density, friction=friction) else: self.body = world.CreateStaticBody(position=position, angle=angle) self.body.userData = self self.body.linearVelocity += momentum_vector / mass self.body.angularVelocity += angular_velocity world_map.add_physical(self)
def build(self, world): # is it an error to call terrain.build() before terrain.generate()? # raise an exception, to make this easier to detect. # Or, terrain.build() could call terrain.generate() if terrain.generate() # has not been called; this makes its API easier to use, and the code # less fragile if not self.generated: return None start = 0 self.bodies = [] for i in range(self.n_segments): body = world.CreateStaticBody( position=self.seg_positions[i], angle=self.seg_angles[i], shapes=polygonShape(box=(self.seg_lengths[i] / 2., .5))) start += self.seg_lengths[i] self.bodies.append(body) return self.length
def move(pi, pf, T): # Calculate trajectory T = int(T) trj_x = TARGET_FPS * (pf[0] - pi[0]) / T * np.matrix(np.ones((T, 1))) trj_y = TARGET_FPS * (pf[1] - pi[1]) / T * np.matrix(np.ones((T, 1))) trj_th = TARGET_FPS * (pf[2] - pi[2]) / T * np.matrix(np.ones((T, 1))) psi = np.hstack((trj_x, trj_y, trj_th)) # Calculate negative area th = atan2(pf[1] - pi[1], pf[0] - pi[0]) d_tot = dist(pf[0:2], pi[0:2]) xc = pi[0] + d_tot * cos(th) yc = pi[1] + d_tot * sin(th) neg_area = world.CreateStaticBody(position=(xc, yc), angle=th) neg_area.active = False na_fixt = neg_area.CreatePolygonFixture(box=(d_tot, a), density=1, friction=1) # color = (124,252,0,0) color = 1 return neg_area, color, psi
def __init__(self, world, start_point, end_point, width=0.5, color=(100, 100, 100, 255)): self.color = color length = math.sqrt((end_point[0] - start_point[0])**2 + (end_point[1] - start_point[1])**2) angle = math.atan2(end_point[1] - start_point[1], end_point[0] - start_point[0]) self.body = world.CreateStaticBody( position=((start_point[0] + end_point[0]) / 2, (start_point[1] + end_point[1]) / 2), angle=angle) # self.body = world.CreateDynamicBody(position=((start_point[0]+end_point[0])/2, (start_point[1]+end_point[1])/2), angle=angle) self.fixture = self.body.CreatePolygonFixture(box=(length / 2, width / 2), density=1, friction=0.3) self.box = self.fixture.shape
def PG(ph, a, d, theta0): # Calculate Vertices of Negative Are theta = theta0 print theta neg_area = world.CreateStaticBody(position=(ph[0] + 0.5 * d * cos(theta), ph[1] + 0.5 * d * sin(theta)), angle=theta) neg_area.active = False na_fixt = neg_area.CreatePolygonFixture(box=(d, a), density=1, friction=1) # shape = na_fixt.shape # vertices = [(neg_area.transform * v) * PPM for v in shape.vertices] # color = [(124,252,0,0)] color = 1 # Calculate Push motion v = d / (N - Nt) trj_x = v * cos(theta) * np.matrix(np.ones((N - Nt, 1))) trj_x = TARGET_FPS * np.vstack((np.zeros((Nt, 1)), trj_x)) trj_y = v * sin(theta) * np.matrix(np.ones((N - Nt, 1))) trj_y = TARGET_FPS * np.vstack((np.zeros((Nt, 1)), trj_y)) trj_th = float(theta0 - init_pos[2]) / (Nt) * np.matrix(np.ones((Nt, 1))) trj_th = TARGET_FPS * np.vstack((trj_th, np.zeros((N - Nt, 1)))) psi_naive = np.hstack((trj_x, trj_y, trj_th)) return neg_area, color, psi_naive
screen = pg.display.set_mode(size) pg.display.update() run = True # переменна, с помощью ее можно выходить из цикла """А вот тут будет волшебство)""" clock = pg.time.Clock() RED = (255, 0, 0) BLACK = (0, 0, 0) world = world(gravity=(0, -10)) # создание поля ground_body = world.CreateStaticBody( position=(0, 10), shapes=polygonShape(box=(20, 0.5)), ) ground_body2 = world.CreateStaticBody( position=(37, 10), shapes=polygonShape(box=(10, 0.5)), ) ground_body3 = world.CreateStaticBody(position=(0, 0), shapes=polygonShape(box=(50, 1))) body = world.CreateStaticBody(position=(20, 15)) circle = body.CreateCircleFixture(radius=1, density=1, friction=0.3) def create_circle(): for i in range(5, 40, 5):
start_frame = int(sys.argv[1]) end_frame = int(sys.argv[2]) # construct world world = world(gravity=(0, 0), doSleep=True) # 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) ground_body = world.CreateStaticBody(position=(0, -0.4), shapes=border_shape_h) border_body3 = world.CreateStaticBody(position=(0.49, 0), shapes=border_shape_v) border_body4 = world.CreateStaticBody(position=(-0.5, 0), shapes=border_shape_v) env = [border_body1, ground_body, border_body3, border_body4] # load strokes chains = [] static_chains = [] dynamic_chains = []
TARGET_FPS = 60 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)), ) agent = AgentBody(world) agents = [agent] 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): # The user closed the window or pressed escape running = False screen.fill((0, 0, 0, 0)) # Draw the world # for body in world.bodies:
TARGET_FPS = 60 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=(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
pygame.init() screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32) pygame.display.set_caption('GraviTask') clock = pygame.time.Clock() world = world(gravity=(0, -10), doSleep=True) ground_shape = Box2D.b2PolygonShape(vertices = [(0 , 10), (2 , 1), (98 , 1), (100 , 10), (100 , 0), (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 = {
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
def test(func, godisp, seed): startrun = 0 from Box2D.b2 import (world, polygonShape, circleShape, staticBody, dynamicBody) if godisp: PPM = 20.0 # pixels per meter TARGET_FPS = 60 TIME_STEP = 1.0 / TARGET_FPS else: TIME_STEP = 1.0 / 60 SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480 # --- pygame setup --- if godisp: 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, -20), doSleep=True) # And a static body to hold the ground shape ground_body = world.CreateStaticBody( position=(0, 0), shapes=polygonShape(box=(50, 1)), ) top = world.CreateStaticBody(position=(0, 24), shapes=polygonShape(box=(50, 1))) side_body = world.CreateStaticBody( position=(0, 0), shapes=polygonShape(box=(1, 50)), ) other_side = world.CreateStaticBody( position=(32, 0), shapes=polygonShape(box=(1, 50)), ) # Create a couple dynamic bodies body = world.CreateDynamicBody(position=(seed[2], seed[3])) ball = body.CreateCircleFixture(radius=0.5, density=.2, friction=0, restitution=1) body = world.CreateDynamicBody(position=(15.5, 0), angle=0) slider = body.CreatePolygonFixture(box=(3, .4), 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. if godisp: 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 if godisp: def my_draw_circle(circle, body, fixture): position = body.transform * circle.pos * PPM position = (position[0], SCREEN_HEIGHT - position[1]) pygame.draw.circle(screen, colors[body.type], [int(x) for x in position], int(circle.radius * PPM)) # Note: Python 3.x will enforce that pygame get the integers it requests, # and it will not convert from float. circleShape.draw = my_draw_circle # --- main game loop --- world.bodies[4].linearVelocity = (seed[0], seed[1]) running = True while running: startrun += 1 movement = 0 movement = func([ world.bodies[5].position[0], world.bodies[4].linearVelocity, world.bodies[4].position ]) * 28 # Check the event queue if godisp: for event in pygame.event.get(): if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT: movement -= 1 if event.key == pygame.K_RIGHT: movement += 1 if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE): running = False pos = world.bodies[5].position # print(pos) world.bodies[5].position = Box2D.b2Vec2(float(movement), pos[1]) if godisp: screen.fill((0, 0, 0, 0)) if world.bodies[4].position[1] <= 1.8: running = False if godisp: # Draw the world for body in world.bodies: for fixture in body.fixtures: fixture.shape.draw(body, fixture) # Make Box2D simulate the physics of our world for one step. world.Step(TIME_STEP, 10, 10) # Flip the screen and try to keep at the target FPS if godisp: pygame.display.flip() clock.tick(TARGET_FPS) if startrun > 5000: return startrun if godisp: pygame.quit() return startrun
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