def step(): world.Step(1.0 / 60.0, 10, 10) for event in pygame.event.get(): if event.type == QUIT: pygame.quit() sys.exit() quit() #change car color car_color = 'blue' for contact in car.contacts: if car.contacts[0].contact.touching: car_color = 'red' # draw background, walls, car screen.fill(THECOLORS['gray']) for track in tracks: for fixture in track[0].fixtures: shape = fixture.shape pygame.draw.polygon(screen, THECOLORS[track[1]], tfm(shape.vertices)) car.position = (pygame.mouse.get_pos()[0] / PPM, pygame.mouse.get_pos()[1] / PPM) shape = car.fixtures[0].shape vertices = [(car.transform * v) * PPM for v in shape.vertices] pygame.draw.polygon(screen, THECOLORS[car_color], vertices) pygame.display.flip() clock.tick(60.0)
def start_balls(): global run fon() while run: fon() time_delta = clock.tick(SPEED) / 1000.0 for body in world.bodies: for fixture in body.fixtures: fixture.shape.draw(body, fixture) world.Step(TIME_STEP, 10, 10) for event in pg.event.get(): if event.type == pg.QUIT: run = False if event.type == pg.MOUSEBUTTONUP: if event.button == 1: create_circle() elif event.button == 3: create_polygons() if event.type == pg.KEYDOWN: if event.key == pg.K_DOWN: pass if event.key == pg.K_UP: pass pg.display.flip() # Обновление кадра
# output_body(balloon1_knot_body, start_frame, frame, 1) # output_chain(chain1, frame, 2) # output_body(balloon2_body, start_frame, frame, 3) # output_body(balloon2_knot_body, start_frame, frame, 4) # output_chain(chain2, frame, 5) for ind in range(len(chains)): chain = chains[ind] output_chain(chain, frame, ind) # 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) world.ClearForces() print("time stepped") # Flip the screen and try to keep at the target FPS pygame.display.flip() clock.tick(TARGET_FPS) if frame == end_frame: break if period_counter == PERIOD: PERIOD = math.ceil(random.random() * 5 + 20) period_counter = 0 rand = [random.random(), random.random()]
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
elif event.key == pygame.K_s: controlState |= TDC_DOWN elif event.type == pygame.KEYUP: if event.key == pygame.K_a: controlState &= ~TDC_LEFT elif event.key == pygame.K_d: controlState &= ~TDC_RIGHT elif event.key == pygame.K_w: controlState &= ~TDC_UP breaking = False elif event.key == pygame.K_s: controlState &= ~TDC_DOWN breaking = False Step(car, controlState) world.Step(diff, 10, 10) position = car.m_body.worldCenter offset = [-position.x * PPM, position.y * PPM] offset[0] += size[0] / 2 offset[1] += size[1] / 2 - 13 topLeft = [0, 0] topLeft[0] = int(position.x / TILE_SIZE - 15) topLeft[1] = int(-position.y / TILE_SIZE - 15) for x in xrange(topLeft[0], topLeft[0] + 30): for y in xrange(topLeft[1], topLeft[1] + 27): if (y < 0 or x < 0 or len(grid) <= y or len(grid[y]) <= x): continue
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
east_doorways=[doorways[4]], name='r 5') building = Building(rooms=[room1, room2, room3, room4, room5], doorways=doorways) num_agents = 30 # agents = [AgentBody(world, x,y) for x,y in [(random.randint(1, 19), random.randint(1, 19)) for i in range(num_agents)]] agents = [agent.Agent(world, building, type='leader', spawn_room=room4) for i in range(num_agents)] 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 building.draw(screen, render_settings) for agent in agents: agent.draw(screen, render_settings) agent.go_to_exit() # Make Box2D simulate the physics of our world for one step. world.Step(render_settings['TIME_STEP'], 10, 10) # Flip the screen and try to keep at the target FPS pygame.display.flip() clock.tick(render_settings['TARGET_FPS']) pygame.quit() print('Done!')
# 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 ('body.transform: ', body.transform) print ('shape.vertices: ', shape.vertices) print ('vertices in pixels: ', 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[body.type], vertices) #running=False # 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, 1000, 1000) # Flip the screen and try to keep at the target FPS pygame.display.flip() clock.tick(TARGET_FPS) pygame.quit() print('Done!')
posb = slingshot.rect.centerx, slingshot.rect.y posb = posb[0] / PPM, (600 - posb[1]) / PPM length = (((posb[0] - posa[0])**2 + (posb[1] - posa[1])**2)**(1 / 2)) if length <= 1.0: in_sling.body.transform = (posa, in_sling.body.angle) else: reduct = 1.0 / length move = (posb[0] - (posb[0] - posa[0]) * reduct, posb[1] - (posb[1] - posa[1]) * reduct) in_sling.body.transform = (move, in_sling.body.angle) else: in_sling.body.awake = False world.Step(TIME_STEP, 10, 10) #always do before drawing!! if art: screen.fill((147, 211, 246)) screen.blit(background_art, (0, 0)) slingshot.draw(screen) draw_sling(SLING_COLOR, slingshot) for each in things: if each != slingshot: each.draw(screen) elif not art: screen.fill((0, 0, 0)) for each in things: each.draw_shape(screen) draw_sling(WHITE, slingshot) pygame.display.flip()
# open or resize window (This function is valid only on PC,Ignored in smartphone apps) core.window(True, 480, 640) cam = core.camera("maincam") cam.orthographicProjection = True cam.position = vmath.vec3(0.0, 0.0, 100.0) showcase = core.showcase() boxes = [] world = world(gravity=(0, -10), doSleep=True) boxes.append(DynamicBox(world, showcase, (0, -100), (150, 20), 0, True)) boxes.append(DynamicBox(world, showcase, (10, 100), (10, 5), 15)) while True: core.update() touch = core.singleTouch() if touch is not None and touch['is_pressed']: boxes.append( DynamicBox(world, showcase, (touch['cur_x'], touch['cur_y']), (10, 5), 15)) world.Step(core.getElapsedTime(), 10, 10) for box in boxes: box.update() core.update() cam.shoot(showcase) core.swap()
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 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