def __init__(self, x0, target, render): self.render = render if self.render: super(CarWorld, self).__init__() else: self.world = b2.b2World(gravity=(0, 0), doSleep=True) self.world.gravity = (0.0, 0.0) self.initial_position = (x0[0], x0[1]) self.initial_angle = 0 ground = self.world.CreateBody(position=(0, 20)) ground.CreateEdgeChain( [(-10, -30), (-10, 5), (-30, 30),] ) ground.CreateEdgeChain( [(10, -30), (10, 5), (-10, 30),] ) xf1 = b2.b2Transform() xf1.angle = 0.3524 * b2.b2_pi xf1.position = b2.b2Mul(xf1.R, (1.0, 0.0)) xf2 = b2.b2Transform() xf2.angle = -0.3524 * b2.b2_pi xf2.position = b2.b2Mul(xf2.R, (-1.0, 0.0)) self.car = TDCar(self.world, position=self.initial_position, angle=self.initial_angle) self.target = self.world.CreateStaticBody( position=target[:2], angle=self.initial_angle, shapes=[b2.b2PolygonShape(vertices=[xf1*(-1, 0), xf1*(1, 0), xf1*(0, .5)]), b2.b2PolygonShape(vertices=[xf2*(-1, 0), xf2*(1, 0), xf2*(0, .5)])], ) self.target.active = False self.start = self.world.CreateStaticBody( position=self.initial_position, angle=self.initial_angle, shapes=[b2.b2PolygonShape(vertices=[xf1*(-1, 0), xf1*(1, 0), xf1*(0, .5)]), b2.b2PolygonShape(vertices=[xf2*(-1, 0), xf2*(1, 0), xf2*(0, .5)])], ) self.start.active = False
def __init__(self, x0, target, render): self.render = render if self.render: super(PointMassWorld, self).__init__() else: self.world = b2.b2World(gravity=(0, -10), doSleep=True) x0 = np.asfarray(x0) target = np.asfarray(target) self.world.gravity = (0.0, 0.0) self.initial_position = (x0[0], x0[1]) self.initial_angle = b2.b2_pi self.initial_linear_velocity = (x0[2], x0[3]) self.initial_angular_velocity = 0 ground = self.world.CreateBody(position=(0, 20)) ground.CreateEdgeChain([(-20, -20), (-20, 20), (20, 20), (20, -20), (-20, -20)]) xf1 = b2.b2Transform() xf1.angle = 0.3524 * b2.b2_pi xf1.position = b2.b2Mul(xf1.R, (1.0, 0.0)) xf2 = b2.b2Transform() xf2.angle = -0.3524 * b2.b2_pi xf2.position = b2.b2Mul(xf2.R, (-1.0, 0.0)) self.body = self.world.CreateDynamicBody( position=self.initial_position, angle=self.initial_angle, linearVelocity=self.initial_linear_velocity, angularVelocity=self.initial_angular_velocity, angularDamping=5, linearDamping=0.1, shapes=[ b2.b2PolygonShape( vertices=[xf1 * (-1, 0), xf1 * (1, 0), xf1 * (0, .5)]), b2.b2PolygonShape( vertices=[xf2 * (-1, 0), xf2 * (1, 0), xf2 * (0, .5)]) ], shapeFixture=b2.b2FixtureDef(density=1.0), ) self.target = self.world.CreateStaticBody( position=target[:2], angle=self.initial_angle, shapes=[ b2.b2PolygonShape( vertices=[xf1 * (-1, 0), xf1 * (1, 0), xf1 * (0, .5)]), b2.b2PolygonShape( vertices=[xf2 * (-1, 0), xf2 * (1, 0), xf2 * (0, .5)]) ], ) self.target.active = False
def __init__(self, x0, target): super(PointMassWorld, self).__init__() self.world.gravity = (0.0, 0.0) self.initial_position = (x0[0], x0[1]) self.initial_angle = b2.b2_pi self.initial_linear_velocity = (x0[2], x0[3]) self.initial_angular_velocity = 0 ground = self.world.CreateBody(position=(0, 20)) ground.CreateEdgeChain( [(-20, -20), (-20, 20), (20, 20), (20, -20), (-20, -20)] ) xf1 = b2.b2Transform() xf1.angle = 0.3524 * b2.b2_pi xf1.position = b2.b2Mul(xf1.R, (1.0, 0.0)) xf2 = b2.b2Transform() xf2.angle = -0.3524 * b2.b2_pi xf2.position = b2.b2Mul(xf2.R, (-1.0, 0.0)) self.body = self.world.CreateDynamicBody( position=self.initial_position, angle=self.initial_angle, linearVelocity=self.initial_linear_velocity, angularVelocity=self.initial_angular_velocity, angularDamping=5, linearDamping=0.1, shapes=[b2.b2PolygonShape(vertices=[xf1*(-1, 0), xf1*(1, 0), xf1*(0, .5)]), b2.b2PolygonShape(vertices=[xf2*(-1, 0), xf2*(1, 0), xf2*(0, .5)])], shapeFixture=b2.b2FixtureDef(density=1.0), ) self.target = self.world.CreateStaticBody( position=target, angle=self.initial_angle, shapes=[b2.b2PolygonShape(vertices=[xf1*(-1, 0), xf1*(1, 0), xf1*(0, .5)]), b2.b2PolygonShape(vertices=[xf2*(-1, 0), xf2*(1, 0), xf2*(0, .5)])], ) self.target.active = False
def b2PolyToPoints(shape): ''' convert a b2Poly into a list of its points ''' points = [] for i in xrange(shape.GetVertexCount()): pt = box2d.b2Mul(shape.GetBody().GetXForm(), shape.getVertex(i)) points.append(pt) return points
def draw(self): """ If a drawing method is specified, this function passes the objects to the module in pixels. Return: True if the objects were successfully drawn False if the renderer was not set or another error occurred """ self.callbacks.start(CALLBACK_DRAWING_START) # No need to run through the loop if there's no way to draw if not self.renderer: return False if self.camera.track_body: # Get Body Center p1 = self.camera.track_body.GetWorldCenter() # Center the Camera There, False = Don't stop the tracking self.camera.center(self.to_screen( (p1.x * self.ppm, p1.y * self.ppm)), stopTrack=False) # Walk through all known elements self.renderer.start_drawing() for body in self.world.bodies: xform = body.transform shape = body.fixtures angle = body.angle if shape: userdata = body.userData if 'color' in userdata: clr = userdata['color'] else: clr = self.colors[0] for shape in body.fixtures: type_ = shape.type if type_ == box2d.b2Shape.e_circle: position = box2d.b2Mul(xform, shape.shape.pos) pos = self.to_screen( (position.x * self.ppm, position.y * self.ppm)) self.renderer.draw_circle( clr, pos, self.meter_to_screen(shape.shape.radius), angle) elif type_ == box2d.b2Shape.e_polygon: points = [] for v in shape.shape.vertices: pt = box2d.b2Mul(xform, v) x, y = self.to_screen( (pt.x * self.ppm, pt.y * self.ppm)) points.append([x, y]) self.renderer.draw_polygon(clr, points) else: print("unknown shape type:%d" % shape.type) for joint in self.world.joints: p2 = joint.anchorA p2 = self.to_screen((p2.x * self.ppm, p2.y * self.ppm)) p1 = joint.anchorB p1 = self.to_screen((p1.x * self.ppm, p1.y * self.ppm)) if p1 == p2: self.renderer.draw_circle((255, 255, 255), p1, 2, 0) if isinstance(joint, box2d.b2RevoluteJoint): self.renderer.draw_circle((255, 255, 255), p1, self.PIN_MOTOR_RADIUS, 0) else: self.renderer.draw_lines((0, 0, 0), False, [p1, p2], 3) self.callbacks.start(CALLBACK_DRAWING_END) self.renderer.after_drawing() return True
def draw(self): """ If a drawing method is specified, this function passes the objects to the module in pixels. Return: True if the objects were successfully drawn False if the renderer was not set or another error occurred """ self.callbacks.start(CALLBACK_DRAWING_START) # No need to run through the loop if there's no way to draw if not self.renderer: return False if self.camera.track_body: # Get Body Center p1 = self.camera.track_body.GetWorldCenter() # Center the Camera There, False = Don't stop the tracking self.camera.center(self.to_screen((p1.x*self.ppm, p1.y*self.ppm)), stopTrack=False) # Walk through all known elements self.renderer.start_drawing() for body in self.world.bodyList: xform = body.GetXForm() shape = body.GetShapeList() angle = body.GetAngle() if shape: userdata = body.GetUserData() clr = userdata['color'] for shape in body.shapeList: type = shape.GetType() if type == box2d.e_circleShape: position = box2d.b2Mul(xform, shape.GetLocalPosition()) pos = self.to_screen((position.x*self.ppm, position.y*self.ppm)) self.renderer.draw_circle(clr, pos, self.meter_to_screen(shape.radius), angle) elif type == box2d.e_polygonShape: points = [] for v in shape.vertices: pt = box2d.b2Mul(xform, v) x, y = self.to_screen((pt.x*self.ppm, pt.y*self.ppm)) points.append([x, y]) self.renderer.draw_polygon(clr, points) else: print " unknown shape type:%d" % shape.GetType() for joint in self.world.jointList: p2 = joint.GetAnchor1() p2 = self.to_screen((p2.x*self.ppm, p2.y*self.ppm)) p1 = joint.GetAnchor2() p1 = self.to_screen((p1.x*self.ppm, p1.y*self.ppm)) if p1 == p2: self.renderer.draw_circle((255,255,255), p1, 2, 0) else: self.renderer.draw_lines((0,0,0), False, [p1, p2], 3) self.callbacks.start(CALLBACK_DRAWING_END) self.renderer.after_drawing() return True
def draw(self): """ If a drawing method is specified, this function passes the objects to the module in pixels. Return: True if the objects were successfully drawn False if the renderer was not set or another error occurred """ self.callbacks.start(CALLBACK_DRAWING_START) # No need to run through the loop if there's no way to draw if not self.renderer: return False if self.camera.track_body: # Get Body Center p1 = self.camera.track_body.GetWorldCenter() # Center the Camera There, False = Don't stop the tracking self.camera.center(self.to_screen((p1.x * self.ppm, p1.y * self.ppm)), stopTrack=False) # Walk through all known elements self.renderer.start_drawing() for body in self.world.bodyList: xform = body.GetXForm() shape = body.GetShapeList() angle = body.GetAngle() if shape: userdata = body.GetUserData() if 'color' in userdata: clr = userdata['color'] else: clr = self.colors[0] for shape in body.shapeList: type_ = shape.GetType() if type_ == box2d.e_circleShape: position = box2d.b2Mul(xform, shape.GetLocalPosition()) pos = self.to_screen((position.x * self.ppm, position.y * self.ppm)) self.renderer.draw_circle( clr, pos, self.meter_to_screen(shape.radius), angle) elif type_ == box2d.e_polygonShape: points = [] for v in shape.vertices: pt = box2d.b2Mul(xform, v) x, y = self.to_screen((pt.x * self.ppm, pt.y * self.ppm)) points.append([x, y]) self.renderer.draw_polygon(clr, points) else: print "unknown shape type:%d" % shape.GetType() for joint in self.world.jointList: p2 = joint.GetAnchor1() p2 = self.to_screen((p2.x * self.ppm, p2.y * self.ppm)) p1 = joint.GetAnchor2() p1 = self.to_screen((p1.x * self.ppm, p1.y * self.ppm)) if isinstance(joint, box2d.b2RevoluteJoint): self.renderer.draw_circle((255, 255, 255), p1, self.PIN_MOTOR_RADIUS, 0) else: self.renderer.draw_lines((0, 0, 0), False, [p1, p2], 3) self.callbacks.start(CALLBACK_DRAWING_END) self.renderer.after_drawing() return True
def draw(self): """ If a drawing method is specified, this function passes the objects to the module in pixels. Return: True if the objects were successfully drawn False if the renderer was not set or another error occurred """ self.callbacks.start(CALLBACK_DRAWING_START) # No need to run through the loop if there's no way to draw if not self.renderer: return False if self.camera.track_body: # Get Body Center p1 = self.camera.track_body.GetWorldCenter() # Center the Camera There, False = Don't stop the tracking self.camera.center( self.to_screen( (p1.x * self.ppm, p1.y * self.ppm)), stopTrack=False) # Walk through all known elements self.renderer.start_drawing() for body in self.world.bodies: xform = body.transform shape = body.fixtures angle = body.angle if shape: userdata = body.userData clr = userdata['color'] for fixture in body.fixtures: type = fixture.type if type == box2d.b2Shape.e_circle: position = box2d.b2Mul(xform, fixture.shape.pos) pos = self.to_screen( (position.x * self.ppm, position.y * self.ppm)) self.renderer.draw_circle( clr, pos, self.meter_to_screen( fixture.shape.radius), angle) elif type == box2d.b2Shape.e_polygon: points = [] for v in fixture.shape.vertices: pt = box2d.b2Mul(xform, v) x, y = self.to_screen( (pt.x * self.ppm, pt.y * self.ppm)) points.append([x, y]) self.renderer.draw_polygon(clr, points) else: print " unknown shape type:%d" % fixture.type for joint in self.world.joints: p2 = joint.anchorA p2 = self.to_screen((p2.x * self.ppm, p2.y * self.ppm)) p1 = joint.anchorB p1 = self.to_screen((p1.x * self.ppm, p1.y * self.ppm)) if p1 == p2: self.renderer.draw_circle((255, 255, 255), p1, 2, 0) else: self.renderer.draw_lines((0, 0, 0), False, [p1, p2], 3) self.callbacks.start(CALLBACK_DRAWING_END) self.renderer.after_drawing() return True
def __init__(self, x0, world_info, target, render): self.render = render if self.render: super(PointMassWorldObstacle, self).__init__() else: self.world = b2.b2World(gravity=(0, -10), doSleep=True) self.world.gravity = (0.0, 0.0) self.initial_position = (x0[0], x0[1]) self.initial_angle = b2.b2_pi self.initial_linear_velocity = (x0[2], x0[3]) self.initial_angular_velocity = 0 ground = self.world.CreateBody(position=(0, 20)) ground.CreateEdgeChain([(-20, -20), (-20, 20), (20, 20), (20, -20), (-20, -20)]) xf1 = b2.b2Transform() xf1.angle = 0.3524 * b2.b2_pi xf1.position = b2.b2Mul(xf1.R, (1.0, 0.0)) xf2 = b2.b2Transform() xf2.angle = -0.3524 * b2.b2_pi xf2.position = b2.b2Mul(xf2.R, (-1.0, 0.0)) """ self.body_shape = [b2.b2PolygonShape(vertices=[xf1*(-1, 0), xf1*(1, 0), xf1*(0, .5)]), b2.b2PolygonShape(vertices=[xf2*(-1, 0), xf2*(1, 0), xf2*(0, .5)])] """ self.body_shape = [b2.b2PolygonShape(box=(1, 1))] self.body = self.world.CreateDynamicBody( position=self.initial_position, angle=self.initial_angle, linearVelocity=self.initial_linear_velocity, angularVelocity=self.initial_angular_velocity, angularDamping=5, linearDamping=0.1, shapes=self.body_shape, shapeFixture=b2.b2FixtureDef(density=1.0), ) self.initial_pos = self.world.CreateStaticBody( position=self.initial_position, angle=self.initial_angle, shapes=self.body_shape, ) self.target = self.world.CreateStaticBody( position=target[:2], angle=self.initial_angle, shapes=[ b2.b2PolygonShape( vertices=[xf1 * (-1, 0), xf1 * (1, 0), xf1 * (0, .5)]), b2.b2PolygonShape( vertices=[xf2 * (-1, 0), xf2 * (1, 0), xf2 * (0, .5)]) ], ) self.obstacle_post = [] self.obstacle_shape = [] self.obstacle = [] obstacles = world_info['obstacles'] self.n_obs = len(obstacles) for i in range(self.n_obs): self.obstacle_post.append(obstacles[i][:2]) self.obstacle_shape.append( [b2.b2PolygonShape(box=tuple(obstacles[i][2:]))]) self.obstacle.append( self.world.CreateStaticBody(position=self.obstacle_post[i], angle=self.initial_angle, shapes=self.obstacle_shape[i])) self.initial_pos.active = False self.target.active = False
def rotate(vec, angle, center = b2d.b2Vec2(0,0)): res = vec.copy() res -= center res = b2d.b2Mul(b2d.b2Mat22(angle), res) res += center return res