def __init__(self, settings, ID, actor=None): self.id = ID self.actor = actor self.rotation_speed = settings.agent_rotation_speed self.force = settings.agent_force self._color = b2Color(0.4, 0.4, 0.6) self.color = b2Color(0.4, 0.4, 0.6)
def color(self): if self.team == 0: return b2Color(0.2, 0.2, 1) if self.team == 1: return b2Color(1, 0.2, 0.2) if self.team == 2: return b2Color(0.2, 1, 0.2)
def __init__(self, container, width, height): SimulationBase.__init__(self, container, width, height) # --- pybox2d world setup --- # Create the world self.m_b2dWorld = world(gravity=(0, -10), doSleep=True) self.m_debugDraw = DebugDrawExtended(surface=settings.OBJ_SURFACE) self.m_b2dWorld.renderer = self.m_debugDraw self.m_b2dWorld.warmStarting = True self.m_b2dWorld.continuousPhysics = True self.m_b2dWorld.subStepping = False self.m_groundBody = None self.mouseJoint = None self.colours = { 'mouse_point': b2Color(0, 1, 0), 'joint_line': b2Color(0.8, 0.8, 0.8) } self.m_joints = [] self.init() self.setupWorld()
def Step(self, settings): # PREDICT # qval = None # fixing = False translating = False if self.car.crashed: for obstacle in self.obstacles: obstacle.fixtures[0].sensor = True self.car.body.transform = (self.car.body.position * -1, 0) self.car.body.angle = math.radians(np.random.randint(0, 359)) translating = True else: for obstacle in self.obstacles: obstacle.fixtures[0].sensor = False self.steps += 1 self.pressed_keys = set() action = self.learner.take_action(self.steps) if action == 0: pass elif action == 1: self.pressed_keys.add('left') elif action == 2: self.pressed_keys.add('right') # UPDATE was_crashed = self.car.crashed was_hitting_goal = self.car.hit_goal self.car.update(self.pressed_keys, settings.hz, self.world, self.goal, self.goal_radius) super(TopDownCar, self).Step(settings) just_crashed = not was_crashed and self.car.crashed just_hit_goal = not was_hitting_goal and self.car.hit_goal if translating: body_angle = self.car.body.angle for tire in self.car.tires: tire.body.angle = body_angle self.learner.reinforce(self.steps, just_crashed, just_hit_goal, self.goal.position) # easy just count number of goals hit # if self.steps % 15000 == 0: # DRAW self.renderer.DrawPoint(self.renderer.to_screen(self.car.lidar), 5.0, b2Color(0.4, 0.9, 0.4)) for pt in self.car.sensor_tips: self.renderer.DrawSegment(self.renderer.to_screen(self.car.lidar), self.renderer.to_screen(pt), b2Color(0.4, 0.9, 0.4)) for dist, pt in self.car.sensor_readings: if dist != self.car.sensor_len: self.renderer.DrawPoint(self.renderer.to_screen(pt), 5.0, b2Color(0.4, 0.9, 0.4))
def _print_observations_to_screen(self): robot_obs_dict = \ self._friendly_robots_handler.get_observations() if not robot_obs_dict: return neg_ecores, pos_ecores = self._ecores_handler.get_ecore_counts() results_str = f'Neg Ecores: {neg_ecores} | Pos Ecores: {pos_ecores}\n' aruco_id = next(iter(robot_obs_dict)) results_str = \ results_str + f'Lower observations for robot: {aruco_id}\n' results_str = results_str + \ f'{"|".join(self._friendly_robots_handler.lower_tags)}\n' angles = [ element * 180 / b2_pi for element in self._friendly_robots_handler.angles ] results_str = results_str + print_observations( robot_obs_dict[aruco_id]['lower_obs'], angles, return_string=True, include_raw=False) results_str_arr = results_str.split("\n") y_start = 230 for line in results_str_arr: self.DrawStringAt(30, y_start, line, b2Color(0, 0, 0)) y_start += 18
def laser_display(renderer, laser_body, length=30.0, laser_color=(1, 0, 0), laser_half_width=2, **kwargs): if not renderer: return p1, p2 = get_laser_line(laser_body, length, laser_half_width) renderer.DrawSegment(renderer.to_screen( p1), renderer.to_screen(p2), b2Color(*laser_color))
def __init__(self, n_agents=[10], actors=None, colors=None, targets=None, **kwargs): self.settings = flockSettings(**kwargs) self.framework = PygletFramework( self.settings) if self.settings.render else NoRender(self.settings) self.framework.env = self self.done = False self.n_agents = n_agents self.n_targets = 1 if targets == None else len(np.unique(targets)) self.targets_idx = [0] * n_agents[0] if targets == None else targets self.time_passed = 0 # create random target location self.targets = [] for t in range(self.n_targets): self.t_min, self.t_max = self.settings.target_mindist, self.settings.target_maxdist rand_angle = 2 * np.pi * random.random() rand_dist = self.t_min + random.random() * (self.t_max - self.t_min) self.targets.append( b2Vec2(rand_dist * np.cos(rand_angle), rand_dist * np.sin(rand_angle))) if self.settings.render: self.framework.gui_objects["target" + str(t)] = { 'shape': 'circle', 'values': [ self.targets[t], self.settings.reward_radius, b2Color(1, 1, 1) ] } self.selected_target = 0 # create agents self.agents = [] for i in range(sum(self.n_agents)): x = self.settings.start_spread * ( random.random() - 0.5) + self.settings.start_point[0] y = self.settings.start_spread * ( random.random() - 0.5) + self.settings.start_point[1] angle = random.uniform(-1, 1) * np.pi agent = Agent(self.settings, ID=i) if actors: agent.actor = actors[i] if colors: agent._color = colors[i] agent.body = self.framework.world.CreateDynamicBody( **self.settings.bodySettings, position=(x, y), angle=angle, userData=agent) self.agents.append(agent) self.create_space() self.create_space_flag = False self.obs = self.get_obs()
def __init__(self, world, renderer, params): self._params = params self.p1_color = b2Color(0.4, 0.9, 0.4) # self.s1_color = b2Color(0.8, 0.8, 0.8) # self.s2_color = b2Color(0.9, 0.9, 0.4) self._line_colors = [ b2Color(0.0, 0.0, 1.0), b2Color(0.0, 1.0, 0.0), b2Color(1.0, 0.0, 0.0) ] self._front_line_color = b2Color(0.0, 1.0, 1.0) self.world = world self.renderer = renderer self.ray_angles = get_ray_angles( 2 * b2_pi / 360 * params["image_processing"]["max_angle_per_side"], self._params["image_processing"]["number_of_rays_per_side"])
def MouseDown(self, p): """ Mouse moved to point p, in world coordinates. """ self.targets[self.selected_target] = p self.framework.gui_objects["target" + str(self.selected_target)] = { 'shape': 'circle', 'values': [p, self.settings.reward_radius, b2Color(1, 1, 1)] }
def test_color(self): x, y, z = 1.0, 2.0, 3.0 c1 = b2Color(x, y, z) c2 = b2Color(z, y, x) c = c1 + c2 self.checkAlmostEqual(c, (x + z, y + y, z + x), msg='b2Color +') c = c1 - c2 self.checkAlmostEqual(c, (x - z, y - y, z - x), msg='b2Color -') c = 2.0 * c1 self.checkAlmostEqual(c, (x + x, y + y, z + z), msg='float * b2Color') c = c1 * 2.0 self.checkAlmostEqual(c, (x + x, y + y, z + z), msg='b2Color * float') c = c1 / 2.0 self.checkAlmostEqual(c, (x / 2.0, y / 2.0, z / 2.0), msg='b2Color / float') c = c1.copy() c *= 2.0 self.checkAlmostEqual(c, (x + x, y + y, z + z), msg='b2Color *= float') c = b2Color(c1) c /= 2.0 self.checkAlmostEqual(c, (x / 2.0, y / 2.0, z / 2.0), msg='b2Color /= float') c1 += (1.0, 1.0, 1.0) self.checkAlmostEqual(c1, (x + 1, y + 1, z + 1), msg='b2Color +=') c1 -= (1.0, 1.0, 1.0) self.checkAlmostEqual(c1, (x, y, z), msg='b2Color -=') bytes = b2Color(1, 1, 1).bytes self.assertEqual(bytes, [255, 255, 255], msg='bytes (1,1,1)=>%s' % bytes) bytes = b2Color(0, 0, 0).bytes self.assertEqual(bytes, [0, 0, 0], msg='bytes (1,1,1)=>%s' % bytes) self.assertEqual(c1[0], x) self.assertEqual(c1[1], y) self.assertEqual(c1[2], z) c1.list = (x * 2, y * 2, z * 2) self.checkAlmostEqual(c1, (x + x, y + y, z + z), msg='b2Color.list')
def get_rewards(self): rewards = {} for contact in self.framework.world.contacts: rewards[contact.fixtureA.body.userData.id] = -1 rewards[contact.fixtureB.body.userData.id] = -1 if self.settings.render: contact.fixtureA.body.userData.color = b2Color(1, 0.2, 0.2) contact.fixtureB.body.userData.color = b2Color(1, 0.2, 0.2) for agent in self.agents: if agent.id not in rewards: d = np.sqrt( b2DistanceSquared(self.targets[self.targets_idx[agent.id]], agent.body.position)) if self.settings.reward_mode == "linear": rewards[agent.id] = (-d / 35) + 1 # arbitrary... if self.settings.render: agent.reset_color() if self.settings.reward_mode == "binary": r = int(d < self.settings.reward_radius) rewards[agent.id] = r return rewards
def laser_display(renderer, laser_body, length=30.0, laser_color=(1, 0, 0), laser_half_width=2, **kwargs): if not renderer: return p1, p2 = get_laser_line(laser_body, length, laser_half_width) renderer.DrawSegment(renderer.to_screen(p1), renderer.to_screen(p2), b2Color(*laser_color))
def ManualDraw(self): """ This implements code normally present in the C++ version, which calls the callbacks that you see in this class (DrawSegment, DrawSolidCircle, etc.). This is implemented in Python as an example of how to do it, and also a test. """ colors = { 'active': b2Color(0.5, 0.5, 0.3), 'static': b2Color(0.5, 0.9, 0.5), 'kinematic': b2Color(0.5, 0.5, 0.9), 'asleep': b2Color(0.6, 0.6, 0.6), 'default': b2Color(0.9, 0.7, 0.7), } if self.test.settings.drawShapes: for body in self.test.world.bodies: transform = body.transform color = body.userData.color if body.userData else colors[ "default"] if not body.active: color = color / 3 for fixture in body.fixtures: # if not body.active: # color = colors['active'] # elif body.type == b2_staticBody: # color = colors['static'] # elif body.type == b2_kinematicBody: # color = colors['kinematic'] # elif not body.awake: # color = colors['asleep'] # else: # color = colors['default'] self.DrawBodyShape(fixture.shape, transform, color) for object in self.test.gui_objects.values(): if object['shape'] == 'circle': self.DrawCircle(object['values'][0], object['values'][1], object['values'][2]) # if self.test.settings.drawJoints: # for joint in self.test.world.joints: # self.DrawJoint(joint) if self.test.settings.drawAABBs: color = b2Color(0.9, 0.3, 0.9) cm = self.test.world.contactManager for body in self.test.world.bodies: if not body.active: continue transform = body.transform for fixture in body.fixtures: shape = fixture.shape for childIndex in range(shape.childCount): self.DrawAABB(shape.getAABB(transform, childIndex), color)
def test_color(self): x, y, z = 1.0, 2.0, 3.0 c1 = b2Color(x, y, z) c2 = b2Color(z, y, x) c = c1 + c2 self.checkAlmostEqual(c, (x+z, y+y, z+x), msg='b2Color +') c = c1 - c2 self.checkAlmostEqual(c, (x-z, y-y, z-x), msg='b2Color -') c = 2.0 * c1 self.checkAlmostEqual(c, (x+x, y+y, z+z), msg='float * b2Color') c = c1 * 2.0 self.checkAlmostEqual(c, (x+x, y+y, z+z), msg='b2Color * float') c = c1 / 2.0 self.checkAlmostEqual(c, (x/2.0, y/2.0, z/2.0), msg='b2Color / float') c = c1.copy() c *= 2.0 self.checkAlmostEqual(c, (x+x, y+y, z+z), msg='b2Color *= float') c = b2Color(c1) c /= 2.0 self.checkAlmostEqual(c, (x/2.0, y/2.0, z/2.0), msg='b2Color /= float') c1 += (1.0, 1.0, 1.0) self.checkAlmostEqual(c1, (x+1, y+1, z+1), msg='b2Color +=') c1 -= (1.0, 1.0, 1.0) self.checkAlmostEqual(c1, (x, y, z), msg='b2Color -=') bytes=b2Color(1, 1, 1).bytes self.assertEqual(bytes, [255,255,255], msg='bytes (1,1,1)=>%s'%bytes) bytes=b2Color(0, 0, 0).bytes self.assertEqual(bytes, [0,0,0], msg='bytes (1,1,1)=>%s'%bytes) self.assertEqual(c1[0], x) self.assertEqual(c1[1], y) self.assertEqual(c1[2], z) c1.list = (x*2, y*2, z*2) self.checkAlmostEqual(c1, (x+x, y+y, z+z), msg='b2Color.list')
def Step(self, settings): Framework.Step(self, settings) renderer = self.renderer try: poly = b2PolygonShape(vertices=self.verts) except AssertionError as ex: self.Print('b2PolygonShape failed: %s' % ex) else: self.Print('Valid: %s' % poly.valid) renderer.DrawPolygon([renderer.to_screen(v) for v in self.verts], b2Color(0.9, 0.9, 0.9)) for i, v in enumerate(self.verts): renderer.DrawPoint(renderer.to_screen(v), 2.0, b2Color(0.9, 0.5, 0.5)) x, y = renderer.to_screen(v) self.DrawStringAt(x + 0.05, y + 0.05, '%d' % i) if self.auto: self.generate()
def DrawShape(self, shape, transform, color, selected=False): """ Draw any type of shape """ cache_hit = False if hash(shape) in self.item_cache: cache_hit = True items = self.item_cache[hash(shape)] items[0].setRotation(transform.angle * 180.0 / b2_pi) if isinstance(shape, b2CircleShape): radius = shape.radius if items[0].radius == radius: center = b2Mul(transform, shape.pos) items[0].setPos(*center) line = items[1] axis = transform.R.x_axis line.setLine(center[0], center[1], (center[0] - radius * axis[0]), (center[1] - radius * axis[1])) else: self._remove_from_cache(shape) cache_hit = False else: items[0].setPos(*transform.position) if not selected or cache_hit: return if selected: color = b2Color(1, 1, 1) temporary = True else: temporary = False if isinstance(shape, b2PolygonShape): self.DrawPolygonShape(shape, transform, color, temporary) elif isinstance(shape, b2EdgeShape): v1 = b2Mul(transform, shape.vertex1) v2 = b2Mul(transform, shape.vertex2) self.DrawSegment(v1, v2, color) elif isinstance(shape, b2CircleShape): self.DrawCircleShape(shape, transform, color, temporary) elif isinstance(shape, b2LoopShape): vertices = shape.vertices v1 = b2Mul(transform, vertices[-1]) for v2 in vertices: v2 = b2Mul(transform, v2) self.DrawSegment(v1, v2, color) v1 = v2
def Step(self, settings): Framework.Step(self, settings) if self.go and settings.hz > 0.0: self.time += 1.0 / settings.hz linear_offset = (6 * sin(2.0 * self.time), 8.0 + 4.0 * sin(1.0 * self.time)) angular_offset = 4.0 * self.time self.joint.linearOffset = linear_offset self.joint.angularOffset = angular_offset renderer = self.renderer renderer.DrawPoint(renderer.to_screen( linear_offset), 4, b2Color(0.9, 0.9, 0.9))
def Step(self, settings): Framework.Step(self, settings) if self.go and settings.hz > 0.0: self.time += 1.0 / settings.hz linear_offset =self.qross.position# (6 * sin(2.0 * self.time), 8.0 + #4.0 * sin(1.0 * self.time)) angular_offset = -2.0 * self.time center=[self.qross.position] print(type(center)) self.joint.linearOffset = linear_offset#linear_offset self.joint.angularOffset = angular_offset renderer = self.renderer renderer.DrawPoint(renderer.to_screen( linear_offset), 4, b2Color(0.9, 0.9, 0.9)) self.viewCenter=(self.qross.position)
def _draw_joint(self, joint): bodyA, bodyB = joint.bodyA, joint.bodyB xf1, xf2 = bodyA.transform, bodyB.transform x1, x2 = xf1.position, xf2.position p1, p2 = joint.anchorA, joint.anchorB color = b2Color(0.5, 0.8, 0.8) x1, x2, p1, p2 = self._fix_vertices((x1 * self._ppm, x2 * self._ppm, p1 * self._ppm, p2 * self._ppm)) if isinstance(joint, b2DistanceJoint): cv2.line(self.screen, cvcoord(p1), cvcoord(p2), cvcolor(color), 1) elif isinstance(joint, b2PulleyJoint): s1, s2 = joint.groundAnchorA, joint.groundAnchorB s1, s2 = self._fix_vertices((s1 * self._ppm, s2 * self._ppm)) cv2.line(self.screen, cvcoord(s1), cvcoord(p1), cvcolor(color), 1) cv2.line(self.screen, cvcoord(s2), cvcoord(p2), cvcolor(color), 1) cv2.line(self.screen, cvcoord(s1), cvcoord(s2), cvcolor(color), 1) elif isinstance(joint, b2MouseJoint): pass # don't draw it here else: cv2.line(self.screen, cvcoord(x1), cvcoord(p1), cvcolor(color), 1) cv2.line(self.screen, cvcoord(p1), cvcoord(p2), cvcolor(color), 1) cv2.line(self.screen, cvcoord(x2), cvcoord(p2), cvcolor(color), 1)
def DrawJoint(self, joint): """ Draw any type of joint """ bodyA, bodyB = joint.bodyA, joint.bodyB xf1, xf2 = bodyA.transform, bodyB.transform x1, x2 = xf1.position, xf2.position p1, p2 = joint.anchorA, joint.anchorB color = b2Color(0.5, 0.8, 0.8) if isinstance(joint, b2DistanceJoint): self.DrawSegment(p1, p2, color) elif isinstance(joint, b2PulleyJoint): s1, s2 = joint.groundAnchorA, joint.groundAnchorB self.DrawSegment(s1, p1, color) self.DrawSegment(s2, p2, color) self.DrawSegment(s1, s2, color) elif isinstance(joint, b2MouseJoint): pass # don't draw it here else: self.DrawSegment(x1, p1, color) self.DrawSegment(p1, p2, color) self.DrawSegment(x2, p2, color)
def _draw_joint(self, joint): bodyA, bodyB = joint.bodyA, joint.bodyB xf1, xf2 = bodyA.transform, bodyB.transform x1, x2 = xf1.position, xf2.position p1, p2 = joint.anchorA, joint.anchorB color = b2Color(0.5, 0.8, 0.8) x1, x2, p1, p2 = self._fix_vertices( (x1 * self._ppm, x2 * self._ppm, p1 * self._ppm, p2 * self._ppm)) if isinstance(joint, b2DistanceJoint): cv2.line(self.screen, cvcoord(p1), cvcoord(p2), cvcolor(color), 1) elif isinstance(joint, b2PulleyJoint): s1, s2 = joint.groundAnchorA, joint.groundAnchorB s1, s2 = self._fix_vertices((s1 * self._ppm, s2 * self._ppm)) cv2.line(self.screen, cvcoord(s1), cvcoord(p1), cvcolor(color), 1) cv2.line(self.screen, cvcoord(s2), cvcoord(p2), cvcolor(color), 1) cv2.line(self.screen, cvcoord(s1), cvcoord(s2), cvcolor(color), 1) elif isinstance(joint, b2MouseJoint): pass # don't draw it here else: cv2.line(self.screen, cvcoord(x1), cvcoord(p1), cvcolor(color), 1) cv2.line(self.screen, cvcoord(p1), cvcoord(p2), cvcolor(color), 1) cv2.line(self.screen, cvcoord(x2), cvcoord(p2), cvcolor(color), 1)
class FrameworkBase(b2ContactListener): """ The base of the main testbed framework. If you are planning on using the testbed framework and: * Want to implement your own renderer (other than Pygame, etc.): You should derive your class from this one to implement your own tests. See empty.py or any of the other tests for more information. * Do NOT want to implement your own renderer: You should derive your class from Framework. The renderer chosen in fwSettings (see settings.py) or on the command line will automatically be used for your test. """ name = "None" description = None TEXTLINE_START = 30 colors = { 'mouse_point': b2Color(0, 1, 0), 'joint_line': b2Color(0.8, 0.8, 0.8), 'contact_add': b2Color(0.3, 0.95, 0.3), 'contact_persist': b2Color(0.3, 0.3, 0.95), 'contact_normal': b2Color(0.4, 0.9, 0.4), } def __reset(self): """ Reset all of the variables to their starting values. Not to be called except at initialization.""" # Box2D-related self.points = [] self.world = None self.mouseJoint = None # self.settings = fwSettings self.mouseWorld = None self.using_contacts = False self.stepCount = 0 # Box2D-callbacks # TODO: add raycast here or not? self.raycastListener = None self.destructionListener = None self.renderer = None def __init__(self): super(FrameworkBase, self).__init__() self.__reset() # Box2D Initialization self.world = b2World(gravity=(0, 0), doSleep=True) self.destructionListener = fwDestructionListener(test=self) self.raycastListener = RayCastClosestCallback() self.world.destructionListener = self.destructionListener self.world.contactListener = self self.t_steps, self.t_draws = [], [] def __del__(self): pass def Step(self, settings): """ The main physics step. Takes care of physics drawing (callbacks are executed after the world.Step() ) and drawing additional information. """ self.stepCount += 1 # Don't do anything if the setting's Hz are <= 0 if settings.hz > 0.0: timeStep = 1.0 / settings.hz else: timeStep = 0.0 renderer = self.renderer # If paused, display so if settings.pause: if settings.singleStep: settings.singleStep = False else: timeStep = 0.0 self.Print("****PAUSED****", (200, 0, 0)) # Set the flags based on what the settings show if renderer: # convertVertices is only applicable when using b2DrawExtended. It # indicates that the C code should transform box2d coords to screen # coordinates. is_extended = isinstance(renderer, b2DrawExtended) renderer.flags = dict( drawShapes=settings.drawShapes, drawJoints=settings.drawJoints, drawAABBs=settings.drawAABBs, drawPairs=settings.drawPairs, drawCOMs=settings.drawCOMs, convertVertices=is_extended, ) # Set the other settings that aren't contained in the flags self.world.warmStarting = settings.enableWarmStarting self.world.continuousPhysics = settings.enableContinuous self.world.subStepping = settings.enableSubStepping # Reset the collision points self.points = [] # Tell Box2D to step t_step = time() self.world.Step(timeStep, settings.velocityIterations, settings.positionIterations) self.world.ClearForces() t_step = time() - t_step # Update the debug draw settings so that the vertices will be properly # converted to screen coordinates t_draw = time() if renderer is not None: renderer.StartDraw() # self.world.DrawDebugData() # Take care of additional drawing (fps, mouse joint, slingshot bomb, # contact points) if renderer: # If there's a mouse joint, draw the connection between the object # and the current pointer position. if self.mouseJoint: p1 = renderer.to_screen(self.mouseJoint.anchorB) p2 = renderer.to_screen(self.mouseJoint.target) renderer.DrawPoint(p1, settings.pointSize, self.colors['mouse_point']) renderer.DrawPoint(p2, settings.pointSize, self.colors['mouse_point']) renderer.DrawSegment(p1, p2, self.colors['joint_line']) # Draw each of the contact points in different colors. if self.settings.drawContactPoints: for point in self.points: if point['state'] == b2_addState: renderer.DrawPoint( renderer.to_screen(point['position']), settings.pointSize, self.colors['contact_add']) elif point['state'] == b2_persistState: renderer.DrawPoint( renderer.to_screen(point['position']), settings.pointSize, self.colors['contact_persist']) if settings.drawContactNormals: for point in self.points: p1 = renderer.to_screen(point['position']) p2 = renderer.axisScale * point['normal'] + p1 renderer.DrawSegment(p1, p2, self.colors['contact_normal']) renderer.EndDraw() t_draw = time() - t_draw t_draw = max(b2_epsilon, t_draw) t_step = max(b2_epsilon, t_step) try: self.t_draws.append(1.0 / t_draw) except: pass else: if len(self.t_draws) > 2: self.t_draws.pop(0) try: self.t_steps.append(1.0 / t_step) except: pass else: if len(self.t_steps) > 2: self.t_steps.pop(0) if settings.drawFPS: self.Print("Combined FPS %d" % self.fps) if settings.drawStats: self.Print("bodies=%d contacts=%d joints=%d proxies=%d" % (self.world.bodyCount, self.world.contactCount, self.world.jointCount, self.world.proxyCount)) self.Print("hz %d vel/pos iterations %d/%d" % (settings.hz, settings.velocityIterations, settings.positionIterations)) if self.t_draws and self.t_steps: self.Print( "Potential draw rate: %.2f fps Step rate: %.2f Hz" "" % (sum(self.t_draws) / len(self.t_draws), sum(self.t_steps) / len(self.t_steps))) def ShiftMouseDown(self, p): """ Indicates that there was a left click at point p (world coordinates) with the left shift key being held down. """ pass def MouseDown(self, p): """ Indicates that there was a left click at point p (world coordinates) """ if self.mouseJoint is not None: return self.env.MouseDown(p) def MouseUp(self, p): """ Left mouse button up. """ pass def MouseMove(self, p): """ Mouse moved to point p, in world coordinates. """ self.env.MouseMove(p) # self.mouseWorld = p # if self.mouseJoint: # self.mouseJoint.target = p pass def SimulationLoop(self): """ The main simulation loop. Don't override this, override Step instead. """ self.Step(self.settings) def ConvertScreenToWorld(self, x, y): """ Return a b2Vec2 in world coordinates of the passed in screen coordinates x, y NOTE: Renderer subclasses must implement this """ raise NotImplementedError() def DrawStringAt(self, x, y, str, color=(229, 153, 153, 255)): """ Draw some text, str, at screen coordinates (x, y). NOTE: Renderer subclasses must implement this """ raise NotImplementedError() def Print(self, str, color=(229, 153, 153, 255)): """ Draw some text at the top status lines and advance to the next line. NOTE: Renderer subclasses must implement this """ raise NotImplementedError() def PreSolve(self, contact, old_manifold): """ This is a critical function when there are many contacts in the world. It should be optimized as much as possible. """ if not (self.settings.drawContactPoints or self.settings.drawContactNormals or self.using_contacts): return elif len(self.points) > self.settings.maxContactPoints: return manifold = contact.manifold if manifold.pointCount == 0: return state1, state2 = b2GetPointStates(old_manifold, manifold) if not state2: return worldManifold = contact.worldManifold # TODO: find some way to speed all of this up. self.points.extend([ dict( fixtureA=contact.fixtureA, fixtureB=contact.fixtureB, position=worldManifold.points[i], normal=worldManifold.normal.copy(), state=state2[i], ) for i, point in enumerate(state2) ]) # These can/should be implemented in the test subclass: (Step() also if necessary) # See empty.py (Box2D) for a simple example. def BeginContact(self, contact): # print("hit") self.env.BeginContact(contact.fixtureA.body.userData, contact.fixtureB.body.userData) pass def EndContact(self, contact): pass def PostSolve(self, contact, impulse): pass def FixtureDestroyed(self, fixture): """ Callback indicating 'fixture' has been destroyed. """ pass def JointDestroyed(self, joint): """ Callback indicating 'joint' has been destroyed. """ pass def Keyboard(self, key): """ Callback indicating 'key' has been pressed down. """ pass def KeyboardUp(self, key): """ Callback indicating 'key' has been released. """ pass
class FrameworkBase(b2ContactListener): """ The base of the main testbed framework. If you are planning on using the testbed framework and: * Want to implement your own renderer (other than Pygame, etc.): You should derive your class from this one to implement your own tests. See empty.py or any of the other tests for more information. * Do NOT want to implement your own renderer: You should derive your class from Framework. The renderer chosen in fwSettings (see settings.py) or on the command line will automatically be used for your test. """ name = "None" description = None TEXTLINE_START = 30 colors = { 'mouse_point': b2Color(0, 1, 0), 'bomb_center': b2Color(0, 0, 1.0), 'bomb_line': b2Color(0, 1.0, 1.0), 'joint_line': b2Color(0.8, 0.8, 0.8), 'contact_add': b2Color(0.3, 0.95, 0.3), 'contact_persist': b2Color(0.3, 0.3, 0.95), 'contact_normal': b2Color(0.4, 0.9, 0.4), } def __reset(self): """ Reset all of the variables to their starting values. Not to be called except at initialization.""" # Box2D-related self.points = [] self.world = None self.bomb = None self.mouseJoint = None self.settings = fwSettings self.bombSpawning = False self.bombSpawnPoint = None self.mouseWorld = None self.using_contacts = False self.stepCount = 0 # Box2D-callbacks self.destructionListener = None self.renderer = None def __init__(self): super(FrameworkBase, self).__init__() self.__reset() # Box2D Initialization self.world = b2World(gravity=(0, -10), doSleep=True) self.destructionListener = fwDestructionListener(test=self) self.world.destructionListener = self.destructionListener self.world.contactListener = self self.t_steps, self.t_draws = [], [] def __del__(self): pass def Step(self, settings): """ The main physics step. Takes care of physics drawing (callbacks are executed after the world.Step() ) and drawing additional information. """ self.stepCount += 1 # Don't do anything if the setting's Hz are <= 0 if settings.hz > 0.0: timeStep = 1.0 / settings.hz else: timeStep = 0.0 renderer = self.renderer # If paused, display so if settings.pause: if settings.singleStep: settings.singleStep = False else: timeStep = 0.0 self.Print("****PAUSED****", (200, 0, 0)) # Set the flags based on what the settings show if renderer: # convertVertices is only applicable when using b2DrawExtended. It # indicates that the C code should transform box2d coords to screen # coordinates. is_extended = isinstance(renderer, b2DrawExtended) renderer.flags = dict( drawShapes=settings.drawShapes, drawJoints=settings.drawJoints, drawAABBs=settings.drawAABBs, drawPairs=settings.drawPairs, drawCOMs=settings.drawCOMs, convertVertices=is_extended, ) # Set the other settings that aren't contained in the flags self.world.warmStarting = settings.enableWarmStarting self.world.continuousPhysics = settings.enableContinuous self.world.subStepping = settings.enableSubStepping # Set the thresholds self.world.velocityThreshold = settings.velocityThreshold self.world.positionThreshold = settings.positionThreshold # Reset the collision points self.points = [] # Tell Box2D to step t_step = time() self.world.Step(timeStep, settings.velocityIterations, settings.positionIterations) self.world.ClearForces() t_step = time() - t_step # Update the debug draw settings so that the vertices will be properly # converted to screen coordinates t_draw = time() if renderer is not None: renderer.StartDraw() self.world.DrawDebugData() # If the bomb is frozen, get rid of it. if self.bomb and not self.bomb.awake: self.world.DestroyBody(self.bomb) self.bomb = None # Take care of additional drawing (fps, mouse joint, slingshot bomb, # contact points) if renderer: # If there's a mouse joint, draw the connection between the object # and the current pointer position. if self.mouseJoint: p1 = renderer.to_screen(self.mouseJoint.anchorB) p2 = renderer.to_screen(self.mouseJoint.target) renderer.DrawPoint(p1, settings.pointSize, self.colors['mouse_point']) renderer.DrawPoint(p2, settings.pointSize, self.colors['mouse_point']) renderer.DrawSegment(p1, p2, self.colors['joint_line']) # Draw the slingshot bomb if self.bombSpawning: renderer.DrawPoint(renderer.to_screen(self.bombSpawnPoint), settings.pointSize, self.colors['bomb_center']) renderer.DrawSegment(renderer.to_screen(self.bombSpawnPoint), renderer.to_screen(self.mouseWorld), self.colors['bomb_line']) # Draw each of the contact points in different colors. if self.settings.drawContactPoints: for point in self.points: if point['state'] == b2_addState: renderer.DrawPoint( renderer.to_screen(point['position']), settings.pointSize, self.colors['contact_add']) elif point['state'] == b2_persistState: renderer.DrawPoint( renderer.to_screen(point['position']), settings.pointSize, self.colors['contact_persist']) if settings.drawContactNormals: for point in self.points: p1 = renderer.to_screen(point['position']) p2 = renderer.axisScale * point['normal'] + p1 renderer.DrawSegment(p1, p2, self.colors['contact_normal']) renderer.EndDraw() t_draw = time() - t_draw t_draw = max(b2_epsilon, t_draw) t_step = max(b2_epsilon, t_step) try: self.t_draws.append(1.0 / t_draw) except: pass else: if len(self.t_draws) > 2: self.t_draws.pop(0) try: self.t_steps.append(1.0 / t_step) except: pass else: if len(self.t_steps) > 2: self.t_steps.pop(0) if settings.drawFPS: self.Print("Combined FPS %d" % self.fps) if settings.drawStats: self.Print("bodies=%d contacts=%d joints=%d proxies=%d" % (self.world.bodyCount, self.world.contactCount, self.world.jointCount, self.world.proxyCount)) self.Print("hz %d vel/pos iterations %d/%d" % (settings.hz, settings.velocityIterations, settings.positionIterations)) if self.t_draws and self.t_steps: self.Print( "Potential draw rate: %.2f fps Step rate: %.2f Hz" "" % (sum(self.t_draws) / len(self.t_draws), sum(self.t_steps) / len(self.t_steps))) def ShiftMouseDown(self, p): """ Indicates that there was a left click at point p (world coordinates) with the left shift key being held down. """ self.mouseWorld = p if not self.mouseJoint: self.SpawnBomb(p) def MouseDown(self, p): """ Indicates that there was a left click at point p (world coordinates) """ if self.mouseJoint is not None: return # Create a mouse joint on the selected body (assuming it's dynamic) # Make a small box. aabb = b2AABB(lowerBound=p - (0.001, 0.001), upperBound=p + (0.001, 0.001)) # Query the world for overlapping shapes. query = fwQueryCallback(p) self.world.QueryAABB(query, aabb) if query.fixture: body = query.fixture.body # A body was selected, create the mouse joint self.mouseJoint = self.world.CreateMouseJoint( bodyA=self.groundbody, bodyB=body, target=p, maxForce=1000.0 * body.mass) body.awake = True def MouseUp(self, p): """ Left mouse button up. """ if self.mouseJoint: self.world.DestroyJoint(self.mouseJoint) self.mouseJoint = None if self.bombSpawning: self.CompleteBombSpawn(p) def MouseMove(self, p): """ Mouse moved to point p, in world coordinates. """ self.mouseWorld = p if self.mouseJoint: self.mouseJoint.target = p def SpawnBomb(self, worldPt): """ Begins the slingshot bomb by recording the initial position. Once the user drags the mouse and releases it, then CompleteBombSpawn will be called and the actual bomb will be released. """ self.bombSpawnPoint = worldPt.copy() self.bombSpawning = True def CompleteBombSpawn(self, p): """ Create the slingshot bomb based on the two points (from the worldPt passed to SpawnBomb to p passed in here) """ if not self.bombSpawning: return multiplier = 30.0 vel = self.bombSpawnPoint - p vel *= multiplier self.LaunchBomb(self.bombSpawnPoint, vel) self.bombSpawning = False def LaunchBomb(self, position, velocity): """ A bomb is a simple circle which has the specified position and velocity. position and velocity must be b2Vec2's. """ if self.bomb: self.world.DestroyBody(self.bomb) self.bomb = None self.bomb = self.world.CreateDynamicBody( allowSleep=True, position=position, linearVelocity=velocity, fixtures=b2FixtureDef(shape=b2CircleShape(radius=0.3), density=20, restitution=0.1)) def LaunchRandomBomb(self): """ Create a new bomb and launch it at the testbed. """ p = b2Vec2(b2Random(-15.0, 15.0), 30.0) v = -5.0 * p self.LaunchBomb(p, v) def SimulationLoop(self): """ The main simulation loop. Don't override this, override Step instead. """ # Reset the text line to start the text from the top self.textLine = self.TEXTLINE_START # Draw the name of the test running self.Print(self.name, (127, 127, 255)) if self.description: # Draw the name of the test running for s in self.description.split('\n'): self.Print(s, (127, 255, 127)) # Do the main physics step self.Step(self.settings) def ConvertScreenToWorld(self, x, y): """ Return a b2Vec2 in world coordinates of the passed in screen coordinates x, y NOTE: Renderer subclasses must implement this """ raise NotImplementedError() def DrawStringAt(self, x, y, str, color=(229, 153, 153, 255)): """ Draw some text, str, at screen coordinates (x, y). NOTE: Renderer subclasses must implement this """ raise NotImplementedError() def Print(self, str, color=(229, 153, 153, 255)): """ Draw some text at the top status lines and advance to the next line. NOTE: Renderer subclasses must implement this """ raise NotImplementedError() def PreSolve(self, contact, old_manifold): """ This is a critical function when there are many contacts in the world. It should be optimized as much as possible. """ if not (self.settings.drawContactPoints or self.settings.drawContactNormals or self.using_contacts): return elif len(self.points) > self.settings.maxContactPoints: return manifold = contact.manifold if manifold.pointCount == 0: return state1, state2 = b2GetPointStates(old_manifold, manifold) if not state2: return worldManifold = contact.worldManifold # TODO: find some way to speed all of this up. self.points.extend([ dict( fixtureA=contact.fixtureA, fixtureB=contact.fixtureB, position=worldManifold.points[i], normal=worldManifold.normal.copy(), state=state2[i], ) for i, point in enumerate(state2) ]) # These can/should be implemented in the test subclass: (Step() also if necessary) # See empty.py for a simple example. def BeginContact(self, contact): pass def EndContact(self, contact): pass def PostSolve(self, contact, impulse): pass def FixtureDestroyed(self, fixture): """ Callback indicating 'fixture' has been destroyed. """ pass def JointDestroyed(self, joint): """ Callback indicating 'joint' has been destroyed. """ pass def Keyboard(self, key): """ Callback indicating 'key' has been pressed down. """ pass def KeyboardUp(self, key): """ Callback indicating 'key' has been released. """ pass
def step(self, actions=None): # print(settings) if self.done: # "Episode is finished." self.quit() # TODO: is quit() necessary? if actions == None: actions = {} for agent in self.agents: if not agent.alive: continue act = agent.actor(self.obs[agent.id]) actions[agent.id] = act assert self.action_space.contains(actions) # Agent actions for agent in self.agents: if not agent.alive: continue # 0,1,2 = CW, NOOP, CCW # Rotation agent.body.angle = ( agent.body.angle + (actions[agent.id][2] - 1) * agent.rotation_speed * (1 / self.settings.hz)) if np.abs(agent.body.angle) > np.pi: agent.body.angle -= np.sign(agent.body.angle) * (2 * np.pi) # Movement angle = agent.body.angle c = 1 / np.sqrt(2) if ((actions[agent.id][0] != 1) and (actions[agent.id][1] != 1)) else 1 x_force = (np.cos(angle) * (actions[agent.id][0] - 1) + np.cos(angle + np.pi / 2) * (actions[agent.id][1] - 1) ) * c * agent.force #* (1/self.settings.hz) y_force = (np.sin(angle) * (actions[agent.id][0] - 1) + np.sin(angle + np.pi / 2) * (actions[agent.id][1] - 1) ) * c * agent.force #* (1/self.settings.hz) agent.body.ApplyForce(force=(x_force, y_force), point=agent.body.position, wake=True) # Attack if agent.cooldown_atk <= 0: if actions[agent.id][3]: point1 = agent.body.position d = (agent.melee_range * np.cos(agent.body.angle), agent.melee_range * np.sin(agent.body.angle)) point2 = point1 + d self.framework.world.RayCast( self.framework.raycastListener, point1, point2) if self.framework.renderer: self.framework.renderer.DrawSegment( point1, point2, b2Color(0.8, 0.8, 0.8)) agent.cooldown_atk = self.cooldown_atk agent.cooldown_mov_penalty = self.cooldown_mov_penalty if self.framework.raycastListener.hit: self.framework.raycastListener.fixture.body.userData.health -= agent.melee_dmg else: agent.cooldown_atk -= (1 / self.settings.hz) # Agent deaths for agent in self.agents: if not agent.alive: continue if agent.health <= 0: agent.alive = False agent.body.active = False self.n_alive[agent.team] -= 1 # We recreate action and obs space when n of agents change self.create_space() # super(World, self).Step(self.settings) self.framework.Step(self.settings) self.obs = self.get_obs() rewards, done = None, None self.time_passed += (1 / self.settings.hz) alive_teams = [i for i, e in enumerate(self.n_alive) if e != 0] if (self.time_passed > self.time_limit): self.done = True # print("Time is up") if len(alive_teams) == 1: self.done = True self.winner = alive_teams[0] # print("winner: team %s" % alive_teams[0]) if len(alive_teams) == 0: self.done = True # print("Draw") return self.obs
from Box2D import b2Color TARGET_FPS = 60 TIME_STEP = 0.01 / TARGET_FPS DRAW_GRAPHICS = True #TARGET_FPS = 60 ZOOM = 5.0 FIELD_H = (174) # (1) Height of the screen pygame with the Box2D FIELD_W = (134) # (2) Width of the screen pygame with the Box2D CORRECTION_FATOR_METER_TO_CM = 10**2 # <==== Very important: 1 m = 1*10^2 cm MAX_ROBOTS_ALLIES = 5 # CENTER_AXIS_X = -0 # CENTER_AXIS_Y = 0 # Proprerties of bodies MASS_BALL = 0.046 # Kg MASS_ROBOT = 5.100 # Kg (adjust later) # --- Colors --- BLUE = b2Color(0,0,1) YELLOW = b2Color(1,1,0) WHITE = b2Color(1,1,1)
class Raycast(Framework): name = "Raycast" description = "Press 1-5 to drop stuff, d to delete, m to switch callback modes" p1_color = b2Color(0.4, 0.9, 0.4) s1_color = b2Color(0.8, 0.8, 0.8) s2_color = b2Color(0.9, 0.9, 0.4) def __init__(self): super(Raycast, self).__init__() self.world.gravity = (0, 0) # The ground ground = self.world.CreateBody(shapes=b2EdgeShape(vertices=[(-40, 0), (40, 0)])) # The various shapes w = 1.0 b = w / (2.0 + sqrt(2.0)) s = sqrt(2.0) * b self.shapes = [ b2PolygonShape(vertices=[(-0.5, 0), (0.5, 0), (0, 1.5)]), b2PolygonShape(vertices=[(-0.1, 0), (0.1, 0), (0, 1.5)]), b2PolygonShape( vertices=[(0.5 * s, 0), (0.5 * w, b), (0.5 * w, b + s), (0.5 * s, w), (-0.5 * s, w), (-0.5 * w, b + s), (-0.5 * w, b), (-0.5 * s, 0.0)]), b2PolygonShape(box=(0.5, 0.5)), b2CircleShape(radius=0.5), ] self.angle = 0 self.callbacks = [ RayCastClosestCallback, RayCastAnyCallback, RayCastMultipleCallback ] self.callback_class = self.callbacks[0] def CreateShape(self, shapeindex): try: shape = self.shapes[shapeindex] except IndexError: return pos = (10.0 * (2.0 * random() - 1.0), 10.0 * (2.0 * random())) defn = b2BodyDef( type=b2_dynamicBody, fixtures=b2FixtureDef(shape=shape, friction=0.3), position=pos, angle=(b2_pi * (2.0 * random() - 1.0)), ) if isinstance(shape, b2CircleShape): defn.angularDamping = 0.02 self.world.CreateBody(defn) def DestroyBody(self): for body in self.world.bodies: if not self.world.locked: self.world.DestroyBody(body) break def Keyboard(self, key): if key in (Keys.K_1, Keys.K_2, Keys.K_3, Keys.K_4, Keys.K_5): self.CreateShape(key - Keys.K_1) elif key == Keys.K_d: self.DestroyBody() elif key == Keys.K_m: idx = ((self.callbacks.index(self.callback_class) + 1) % len(self.callbacks)) self.callback_class = self.callbacks[idx] def Step(self, settings): super(Raycast, self).Step(settings) def draw_hit(cb_point, cb_normal): cb_point = self.renderer.to_screen(cb_point) head = b2Vec2(cb_point) + 0.5 * cb_normal cb_normal = self.renderer.to_screen(cb_normal) self.renderer.DrawPoint(cb_point, 5.0, self.p1_color) self.renderer.DrawSegment(point1, cb_point, self.s1_color) self.renderer.DrawSegment(cb_point, head, self.s2_color) # Set up the raycast line length = 11 point1 = b2Vec2(0, 10) d = (length * cos(self.angle), length * sin(self.angle)) point2 = point1 + d callback = self.callback_class() self.world.RayCast(callback, point1, point2) # The callback has been called by this point, and if a fixture was hit it will have been # set to callback.fixture. point1 = self.renderer.to_screen(point1) point2 = self.renderer.to_screen(point2) if callback.hit: if hasattr(callback, 'points'): for point, normal in zip(callback.points, callback.normals): draw_hit(point, normal) else: draw_hit(callback.point, callback.normal) else: self.renderer.DrawSegment(point1, point2, self.s1_color) self.Print("Callback: %s" % callback) if callback.hit: self.Print("Hit") if not settings.pause or settings.singleStep: self.angle += 0.25 * b2_pi / 180
class EdgeShapes(Framework): name = "Edge Shapes" description = "Press 1-5 to drop stuff, and d to delete" p1_color = b2Color(0.4, 0.9, 0.4) s1_color = b2Color(0.8, 0.8, 0.8) s2_color = b2Color(0.9, 0.9, 0.4) def __init__(self): super(EdgeShapes, self).__init__() self.ground = self.world.CreateStaticBody(shapes=[ b2EdgeShape(vertices=v) for v in get_sinusoid_vertices(-20.0, VERTEX_COUNT) ]) self.shapes = [ b2PolygonShape(vertices=[(-0.5, 0), (0.5, 0), (0, 1.5)]), b2PolygonShape(vertices=[(-0.1, 0), (0.1, 0), (0, 1.5)]), b2PolygonShape(vertices=get_octagon_vertices(1.0)), b2PolygonShape(box=(0.5, 0.5)), b2CircleShape(radius=0.5), ] self.angle = 0 self.callback = RayCastCallback() @property def bodies(self): return [body for body in self.world.bodies if body != self.ground] def CreateShape(self, shapeindex): try: shape = self.shapes[shapeindex] except IndexError: return pos = (10.0 * (2.0 * random() - 1.0), 10.0 * (2.0 * random() + 1.0)) defn = b2BodyDef( type=b2_dynamicBody, fixtures=b2FixtureDef(shape=shape, friction=0.3), position=pos, angle=(b2_pi * (2.0 * random() - 1.0)), ) if isinstance(shape, b2CircleShape): defn.angularDamping = 0.02 self.world.CreateBody(defn) def DestroyBody(self): if not self.world.locked: for body in self.bodies: self.world.DestroyBody(body) break def Keyboard(self, key): if key in (Keys.K_1, Keys.K_2, Keys.K_3, Keys.K_4, Keys.K_5): self.CreateShape(key - Keys.K_1) elif key == Keys.K_d: self.DestroyBody() def Step(self, settings): super(EdgeShapes, self).Step(settings) # Set up the raycast line length = 25.0 point1 = b2Vec2(0, 10) d = (length * cos(self.angle), length * sin(self.angle)) point2 = point1 + d callback = self.callback callback.fixture = None self.world.RayCast(callback, point1, point2) # The callback has been called by this point, and if a fixture was hit it will have been # set to callback.fixture. point1 = self.renderer.to_screen(point1) point2 = self.renderer.to_screen(point2) if callback.fixture: cb_point = self.renderer.to_screen(callback.point) self.renderer.DrawPoint(cb_point, 5.0, self.p1_color) self.renderer.DrawSegment(point1, cb_point, self.s1_color) head = b2Vec2(cb_point) + 0.5 * callback.normal self.renderer.DrawSegment(cb_point, head, self.s2_color) else: self.renderer.DrawSegment(point1, point2, self.s1_color) if not settings.pause or settings.singleStep: self.angle += 0.25 * b2_pi / 180
class Distance(Framework): name = "Distance" description = ("Use WASD to move and QE to rotate the small rectangle.\n" "The distance between the marked points is shown.") point_a_color = b2Color(1, 0, 0) point_b_color = b2Color(1, 1, 0) poly_color = b2Color(0.9, 0.9, 0.9) def __init__(self): super(Distance, self).__init__() # Transform A -- a simple translation/offset of (0,-0.2) self.transformA = b2Transform() self.transformA.SetIdentity() self.transformA.position = (0, -0.2) # Transform B -- a translation and a rotation self.transformB = b2Transform() self.positionB = b2Vec2(12.017401, 0.13678508) self.angleB = -0.0109265 self.transformB.Set(self.positionB, self.angleB) # The two shapes, transformed by the respective transform[A,B] self.polygonA = b2PolygonShape(box=(10, 0.2)) self.polygonB = b2PolygonShape(box=(2, 0.1)) def Step(self, settings): super(Distance, self).Step(settings) # Calculate the distance between the two shapes with the specified # transforms dist_result = b2Distance(shapeA=self.polygonA, shapeB=self.polygonB, transformA=self.transformA, transformB=self.transformB) pointA, pointB, distance, iterations = dist_result self.Print('Distance = %g' % distance) self.Print('Iterations = %d' % iterations) # Manually transform the vertices and draw the shapes for shape, transform in [(self.polygonA, self.transformA), (self.polygonB, self.transformB)]: new_verts = [ self.renderer.to_screen(transform * v) for v in shape.vertices ] self.renderer.DrawPolygon(new_verts, self.poly_color) self.renderer.DrawPoint(self.renderer.to_screen(pointA), 4, self.point_a_color) self.renderer.DrawPoint(self.renderer.to_screen(pointB), 4, self.point_b_color) def Keyboard(self, key): if key == Keys.K_a: self.positionB -= (0.1, 0) elif key == Keys.K_d: self.positionB += (0.1, 0) elif key == Keys.K_w: self.positionB += (0, 0.1) elif key == Keys.K_s: self.positionB -= (0, 0.1) elif key == Keys.K_q: self.angleB += 0.1 * b2_pi elif key == Keys.K_e: self.angleB -= 0.1 * b2_pi self.transformB.Set(self.positionB, self.angleB)
def ManualDraw(self): """ This implements code normally present in the C++ version, which calls the callbacks that you see in this class (DrawSegment, DrawSolidCircle, etc.). This is implemented in Python as an example of how to do it, and also a test. """ colors = { 'active': b2Color(0.5, 0.5, 0.3), 'static': b2Color(0.5, 0.9, 0.5), 'kinematic': b2Color(0.5, 0.5, 0.9), 'asleep': b2Color(0.6, 0.6, 0.6), 'default': b2Color(0.9, 0.7, 0.7), } settings = self.test.settings world = self.test.world if self.test.selected_shapebody: sel_shape, sel_body = self.test.selected_shapebody else: sel_shape = None if settings.drawShapes: for body in world.bodies: transform = body.transform for fixture in body.fixtures: shape = fixture.shape if not body.active: color = colors['active'] elif body.type == b2_staticBody: color = colors['static'] elif body.type == b2_kinematicBody: color = colors['kinematic'] elif not body.awake: color = colors['asleep'] else: color = colors['default'] self.DrawShape(fixture.shape, transform, color, (sel_shape == shape)) if settings.drawJoints: for joint in world.joints: self.DrawJoint(joint) # if settings.drawPairs # pass if settings.drawAABBs: color = b2Color(0.9, 0.3, 0.9) # cm = world.contactManager for body in world.bodies: if not body.active: continue transform = body.transform for fixture in body.fixtures: shape = fixture.shape for childIndex in range(shape.childCount): self.DrawAABB(shape.getAABB(transform, childIndex), color)
def Step(self, settings): super(TimeOfImpact, self).Step(settings) # b2Sweep describes the motion of a body/shape for TOI computation. # Shapes are defined with respect to the body origin, which may no # coincide with the center of mass. However, to support dynamics we # must interpolate the center of mass position. sweepA = b2Sweep(c0=(0, 0), c=(0, 0), a=0, a0=0, localCenter=(0, 0)) # The parameters of the sweep are defined as follows: # localCenter - local center of mass position # c0, c - center world positions # a0, a - world angles # t0 - time interval = [t0,1], where t0 is in [0,1] sweepB = b2Sweep(c0=(-0.20382018, 2.1368704), a0=-3.1664171, c=(-0.26699525, 2.3552670), a=-3.3926492, localCenter=(0, 0)) type_, time_of_impact = b2TimeOfImpact(shapeA=self.shapeA, shapeB=self.shapeB, sweepA=sweepA, sweepB=sweepB, tMax=1.0) # Alternative pybox2d syntax (no kwargs): # type_, t = b2TimeOfImpact(self.shapeA, self.shapeB, sweepA, sweepB, 1.0) # # And even uglier: # input=b2TOIInput(proxyA=b2DistanceProxy(shape=self.shapeA), proxyB=b2DistanceProxy(shape=self.shapeB), sweepA=sweepA, sweepB=sweepB, tMax=1.0) # type_, t = b2TimeOfImpact(input) self.Print("TOI = %g" % time_of_impact) self.Print("max toi iters = %d, max root iters = %d" % (b2Globals.b2_toiMaxIters, b2Globals.b2_toiMaxRootIters)) # Draw the shapes at their current position (t=0) # shapeA (the vertical polygon) transform = sweepA.GetTransform(0) self.renderer.DrawPolygon([self.renderer.to_screen(transform * v) for v in self.shapeA.vertices], b2Color(0.9, 0.9, 0.9)) # shapeB (the horizontal polygon) transform = sweepB.GetTransform(0) self.renderer.DrawPolygon([self.renderer.to_screen(transform * v) for v in self.shapeB.vertices], b2Color(0.5, 0.9, 0.5)) # localPoint=(2, -0.1) # rB = transform * localPoint - sweepB.c0 # wB = sweepB.a - sweepB.a0 # vB = sweepB.c - sweepB.c0 # v = vB + b2Cross(wB, rB) # Now, draw shapeB in a different color when they would collide (i.e., # at t=time of impact) This shows that the polygon would rotate upon # collision transform = sweepB.GetTransform(time_of_impact) self.renderer.DrawPolygon([self.renderer.to_screen(transform * v) for v in self.shapeB.vertices], b2Color(0.5, 0.7, 0.9)) # And finally, draw shapeB at t=1.0, where it would be if it did not # collide with shapeA In this case, time_of_impact = 1.0, so these # become the same polygon. transform = sweepB.GetTransform(1.0) self.renderer.DrawPolygon([self.renderer.to_screen(transform * v) for v in self.shapeB.vertices], b2Color(0.9, 0.5, 0.5))
def ManualDraw(self): """ This implements code normally present in the C++ version, which calls the callbacks that you see in this class (DrawSegment, DrawSolidCircle, etc.). This is implemented in Python as an example of how to do it, and also a test. """ colors = { 'active': b2Color(0.5, 0.5, 0.3), 'static': b2Color(0.5, 0.9, 0.5), 'kinematic': b2Color(0.5, 0.5, 0.9), 'asleep': b2Color(0.6, 0.6, 0.6), 'default': b2Color(0.9, 0.7, 0.7), } settings = self.test.settings world = self.test.world if self.test.selected_shapebody: sel_shape, sel_body = self.test.selected_shapebody else: sel_shape = None if settings.drawShapes: for body in world.bodies: transform = body.transform for fixture in body.fixtures: shape = fixture.shape if not body.active: color = colors['active'] elif body.type == b2_staticBody: color = colors['static'] elif body.type == b2_kinematicBody: color = colors['kinematic'] elif not body.awake: color = colors['asleep'] else: color = colors['default'] self.DrawShape(fixture.shape, transform, color, (sel_shape == shape)) if settings.drawJoints: for joint in world.joints: self.DrawJoint(joint) # if settings.drawPairs # pass if settings.drawAABBs: color = b2Color(0.9, 0.3, 0.9) # cm = world.contactManager for body in world.bodies: if not body.active: continue transform = body.transform for fixture in body.fixtures: shape = fixture.shape for childIndex in range(shape.childCount): self.DrawAABB(shape.getAABB( transform, childIndex), color)
class FrameworkBase(b2ContactListener): """ The base of the main testbed framework. If you are planning on using the testbed framework and: * Want to implement your own renderer (other than Pygame, etc.): You should derive your class from this one to implement your own tests. See empty.py or any of the other tests for more information. * Do NOT want to implement your own renderer: You should derive your class from Framework. The renderer chosen in fwSettings (see settings.py) or on the command line will automatically be used for your test. """ name = "None" description = None TEXTLINE_START = 30 colors = { # 'static_body': b2Color(0.7, 1, 0.4), 'mouse_point': b2Color(0, 1, 0), 'joint_line': b2Color(0.8, 0.8, 0.8), 'contact_add': b2Color(0.3, 0.95, 0.3), 'contact_persist': b2Color(0.3, 0.3, 0.95), 'contact_normal': b2Color(0.4, 0.9, 0.4), } def __reset(self): """ Reset all of the variables to their starting values. Not to be called except at initialization.""" # Box2D-related self.points = [] self.world = None self.mouseJoint = None self.settings = fwSettings self.mouseWorld = None self.using_contacts = False self.stepCount = 0 # Box2D-callbacks self.destructionListener = None self.renderer = None def __init__(self): super(FrameworkBase, self).__init__() self.__reset() # Box2D Initialization self.world = b2World(gravity=(0, -10), doSleep=True) self.destructionListener = fwDestructionListener(test=self) self.world.destructionListener = self.destructionListener self.world.contactListener = self self.t_steps, self.t_draws = [], [] def __del__(self): pass def Step(self, settings): """ The main physics step. Takes care of physics drawing (callbacks are executed after the world.Step() ) and drawing additional information. """ self.stepCount += 1 # Don't do anything if the setting's Hz are <= 0 if settings.hz > 0.0: timeStep = 1.0 / settings.hz else: timeStep = 0.0 renderer = self.renderer # If paused, display so if settings.pause: if settings.singleStep: settings.singleStep = False else: timeStep = 0.0 # Set the flags based on what the settings show if renderer: # convertVertices is only applicable when using b2DrawExtended. It # indicates that the C code should transform box2d coords to screen # coordinates. is_extended = isinstance(renderer, b2DrawExtended) renderer.flags = dict( drawShapes=settings.drawShapes, drawJoints=settings.drawJoints, drawAABBs=settings.drawAABBs, drawPairs=settings.drawPairs, drawCOMs=settings.drawCOMs, convertVertices=is_extended, ) # Set the other settings that aren't contained in the flags self.world.warmStarting = settings.enableWarmStarting self.world.continuousPhysics = settings.enableContinuous self.world.subStepping = settings.enableSubStepping # Reset the collision points self.points = [] # Tell Box2D to step t_step = time() self.world.Step(timeStep, settings.velocityIterations, settings.positionIterations) self.world.ClearForces() t_step = time() - t_step # Update the debug draw settings so that the vertices will be properly # converted to screen coordinates t_draw = time() if renderer is not None: renderer.StartDraw() self.world.DrawDebugData() # Take care of additional drawing (fps, mouse joint, slingshot bomb, # contact points) if renderer: # If there's a mouse joint, draw the connection between the object # and the current pointer position. if self.mouseJoint: p1 = renderer.to_screen(self.mouseJoint.anchorB) p2 = renderer.to_screen(self.mouseJoint.target) renderer.DrawPoint(p1, settings.pointSize, self.colors['mouse_point']) renderer.DrawPoint(p2, settings.pointSize, self.colors['mouse_point']) renderer.DrawSegment(p1, p2, self.colors['joint_line']) renderer.EndDraw() t_draw = time() - t_draw t_draw = max(b2_epsilon, t_draw) t_step = max(b2_epsilon, t_step) try: self.t_draws.append(1.0 / t_draw) except: pass else: if len(self.t_draws) > 2: self.t_draws.pop(0) try: self.t_steps.append(1.0 / t_step) except: pass else: if len(self.t_steps) > 2: self.t_steps.pop(0) def MouseDown(self, p): """ Indicates that there was a left click at point p (world coordinates) """ if self.mouseJoint is not None: return # Create a mouse joint on the selected body (assuming it's dynamic) # Make a small box. aabb = b2AABB(lowerBound=p - (0.001, 0.001), upperBound=p + (0.001, 0.001)) # Query the world for overlapping shapes. query = fwQueryCallback(p) self.world.QueryAABB(query, aabb) if query.fixture: body = query.fixture.body # A body was selected, create the mouse joint self.mouseJoint = self.world.CreateMouseJoint( bodyA=self.groundbody, bodyB=body, target=p, maxForce=(100.0**3) * body.mass) body.awake = True def rotateLeft(self, p): """ Indicates that there was a left click at point p (world coordinates) """ # if self.mouseJoint is not None: # return # Create a mouse joint on the selected body (assuming it's dynamic) # Make a small box. aabb = b2AABB(lowerBound=p - (0.001, 0.001), upperBound=p + (0.001, 0.001)) # Query the world for overlapping shapes. query = fwQueryCallback(p) self.world.QueryAABB(query, aabb) if query.fixture: body = query.fixture.body body.ApplyAngularImpulse(15**3, True) def rotateRight(self, p): """ Indicates that there was a left click at point p (world coordinates) """ # if self.mouseJoint is not None: # return # Create a mouse joint on the selected body (assuming it's dynamic) # Make a small box. aabb = b2AABB(lowerBound=p - (0.001, 0.001), upperBound=p + (0.001, 0.001)) # Query the world for overlapping shapes. query = fwQueryCallback(p) self.world.QueryAABB(query, aabb) if query.fixture: body = query.fixture.body body.ApplyAngularImpulse(-15**3, True) def MouseUp(self, p): """ Left mouse button up. """ if self.mouseJoint: self.world.DestroyJoint(self.mouseJoint) self.mouseJoint = None def MouseMove(self, p): """ Mouse moved to point p, in world coordinates. """ self.mouseWorld = p if self.mouseJoint: self.mouseJoint.target = p def SimulationLoop(self): """ The main simulation loop. Don't override this, override Step instead. """ self.Step(self.settings) def ConvertScreenToWorld(self, x, y): """ Return a b2Vec2 in world coordinates of the passed in screen coordinates x, y NOTE: Renderer subclasses must implement this """ raise NotImplementedError() def DrawStringAt(self, x, y, str, color=(229, 153, 153, 255)): """ Draw some text, str, at screen coordinates (x, y). NOTE: Renderer subclasses must implement this """ raise NotImplementedError() def PreSolve(self, contact, old_manifold): """ This is a critical function when there are many contacts in the world. It should be optimized as much as possible. """ if not (self.settings.drawContactPoints or self.settings.drawContactNormals or self.using_contacts): return elif len(self.points) > self.settings.maxContactPoints: return manifold = contact.manifold if manifold.pointCount == 0: return state1, state2 = b2GetPointStates(old_manifold, manifold) if not state2: return worldManifold = contact.worldManifold # TODO: find some way to speed all of this up. self.points.extend([ dict( fixtureA=contact.fixtureA, fixtureB=contact.fixtureB, position=worldManifold.points[i], normal=worldManifold.normal.copy(), state=state2[i], ) for i, point in enumerate(state2) ]) def destroy_bodies(self): """ Finish the bodies into world when quit button is pressed """ for body in self.world.bodies: self.world.DestroyBody(body) # These can/should be implemented in the test subclass: (Step() also if necessary) # See empty.py for a simple example. def BeginContact(self, contact): pass def EndContact(self, contact): pass def PostSolve(self, contact, impulse): pass def FixtureDestroyed(self, fixture): """ Callback indicating 'fixture' has been destroyed. """ pass def JointDestroyed(self, joint): """ Callback indicating 'joint' has been destroyed. """ pass def Keyboard(self, key): """ Callback indicating 'key' has been pressed down. """ pass def KeyboardUp(self, key): """ Callback indicating 'key' has been released. """ pass