def build_rays(self, rays): ret = [] for i in rays: input = b2.rayCastInput(p1=self.body.transform * i, p2=self.body.worldCenter, maxFraction=1) output = b2.rayCastOutput() self.body.fixtures[0].RayCast(output, input, 0) ret.append((self.body.localCenter + i * (1. - output.fraction), i)) return ret
def get_boundary_intersection(self, p1, p2): input = b2.rayCastInput(p1=p1, p2=p2, maxFraction=1) output = b2.rayCastOutput() closest_fraction = None closest_point = None for i in self.boundary.fixtures: if i.RayCast(output, input, 0): hit_point = input.p1 + output.fraction * (input.p2 - input.p1) if closest_point is None or closest_fraction > output.fraction: closest_point = hit_point closest_fraction = output.fraction return closest_point, closest_fraction
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 simulate(cmd, trj): import Box2D # The main library # Box2D.b2 maps Box2D.b2Vec2 to vec2 (and so on) from Box2D.b2 import (world, polygonShape, circleShape, staticBody, dynamicBody, vec2) # --- constants --- # Box2D deals with meters, but we want to display pixels, # so define a conversion factor: PPM = 20.0 # pixels per meter TIME_STEP = 1.0 / TARGET_FPS SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480 # --- pygame setup --- screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32) pygame.display.set_caption('Simple pygame example') clock = pygame.time.Clock() # --- pybox2d world setup --- # Create the world world = world(gravity=(0, 0), doSleep=True) # Add a static body to hold the ground shape ground_body = world.CreateStaticBody( position=(0, 1), shapes=polygonShape(box=(50, 1)), ) # Create dynamic bodies des_body = world.CreateDynamicBody(position=(15, 12), angle=0, linearDamping=0.5 * 4 * 9.8, angularDamping=0.3 * 1 / 12 * 9.8) obs1_body = world.CreateDynamicBody(position=(17, 9), angle=30, linearDamping=0.5 * 18 * 9.8, angularDamping=0.3 * 4 / 12 * 9.8) obs2_body = world.CreateDynamicBody(position=(11, 11), angle=0, linearDamping=0.5 * 24 * 9.8, angularDamping=0.3 * 4 / 12 * 9.8) # Create fingers as kinematic bodies (infinite masses and directly controls velocity) width = 2.5 fng1 = world.CreateKinematicBody(position=(16, 5), angle=0) fng2 = world.CreateKinematicBody(position=(16 + width, 5), angle=0) # And add box fixtures onto it (with a nonzero density, so it will move) des_box = des_body.CreatePolygonFixture(box=(1, 1), density=1, friction=0.3, restitution=0.8) obs1_box = obs1_body.CreatePolygonFixture(box=(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) printflag = True # Model fingers as small circular cross sections # circle = circleShape(radius=0.1) fng1_cir = fng1.CreatePolygonFixture(box=(0.1, 0.1), density=5, friction=0.3) fng2_cir = fng2.CreatePolygonFixture(box=(0.1, 0.1), density=5, friction=0.3) # Mass and Moment of Inertia data # print "des_body: " + str(des_body.mass) + " kg , " + str(des_body.inertia) + " kg*m^2" # print "obs_body1: " + str(obs_body1.mass) + " kg , " + str(obs_body1.inertia) + " kg*m^2" # print fng1.linearVelocity colors = [(255, 255, 255, 255), (255, 50, 50, 255), (124, 252, 0), (124, 252, 0), (50, 50, 255, 255), (50, 50, 255, 255), (255, 255, 255, 255), (255, 255, 255, 255)] bodies = [ground_body, des_body, obs1_body, obs2_body, fng1, fng2] # LOAD DESIRED VELOCITY PROFILE if cmd == 'naive': v_prof = np.loadtxt('examples/vel_prof1.txt', delimiter=';') v_x = v_prof[:, 0] v_y = v_prof[:, 1] psi_prof = np.loadtxt('examples/pos_prof1.txt', delimiter=';') xfng = np.reshape(np.matrix(psi_prof[:, 0]), (N, 1)) yfng = np.reshape(np.matrix(psi_prof[:, 1]), (N, 1)) # print xfng # print v_y/TARGET_FPS else: v_prof = np.gradient(trj, axis=0) * TARGET_FPS v_x = v_prof[:, 0] v_y = v_prof[:, 1] xfng = trj[:, 0] yfng = trj[:, 1] # print 'something else', v_x # GATHER ACTUAL FNG POSITIONS # xfng = np.zeros((N, 1)) # yfng = np.zeros((N, 1)) # INTIALIZE THE COST FUNCTION fx = 0 fy = 0 LQ = 1 xdes = np.zeros((N, 1)) ydes = np.zeros((N, 1)) # --- 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 hits = 0 cnt_reward = 0 # Cast a ray from the center of the fingers to the desired object x_ray = fng1.worldCenter[0] + width / 2 y_ray = fng1.worldCenter[1] x_db = des_body.worldCenter[0] y_db = des_body.worldCenter[1] input = rayCastInput(p1=(x_ray, y_ray), p2=(x_db, y_db), maxFraction=1) output = rayCastOutput() for body in bodies: # or: world.bodies # The body gives us the position and angle of its shapes for fixture in body.fixtures: # The fixture holds information like density and friction, # and also the shape. shape = fixture.shape # Naively assume that this is a polygon shape. (not good normally!) # We take the body's transform and multiply it with each # vertex, and then convert from meters to pixels with the scale # factor. vertices = [(body.transform * v) * PPM for v in shape.vertices] # But wait! It's upside-down! Pygame and Box2D orient their # axes in different ways. Box2D is just like how you learned # in high school, with positive x and y directions going # right and up. Pygame, on the other hand, increases in the # right and downward directions. This means we must flip # the y components. vertices = [(v[0], SCREEN_HEIGHT - v[1]) for v in vertices] if body != des_body: hit = shape.RayCast(output, input, body.transform, 0) if hit: hits = 1 + hits pygame.draw.polygon(screen, colors[i], vertices) i = i + 1 # print i vd = vec2((float)(v_x[k]), (float)(v_y[k])) fng1.linearVelocity = vd fng2.linearVelocity = vd # print fng1.linearVelocity # print fng2.linearVelocity if hits > 0: cnt_reward = 0 else: cnt_reward = 1 * LQ print cnt_reward # Collect data from these bodies KEx_des, KEy_des = KE(des_body) KEx_obs1, KEy_obs1 = KE(obs1_body) KEx_obs2, KEy_obs2 = KE(obs2_body) KEx_obs = KEx_obs1 + KEx_obs2 KEy_obs = KEy_obs1 + KEy_obs2 psi_des = des_body.GetWorldPoint(des_body.localCenter + [-width / 2, 0]) xdes[k] = psi_des[0] ydes[k] = psi_des[1] # xdes[k] = 13.75 # (CONSTANT) # ydes[k] = 12 # (CONSTANT)12 # Collect data from the fingers KEx_fng1, KEy_fng1 = KE_fng(fng1, mfng) KEx_fng2, KEy_fng2 = KE_fng(fng2, mfng) # xfng[k] = fng1.worldCenter[0] # yfng[k] = fng1.worldCenter[1] # # Check contacts # cnt_reward = 0 # for c in des_body.contacts: # # if printflag: # # print c.contact.touching # # printflag = False # if c.contact.touching : # # print "sensor triggered" # cnt_reward = 0 # else: # # print "contacts points are free" # cnt_reward = 1*LQ # Integrate the Cost function # print cnt_reward fx = C1 * KEx_fng1 + C2 * mfng * np.abs( xfng[k] - xdes[k])**2 + C3 * KEx_des + C4 * KEx_obs - C5 * cnt_reward + fx fy = C1 * KEy_fng1 + C2 * mfng * np.abs( yfng[k] - ydes[k])**2 + C3 * KEy_des + C4 * KEy_obs - C5 * cnt_reward + fy # print "KEx: " + str(KEx_fng1) + ", KEy: " + str(KEy_fng1) # Make Box2D simulate the physics of our world for one step. # Instruct the world to perform a single step of simulation. It is # generally best to keep the time step and iterations fixed. # See the manual (Section "Simulating the World") for further discussion # on these parameters and their implications. world.Step(TIME_STEP, 10, 10) # Flip the screen and try to keep at the target FPS if cmd == "naive" or cmd == "show": pygame.display.flip() clock.tick(TARGET_FPS) else: pass pygame.quit() print('Simulation Done!') # print 'xdes', xdes # print 'xfng', xfng return xfng, yfng, xdes, ydes, fx, fy
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 cnt_reward = 0 # Cast a ray from the center of the fingers to the desired object x_ray = fng1.worldCenter[0] + width / 2 y_ray = fng1.worldCenter[1] x_db = des_body.worldCenter[0] y_db = des_body.worldCenter[1] input = rayCastInput(p1=(x_ray, y_ray), p2=(x_db, y_db), maxFraction=1) output = rayCastOutput() hits = 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]