def joint(self, *args): print "* Add Joint:", args if len(args) == 5: # Tracking Joint b1, b2, p1, p2, flag = args p1 = self.to_b2vec(p1) p2 = self.to_b2vec(p2) jointDef = box2d.b2DistanceJointDef() jointDef.Initialize(b1, b2, p1, p2) jointDef.collideConnected = flag self.parent.world.CreateJoint(jointDef) elif len(args) == 4: # Distance Joint b1, b2, p1, p2 = args p1 = self.to_b2vec(p1) p2 = self.to_b2vec(p2) jointDef = box2d.b2DistanceJointDef() jointDef.Initialize(b1, b2, p1, p2) jointDef.collideConnected = True self.parent.world.CreateJoint(jointDef) elif len(args) == 3: # Revolute Joint between two bodies (unimplemented) pass elif len(args) == 2: # Revolute Joint to the Background, at point b1 = self.parent.world.groundBody b2 = args[0] p1 = self.to_b2vec(args[1]) jointDef = box2d.b2RevoluteJointDef() jointDef.Initialize(b1, b2, p1) self.parent.world.CreateJoint(jointDef) elif len(args) == 1: # Revolute Joint to the Background, body center b1 = self.parent.world.groundBody b2 = args[0] p1 = b2.GetWorldCenter() jointDef = box2d.b2RevoluteJointDef() jointDef.Initialize(b1, b2, p1) self.parent.world.CreateJoint(jointDef)
def handleEvents(self, event, bridge): #look for default events, and if none are handled then try the custom events if super(BridgeJointTool, self).handleEvents(event, bridge): return if event.type != MOUSEBUTTONUP or event.button != 1: return bodies = self.game.world.get_bodies_at_pos(event.pos, include_static=True) if not bodies or len(bodies) > 2: return jointDef = box2d.b2RevoluteJointDef() if len(bodies) == 1: if not bodies[0].type == box2d.b2_staticBody: if event.pos[1] > 550 and (event.pos[0] < 350 or event.pos[0] > 850): jointDef.Initialize(self.game.world.world.groundBody, bodies[0], self.to_b2vec(event.pos)) else: return else: return elif len(bodies) == 2: if bodies[0].type == box2d.b2_staticBody: jointDef.Initialize(self.game.world.world.groundBody, bodies[1], self.to_b2vec(event.pos)) elif bodies[1].type == box2d.b2_staticBody: jointDef.Initialize(self.game.world.world.groundBody, bodies[0], self.to_b2vec(event.pos)) else: jointDef.Initialize(bodies[0], bodies[1], self.to_b2vec(event.pos)) joint = self.game.world.world.CreateJoint(jointDef) self.game.bridge.joint_added(joint)
def load(self, size): bd=box2d.b2BodyDef() bd.position=self.shape.center[0], self.shape.center[1] sd=box2d.b2PolygonDef() sd.SetAsBox(size[0]/2.0, size[1]/2.0) sd.filter.categoryBits= BallGen.catBits self.body=WorldConfig.world.CreateBody(bd) self.body.CreateShape(sd) self.body.SetMassFromShapes() self.body.userData = self sd.density=2.0 sd.SetAsBox(0.1, 1.8) bd.isBullet=True bd.position=self.body.position.x+1, self.body.position.y+4 body_bar=WorldConfig.world.CreateBody(bd) body_bar.CreateShape(sd) body_bar.SetMassFromShapes() jd= box2d.b2RevoluteJointDef() jd.Initialize(self.body, body_bar, (self.body.position.x+1, self.body.position.y+size[1]/2.0)) jd.maxMotorTorque= 30000.0 jd.motorSpeed = 2* box2d.b2_pi jd.enableMotor = True WorldConfig.world.CreateJoint(jd).getAsType() self.body_bar=body_bar
def handleEvents(self,event): #look for default events, and if none are handled then try the custom events if super(BridgeJointTool,self).handleEvents(event): return if event.type != MOUSEBUTTONUP or event.button != 1: return bodies = self.game.world.get_bodies_at_pos(event.pos, include_static=True) if not bodies or len(bodies) > 2: return jointDef = box2d.b2RevoluteJointDef() if len(bodies) == 1: if not bodies[0].IsStatic(): if event.pos[1] > 550 and (event.pos[0] < 350 or event.pos[0] > 850): jointDef.Initialize(self.game.world.world.GetGroundBody(), bodies[0], self.to_b2vec(event.pos)) else: return else: return elif len(bodies) == 2: if bodies[0].IsStatic(): jointDef.Initialize(self.game.world.world.GetGroundBody(), bodies[1], self.to_b2vec(event.pos)) elif bodies[1].IsStatic(): jointDef.Initialize(self.game.world.world.GetGroundBody(), bodies[0], self.to_b2vec(event.pos)) else: jointDef.Initialize(bodies[0], bodies[1], self.to_b2vec(event.pos)) joint = self.game.world.world.CreateJoint(jointDef) self.game.bridge.joint_added(joint)
def __init__(self,figura_1, figura_2, figura_1_punto=(0,0), figura_2_punto=(0,0), angulo_minimo=None,angulo_maximo=None, fisica=None, con_colision=True): """ Inicializa la constante :param figura_1: Una de las figuras a conectar por la constante. :param figura_2: La otra figura a conectar por la constante. :param figura_1_punto: Punto de rotación de figura_1 :param figura_2_punto: Punto de rotación de figura_2 :param angulo_minimo: Angulo minimo de rotacion para figura_2 con respecto a figura_1_punto :param angulo_maximo: Angulo maximo de rotacion para figura_2 con respecto a figura_1_punto :param fisica: Referencia al motor de física. :param con_colision: Indica si se permite colisión entre las dos figuras. """ if not fisica: fisica = pilas.escena_actual().fisica if not isinstance(figura_1, Figura) or not isinstance(figura_2, Figura): raise Exception("Las dos figuras tienen que ser objetos de la clase Figura.") constante = box2d.b2RevoluteJointDef() constante.Initialize(bodyA=figura_1._cuerpo, bodyB=figura_2._cuerpo,anchor=(0,0)) constante.localAnchorA = convertir_a_metros(figura_1_punto[0]), convertir_a_metros(figura_1_punto[1]) constante.localAnchorB = convertir_a_metros(figura_2_punto[0]), convertir_a_metros(figura_2_punto[1]) if angulo_minimo != None or angulo_maximo != None: constante.enableLimit = True constante.lowerAngle = math.radians(angulo_minimo) constante.upperAngle = math.radians(angulo_maximo) constante.collideConnected = con_colision self.constante = fisica.mundo.CreateJoint(constante)
def jointMotor(self, b1, b2, p1, torque=900, speed=-10): p1 = self.to_b2vec(p1) jointDef = box2d.b2RevoluteJointDef() jointDef.Initialize(b1, b2, p1) jointDef.maxMotorTorque = torque jointDef.motorSpeed = speed jointDef.enableMotor = True self.parent.world.CreateJoint(jointDef)
def addRevolute(self): sd=box2d.b2PolygonDef() sd.SetAsBox(1, 0.2) sd.density = 2.0 sd.filter.groupIndex=-2 bd1=box2d.b2BodyDef() ppos= self.body.GetWorldCenter() bd1.position = (ppos.x-0.2, ppos.y+0.8) body_arm1 = WorldConfig.world.CreateBody(bd1) body_arm1.CreateShape(sd) body_arm1.SetMassFromShapes() jointDef1 = box2d.b2RevoluteJointDef() jointDef1.Initialize(self.body, body_arm1, (body_arm1.position.x-1, body_arm1.position.y)) # jointDef1.collideConnected = True jointDef1.lowerAngle = 0.1 * box2d.b2_pi # -90 degrees jointDef1.upperAngle = 0.2 * box2d.b2_pi # 45 degrees jointDef1.enableLimit = True # jointDef1.maxMotorTorque= 100.0 jointDef1.motorSpeed = 0.0 jointDef1.enableMotor = True WorldConfig.world.CreateJoint(jointDef1).getAsType() self.bodies.append(body_arm1) bd1.position = (body_arm1.position.x+2, body_arm1.position.y) body_arm2 = WorldConfig.world.CreateBody(bd1) body_arm2.CreateShape(sd) body_arm2.SetMassFromShapes() jointDef1.Initialize(body_arm1, body_arm2, (body_arm2.position.x-1, body_arm2.position.y)) # jointDef1.collideConnected = True jointDef1.lowerAngle = -0.25 * box2d.b2_pi # -90 degrees jointDef1.upperAngle = -0.1 * box2d.b2_pi # 45 degrees jointDef1.enableLimit = True # jointDef1.maxMotorTorque= 1000.0 jointDef1.motorSpeed = 0.0 jointDef1.enableMotor = True WorldConfig.world.CreateJoint(jointDef1).getAsType() self.bodies.append(body_arm2) bd1.position = (body_arm2.position.x+2, body_arm2.position.y) body_arm3 = WorldConfig.world.CreateBody(bd1) body_arm3.CreateShape(sd) body_arm3.SetMassFromShapes() jointDef1.Initialize(body_arm2, body_arm3, (body_arm3.position.x-1, body_arm3.position.y)) # jointDef1.collideConnected = True jointDef1.lowerAngle = -0.25 * box2d.b2_pi # -90 degrees jointDef1.upperAngle = -0.1 * box2d.b2_pi # 45 degrees jointDef1.enableLimit = True # jointDef1.maxMotorTorque= 1000.0 jointDef1.motorSpeed = 0.0 jointDef1.enableMotor = True WorldConfig.world.CreateJoint(jointDef1).getAsType() self.bodies.append(body_arm3)
def addWheel(self, wheel, world, carBody, carPos, cfg): wheel.create(world, carBody, carPos, cfg) jointDef = b2.b2RevoluteJointDef() jointDef.bodyA = self.body jointDef.enableLimit = True jointDef.lowerAngle = 0 jointDef.upperAngle = 0 # Center of wheel jointDef.localAnchorB.SetZero() jointDef.bodyB = wheel.body jointDef.localAnchorA.Set(*cfg.anchor) return world.CreateJoint(jointDef)
def motor(self, body, pt, torque=900, speed=-10): # Revolute joint to the background with motor torque applied b1 = self.parent.world.groundBody pt = self.to_b2vec(pt) jointDef = box2d.b2RevoluteJointDef() jointDef.Initialize(b1, body, pt) jointDef.maxMotorTorque = torque jointDef.motorSpeed = speed jointDef.enableMotor = True self.parent.world.CreateJoint(jointDef)
def __init__(self, world, pos, length, thickness=0.05): self.world = world self.pos = np.array(pos) ground = self.world.CreateStaticBody(position=pos) p1 = [pos[0], pos[1] + length] doorBody = self.world.CreateDynamicBody(position=p1) doorBody.CreatePolygonFixture(box=(thickness / 2, length / 2, (0, -length / 2), 0), density=0.1, friction=0.3) angle = 2 * math.pi / 8 radius = 0.11 verts = [] xOffset = 0.15 yOffset = -0.15 for i in range(8): x = xOffset + radius * math.cos(angle * i) y = yOffset + radius * math.sin(angle * i) verts.append((x, y)) self.knob = doorBody.CreatePolygonFixture(vertices=verts, density=0.1, friction=0.3) wjd = Box2D.b2RevoluteJointDef( bodyA=ground, bodyB=doorBody, localAnchorA=(0, 0), localAnchorB=(0, -length), enableMotor=True, enableLimit=True, maxMotorTorque=.1, motorSpeed=.5, referenceAngle=0, lowerAngle=-math.pi / 2, upperAngle=0.0001, ) knobX = xOffset knobY = length - yOffset self.knobLength = math.sqrt((knobX**2) + (knobY**2)) self.knobAngle = math.atan2(knobX, knobY) self.joint = self.world.CreateJoint(wjd) self.body = doorBody
def mirror(self, orientation=(-1, 1)): for part in self.parts: part['obj'].mirror(orientation=orientation) part['trans'] = (part['trans'][0] * orientation[0], part['trans'][1] * orientation[1]) part['obj'].set_position( (self.get_position()[0] + part['trans'][0], self.get_position()[1] + part['trans'][1])) ''' q = Geo.quarter(self.get_position(), part['obj'].get_position()) if q == 2 or 4: ''' part['obj'].set_angle(part['obj'].get_angle() * -2) for joint in self.joints: if joint.type == 1: new_joint = B2.b2RevoluteJointDef() # new_joint.enableLimit = joint.limitEnabled #new_joint.lowerAngle = joint.lowerLimit - pi #new_joint.upperAngle = joint.upperLimit - pi elif joint.type == 2: new_joint = B2.b2PrismaticJointDef elif joint.type == 3: new_joint = B2.b2DistanceJointDef elif joint.type == 4: new_joint = B2.b2PulleyJointDef elif joint.type == 7: new_joint = B2.b2WheelJointDef elif joint.type == 8: new_joint = B2.b2WeldJointDef elif joint.type == 9: new_joint = B2.b2FrictionJointDef elif joint.type == 10: new_joint = B2.b2RopeJointDef new_joint.bodyA = joint.bodyA new_joint.bodyB = joint.bodyB anchor_a = Geo.to_angle(joint.bodyA.position, joint.anchorA, joint.bodyA.angle) new_joint.localAnchorA = (anchor_a[0] * orientation[0], anchor_a[1] * orientation[1]) anchor_b = Geo.to_angle(joint.bodyB.position, joint.anchorB, joint.bodyB.angle) new_joint.localAnchorB = (anchor_b[0] * orientation[0], anchor_b[1] * orientation[1]) new_joint.collideConnected = joint.collideConnected self.game.world.DestroyJoint(joint) self.game.world.CreateJoint(new_joint)
def __init__(self, wld, kind, name, pos, ang): w = wld.w Tank.ntanks += 1 self.n = Tank.ntanks self.alive = True self.health = conf.maxhealth self.kind = kind self.name = name self._pingtype = 'w' self._pingangle = 0 self._pingdist = 0 self._pinged = -5 self._cannonheat = 0 self._cannonreload = 0 self._kills = 0 self._damage_caused = 0 bodyDef = box2d.b2BodyDef() bodyDef.position = pos bodyDef.angle = ang bodyDef.linearDamping = conf.tank_linearDamping bodyDef.angularDamping = conf.tank_angularDamping bodyDef.userData = {} body = w.CreateBody(bodyDef) shapeDef = box2d.b2PolygonDef() shapeDef.SetAsBox(1, 1) shapeDef.density = conf.tank_density shapeDef.friction = conf.tank_friction shapeDef.restitution = conf.tank_restitution shapeDef.filter.groupIndex = -self.n body.CreateShape(shapeDef) body.SetMassFromShapes() body.userData['actor'] = self body.userData['kind'] = 'tank' self.body = body turretDef = box2d.b2BodyDef() turretDef.position = pos turretDef.angle = ang turretDef.linearDamping = 0 turretDef.angularDamping = 0 turret = w.CreateBody(bodyDef) shapeDef = box2d.b2PolygonDef() shapeDef.SetAsBox(.1, .1) shapeDef.density = 1 shapeDef.friction = 0 shapeDef.restitution = 0 shapeDef.filter.groupIndex = -self.n turret.CreateShape(shapeDef) turret.SetMassFromShapes() self.turret = turret jointDef = box2d.b2RevoluteJointDef() jointDef.Initialize(body, turret, pos) jointDef.maxMotorTorque = conf.turret_maxMotorTorque jointDef.motorSpeed = 0.0 jointDef.enableMotor = True self.turretjoint = w.CreateJoint(jointDef) self._turretangletarget = 0 v = wld.v.addtank(pos, ang) self.v = v i = wld.v.addtankinfo(self.n, name) self.i = i
def __init__(self, world, angles, lengths, basePos=(0, 0), thickness=0.05, dense=0.1, motorRange=math.pi, maxTorques=[2, 1.6, .8, .4], rot_damping=10, lin_damping=10, target_pose=[0, 2, math.pi / 2]): rotMin = (math.pi / 2) - (motorRange / 2) rotMax = (math.pi / 2) + (motorRange / 2) self.motorRange = motorRange self.prev_error = np.array([0, 0, 0, 0, 0]) self.target_pose = np.array(target_pose) self.world = world self.angles = np.array(angles) self.basePos = np.array(basePos) self.pos_mode = False self.set_target_angles(self.angles) self.lengths = np.array(lengths) self.thickness = thickness self.absAngles = self.get_abs_angles(angles) ground = self.world.CreateStaticBody(position=basePos) dif0 = self.getXY(self.angles[0], self.lengths[0]) p1 = dif0 + basePos shoulderBody = self.world.CreateDynamicBody(position=p1, angle=0, angularDamping=rot_damping, linearDamping=lin_damping) shoulderBody.CreatePolygonFixture(box=(self.lengths[0] / 2, self.thickness, (-dif0[0] / 2, -dif0[1] / 2), self.angles[0]), density=dense, friction=0.3) sjd = Box2D.b2RevoluteJointDef( bodyA=ground, bodyB=shoulderBody, localAnchorA=(0, 0), localAnchorB=(-dif0[0], -dif0[1]), enableMotor=True, enableLimit=True, maxMotorTorque=maxTorques[0], motorSpeed=0, referenceAngle=-self.angles[0], lowerAngle=rotMin, upperAngle=rotMax, ) self.shoulderMotor = self.world.CreateJoint(sjd) dif = [ self.lengths[1] * math.cos(self.absAngles[1]), self.lengths[1] * math.sin(self.absAngles[1]) ] p2 = [p1[0] + dif[0], p1[1] + dif[1]] elbowBody = self.world.CreateDynamicBody(position=p2, angle=0, angularDamping=rot_damping, linearDamping=lin_damping) elbowBody.CreatePolygonFixture(box=(self.lengths[1] / 2, self.thickness, (-dif[0] / 2, -dif[1] / 2), self.absAngles[1]), density=dense, friction=0.3) ejd = Box2D.b2RevoluteJointDef( bodyA=shoulderBody, bodyB=elbowBody, localAnchorA=(0, 0), localAnchorB=(-dif[0], -dif[1]), enableMotor=True, enableLimit=True, maxMotorTorque=maxTorques[1], motorSpeed=0, referenceAngle=-self.angles[1], lowerAngle=rotMin, upperAngle=rotMax, ) self.elbowMotor = self.world.CreateJoint(ejd) dif2 = [ self.lengths[2] * math.cos(self.absAngles[2]), self.lengths[2] * math.sin(self.absAngles[2]) ] p3 = [p2[0] + dif2[0], p2[1] + dif2[1]] wristBody = self.world.CreateDynamicBody(position=p3, angle=0, angularDamping=rot_damping, linearDamping=lin_damping) wristBody.CreatePolygonFixture(box=(self.lengths[2] / 2, self.thickness, (-dif2[0] / 2, -dif2[1] / 2), self.absAngles[2]), density=dense, friction=0.3) wristBody.CreatePolygonFixture(box=(self.lengths[2] / 2, self.thickness, (0, 0), self.absAngles[2] + math.pi / 2), density=dense, friction=0.3) wjd = Box2D.b2RevoluteJointDef( bodyA=elbowBody, bodyB=wristBody, localAnchorA=(0, 0), localAnchorB=(-dif2[0], -dif2[1]), enableMotor=True, enableLimit=True, maxMotorTorque=maxTorques[2], motorSpeed=0, referenceAngle=-self.angles[2], lowerAngle=rotMin, upperAngle=rotMax, ) self.wristMotor = self.world.CreateJoint(wjd) dif3 = [ self.lengths[3] * math.cos(self.absAngles[3]), self.lengths[3] * math.sin(self.absAngles[3]) ] dif4 = [ self.lengths[2] / 2 * math.cos(self.absAngles[3] + math.pi / 2), self.lengths[2] / 2 * math.sin(self.absAngles[3] + math.pi / 2) ] p4 = [p3[0] + dif3[0] + dif4[0], p3[1] + dif3[1] + dif4[1]] clawBody = self.world.CreateDynamicBody(position=p4, angle=0, angularDamping=rot_damping, linearDamping=lin_damping) clawBody.CreatePolygonFixture(box=(self.lengths[3] / 2, self.thickness, (-dif3[0] / 2, -dif3[1] / 2), self.absAngles[3]), density=dense, friction=1.3) cjd = Box2D.b2RevoluteJointDef( bodyA=wristBody, bodyB=clawBody, localAnchorA=(dif4[0], dif4[1]), localAnchorB=(-dif3[0], -dif3[1]), enableMotor=True, enableLimit=True, maxMotorTorque=maxTorques[3], motorSpeed=0, referenceAngle=-self.angles[3], lowerAngle=math.pi / 4, upperAngle=3 * math.pi / 4, ) self.clawMotor = self.world.CreateJoint(cjd) p5 = [p3[0] + dif3[0] - dif4[0], p3[1] + dif3[1] - dif4[1]] clawBody2 = self.world.CreateDynamicBody(position=p5, angle=0, angularDamping=rot_damping, linearDamping=lin_damping) clawBody2.CreatePolygonFixture(box=(self.lengths[3] / 2, self.thickness, (-dif3[0] / 2, -dif3[1] / 2), self.absAngles[4]), density=dense, friction=1.3) cjd2 = Box2D.b2RevoluteJointDef( bodyA=wristBody, bodyB=clawBody2, localAnchorA=(-dif4[0], -dif4[1]), localAnchorB=(-dif3[0], -dif3[1]), enableMotor=True, enableLimit=True, maxMotorTorque=maxTorques[3], motorSpeed=0, referenceAngle=-self.angles[4], lowerAngle=math.pi / 4, upperAngle=3 * math.pi / 4, ) self.clawMotor2 = self.world.CreateJoint(cjd2) self.update_arm()
def __init__(self, world, x=10, y=10, l=3, h=1, thruster_size=0.3, thruster_limit=30, ray_length=10, thruster_power_limit=50): """ x,y defines the starting position of the AUV. 0,0 is bottom left. l,h length and height of the AUV. thruster_size is the size of the triangular thruster attached to the auv thruster_limit limits the rotation of the thruster to +/- this many degrees. """ # CCW, centered on the rectangle center auv_vertices = [ # top right (l / 2, h / 2), # top left (-l / 2, h / 2), # bottom left (-l / 2, -h / 2), # bottom right (l / 2, -h / 2) ] # where the thruster will connect to the auv # takes the center of the auv as origin # offset a little auv_local_anchor_thruster = (-l / 2 - 0.2, 0) # dynamic point auv = world.world.CreateDynamicBody( position=(x, y), angle=0., linearDamping=0.2, angularDamping=0.6, fixtures=b2.b2FixtureDef( shape=b2.b2PolygonShape(vertices=auv_vertices), density=10, # slidey friction=0.9, # not very bouncy restitution=0.01)) # where the thruster is attached to the auv thruster_local_anchor_auv = (0, 0) # a simple triangle auv_thruster_vertices = [ (0, 0), (-thruster_size, -thruster_size), (-thruster_size, thruster_size) ] auv_thruster = world.world.CreateDynamicBody( position=(x - l / 2, y), angle=0, fixtures=b2.b2FixtureDef( shape=b2.b2PolygonShape( vertices=auv_thruster_vertices), density=1, friction=0.1, restitution=0)) thruster_joint = b2.b2RevoluteJointDef( bodyA=auv, bodyB=auv_thruster, localAnchorA=auv_local_anchor_thruster, localAnchorB=thruster_local_anchor_auv, enableMotor=True, enableLimit=True, maxMotorTorque=50) # dont let the thruster collide with the auv body thruster_joint.collideConnected = False # rotate between these angles hopefully joint_angle_limit = thruster_limit / C.RADTODEG thruster_joint.lowerAngle = -joint_angle_limit thruster_joint.upperAngle = joint_angle_limit # create the joint auv_thruster.joint = world.world.CreateJoint(thruster_joint) # for future reference self._thruster_joint = auv_thruster.joint self._thruster = auv_thruster self._auv = auv self._thruster_power_limit = thruster_power_limit self._thruster_limit = thruster_limit self._thruster_angle_limits = ( thruster_joint.lowerAngle, thruster_joint.upperAngle) self._world = world # create rays for the proximity sensor of the AUV # these rays need to be rotated and moved with the auv body self._prox_rays = [] num_rays = 8 angle_offset = 45 / 2 for i in range(num_rays): angle = i * 360 / num_rays angle += angle_offset angle = angle % 360 p1 = b2.b2Vec2([0, 0]) p2 = b2.b2Vec2([ray_length * np.cos(angle / C.RADTODEG), ray_length * np.sin(angle / C.RADTODEG)]) self._prox_rays.append((p1, p2)) # helper object for raycasting. Keeps a reference to auv transform in itself # together with the rays and world self._raycaster = Raycaster(self._world.world, self._prox_rays, self._auv.transform, ray_length) # for outside things (like visualizer) to get some points from the raycaster self.last_casted_points = None # current target angle of the thrust # in degrees self._target_thrust_angle = 0
def json_load(self, path, serialized=False): import json self.world.GetGroundBody().userData = {"saveid": 0} f = open(path, 'r') worldmodel = json.loads(f.read()) f.close() # clean world for joint in self.world.GetJointList(): self.world.DestroyJoint(joint) for body in self.world.GetBodyList(): if body != self.world.GetGroundBody(): self.world.DestroyBody(body) # load bodies for body in worldmodel['bodylist']: bodyDef = box2d.b2BodyDef() bodyDef.position = body['position'] bodyDef.userData = body['userData'] bodyDef.angle = body['angle'] newBody = self.world.CreateBody(bodyDef) #_logger.debug(newBody) newBody.angularVelocity = body['angularVelocity'] newBody.linearVelocity = body['linearVelocity'] if 'shapes' in body: for shape in body['shapes']: if shape['type'] == 'polygon': polyDef = box2d.b2PolygonDef() polyDef.setVertices(shape['vertices']) polyDef.density = shape['density'] polyDef.restitution = shape['restitution'] polyDef.friction = shape['friction'] newBody.CreateShape(polyDef) if shape['type'] == 'circle': circleDef = box2d.b2CircleDef() circleDef.radius = shape['radius'] circleDef.density = shape['density'] circleDef.restitution = shape['restitution'] circleDef.friction = shape['friction'] circleDef.localPosition = shape['localPosition'] newBody.CreateShape(circleDef) newBody.SetMassFromShapes() for joint in worldmodel['jointlist']: if joint['type'] == 'distance': jointDef = box2d.b2DistanceJointDef() body1 = self.getBodyWithSaveId(joint['body1']) anch1 = joint['anchor1'] body2 = self.getBodyWithSaveId(joint['body2']) anch2 = joint['anchor2'] jointDef.collideConnected = joint['collideConnected'] jointDef.Initialize(body1, body2, anch1, anch2) jointDef.SetUserData(joint['userData']) self.world.CreateJoint(jointDef) if joint['type'] == 'revolute': jointDef = box2d.b2RevoluteJointDef() body1 = self.getBodyWithSaveId(joint['body1']) body2 = self.getBodyWithSaveId(joint['body2']) anchor = joint['anchor'] jointDef.Initialize(body1, body2, anchor) jointDef.SetUserData(joint['userData']) jointDef.enableMotor = joint['enableMotor'] jointDef.motorSpeed = joint['motorSpeed'] jointDef.maxMotorTorque = joint['maxMotorTorque'] self.world.CreateJoint(jointDef) self.additional_vars = {} addvars = {} for (k, v) in worldmodel['additional_vars'].items(): addvars[k] = v if serialized and 'trackinfo' in addvars: trackinfo = addvars['trackinfo'] for key, info in trackinfo.iteritems(): if not info[3]: addvars['trackinfo'][key][0] = \ self.getBodyWithSaveId(info[0]) addvars['trackinfo'][key][1] = \ self.getBodyWithSaveId(info[1]) else: addvars['trackinfo'][key][0] = None addvars['trackinfo'][key][1] = None self.additional_vars = addvars for body in self.world.GetBodyList(): del body.userData['saveid'] # remove temporary data
def _spawn(self, module): """Helper method to spawn 2D module inside Box2D""" bodies = [] joints = [] position_correction = np.array([0.0, GROUND_HEIGHT + 0.1]) axis, angle = module.orientation.as_axis() # angle = module.orientation.as_euler()[0] angle = ((axis[1] * angle) % (2. * np.pi)) * -1. if isinstance(module, Rect): # Spawn rectangle module body = self.world.CreateDynamicBody( position=module.position[::2] + position_correction, angle=angle, fixtures=CUBE_FD) body.color1 = (0.23, 0.32, 0.55) body.color2 = (0, 0, 0) body.draw_connection = True bodies.append(body) elif isinstance(module, Servo): # Spawn rectangle module body = self.world.CreateDynamicBody( position=module.position[::2] + position_correction, angle=angle, fixtures=CUBE_FD) body.color1 = (0.13, 0.57, 0.55) body.color2 = (0, 0, 0) bodies.append(body) # Spawn plate plate_corr = module.orientation.rotate(np.array([-29., 0., 0])) plate = self.world.CreateDynamicBody( position=module.position[::2] + position_correction + plate_corr[::2], angle=angle, fixtures=PLATE_FD) plate.color1 = (0.99, 0.90, 0.15) plate.color2 = (0, 0, 0) bodies.append(plate) # Create joint between bodies joint = Box2D.b2RevoluteJointDef( bodyA=body, bodyB=plate, localAnchorA=(0, 0), localAnchorB=-plate_corr[::2] if math.isclose(angle, 0.0) else (29., 0.), enableMotor=True, enableLimit=True, maxMotorTorque=1000.8, lowerAngle=-1.57, upperAngle=1.57, collideConnected=False) joint = self.world.CreateJoint(joint) joints.append(joint) else: raise NotImplementedError("Unknown module type: '{!s}'" .format(type(module))) # Do collision detection self.world.contactManager.FindNewContacts() self.world.contactManager.Collide() if any(map(lambda c: c.touching, self.world.contacts)): # Collision detected, remove bodies for joint in joints: self.world.DestroyJoint(joint) for body in bodies: self.world.DestroyBody(body) return False # No collision detected add bodies self._bodies.extend(bodies) self._joints.extend(joints) self._module_map[module] = bodies return True
def create_jointDef(jsw_joint, b2_world): """create a b2JointDef from the json parameters of the joint :param jsw_joint: dictionary defining the parameters of the joint :type jsw_joint: dict(sting: variant) :param b2_world: an handler to a b2World object :type b2_world: b2World reference :return: the joint definition object :rtype: b2JointDef """ joint_type = jsw_joint["type"] # naming # --------------------------------------------------- if joint_type == "revolute": # Done jointDef = b2.b2RevoluteJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "enableLimit", jointDef) setAttr(jsw_joint, "enableMotor", jointDef) setAttr(jsw_joint, "jointSpeed", jointDef, "motorSpeed") setAttr(jsw_joint, "lowerLimit", jointDef, "lowerAngle") setAttr(jsw_joint, "maxMotorTorque", jointDef) setAttr(jsw_joint, "motorSpeed", jointDef) setAttr(jsw_joint, "refAngle", jointDef, "referenceAngle") setAttr(jsw_joint, "upperLimit", jointDef, "upperAngle") # --------------------------------------------------- elif joint_type == "distance": # Done jointDef = b2.b2DistanceJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "dampingRatio", jointDef) setAttr(jsw_joint, "frequency", jointDef, "frequencyHz") setAttr(jsw_joint, "length", jointDef) # --------------------------------------------------- elif joint_type == "prismatic": # Done jointDef = b2.b2PrismaticJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "enableLimit", jointDef) setAttr(jsw_joint, "enableMotor", jointDef) setB2Vec2Attr(jsw_joint, "localAxisA", jointDef, "axis") setAttr(jsw_joint, "lowerLimit", jointDef, "lowerTranslation") setAttr(jsw_joint, "maxMotorForce", jointDef) setAttr(jsw_joint, "motorSpeed", jointDef) setAttr(jsw_joint, "refAngle", jointDef, "referenceAngle") setAttr(jsw_joint, "upperLimit", jointDef, "upperTranslation") # --------------------------------------------------- elif joint_type == "wheel": # Done jointDef = b2.b2WheelJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "enableMotor", jointDef) setB2Vec2Attr(jsw_joint, "localAxisA", jointDef) setAttr(jsw_joint, "maxMotorTorque", jointDef) setAttr(jsw_joint, "motorSpeed", jointDef) setAttr(jsw_joint, "springDampingRatio", jointDef, "dampingRatio") setAttr(jsw_joint, "springFrequency", jointDef, "frequencyHz") # --------------------------------------------------- elif joint_type == "rope": # Done jointDef = b2.b2RopeJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "maxLength", jointDef) # --------------------------------------------------- elif joint_type == "motor": # Done jointDef = b2.b2MotorJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "maxForce", jointDef) setAttr(jsw_joint, "maxTorque", jointDef) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "linearOffset") setAttr(jsw_joint, "correctionFactor", jointDef) # --------------------------------------------------- elif joint_type == "weld": # Done jointDef = b2.b2WeldJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "refAngle", jointDef, "referenceAngle") setAttr(jsw_joint, "dampingRatio", jointDef) setAttr(jsw_joint, "frequency", jointDef, "frequencyHz") # --------------------------------------------------- elif joint_type == "friction": # Done jointDef = b2.b2FrictionJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "maxForce", jointDef) setAttr(jsw_joint, "maxTorque", jointDef) else: print("unsupported joint type") return jointDef
def create_jointDef(jsw_joint, b2_world): joint_type = jsw_joint["type"] # naming #--------------------------------------------------- if joint_type == "revolute": # Done jointDef = b2.b2RevoluteJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "enableLimit", jointDef) setAttr(jsw_joint, "enableMotor", jointDef) setAttr(jsw_joint, "jointSpeed", jointDef, "motorSpeed") setAttr(jsw_joint, "lowerLimit", jointDef, "lowerAngle") setAttr(jsw_joint, "maxMotorTorque", jointDef) setAttr(jsw_joint, "motorSpeed", jointDef) setAttr(jsw_joint, "refAngle", jointDef, "referenceAngle") setAttr(jsw_joint, "upperLimit", jointDef, "upperAngle") #--------------------------------------------------- elif joint_type == "distance": # Done jointDef = b2.b2DistanceJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "dampingRatio", jointDef) setAttr(jsw_joint, "frequency", jointDef, "frequencyHz") setAttr(jsw_joint, "length", jointDef) #--------------------------------------------------- elif joint_type == "prismatic": # Done jointDef = b2.b2PrismaticJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "enableLimit", jointDef) setAttr(jsw_joint, "enableMotor", jointDef) setB2Vec2Attr(jsw_joint, "localAxisA", jointDef, "axis") setAttr(jsw_joint, "lowerLimit", jointDef, "lowerTranslation") setAttr(jsw_joint, "maxMotorForce", jointDef) setAttr(jsw_joint, "motorSpeed", jointDef) setAttr(jsw_joint, "refAngle", jointDef, "referenceAngle") setAttr(jsw_joint, "upperLimit", jointDef, "upperTranslation") #--------------------------------------------------- elif joint_type == "wheel": # Done jointDef = b2.b2WheelJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "enableMotor", jointDef) setB2Vec2Attr(jsw_joint, "localAxisA", jointDef) setAttr(jsw_joint, "maxMotorTorque", jointDef) setAttr(jsw_joint, "motorSpeed", jointDef) setAttr(jsw_joint, "springDampingRatio", jointDef, "dampingRatio") setAttr(jsw_joint, "springFrequency", jointDef, "frequencyHz") #--------------------------------------------------- elif joint_type == "rope": # Done jointDef = b2.b2RopeJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "maxLength", jointDef) #--------------------------------------------------- elif joint_type == "motor": # Done jointDef = b2.b2MotorJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "maxForce", jointDef) setAttr(jsw_joint, "maxTorque", jointDef) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "linearOffset") setAttr(jsw_joint, "correctionFactor", jointDef) #--------------------------------------------------- elif joint_type == "weld": # Done jointDef = b2.b2WeldJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "refAngle", jointDef, "referenceAngle") setAttr(jsw_joint, "dampingRatio", jointDef) setAttr(jsw_joint, "frequency", jointDef, "frequencyHz") #--------------------------------------------------- elif joint_type == "friction": # Done jointDef = b2.b2FrictionJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "maxForce", jointDef) setAttr(jsw_joint, "maxTorque", jointDef) else: print("unsupported joint type") return jointDef
def __init__(self, wld, kind, name, pos, ang): w = wld.w Robot.nrobots += 1 self.n = Robot.nrobots self.alive = True self.health = conf.maxhealth self.kind = kind self.name = name self._pingtype = 'w' self._pingangle = 0 self._pingdist = 0 self._pinged = -5 # Tick most recently pinged by another robot's radar self._cannonheat = 0 self._cannonreload = 0 self._kills = 0 # number of robots killed while this one is still alive self._damage_caused = 0 bodyDef = box2d.b2BodyDef() bodyDef.position = pos bodyDef.angle = ang bodyDef.linearDamping = conf.robot_linearDamping bodyDef.angularDamping = conf.robot_angularDamping bodyDef.userData = {} body = w.CreateBody(bodyDef) shapeDef = box2d.b2PolygonDef() shapeDef.SetAsBox(1, 1) shapeDef.density = conf.robot_density shapeDef.friction = conf.robot_friction shapeDef.restitution = conf.robot_restitution shapeDef.filter.groupIndex = -self.n body.CreateShape(shapeDef) body.SetMassFromShapes() body.userData['actor'] = self body.userData['kind'] = 'robot' self.body = body turretDef = box2d.b2BodyDef() turretDef.position = pos turretDef.angle = ang turretDef.linearDamping = 0 turretDef.angularDamping = 0 turret = w.CreateBody(bodyDef) shapeDef = box2d.b2PolygonDef() shapeDef.SetAsBox(.1, .1) shapeDef.density = 1 shapeDef.friction = 0 shapeDef.restitution = 0 shapeDef.filter.groupIndex = -self.n turret.CreateShape(shapeDef) turret.SetMassFromShapes() self.turret = turret jointDef = box2d.b2RevoluteJointDef() jointDef.Initialize(body, turret, pos) jointDef.maxMotorTorque = conf.turret_maxMotorTorque jointDef.motorSpeed = 0.0 jointDef.enableMotor = True self.turretjoint = w.CreateJoint(jointDef).getAsType() self._turretangletarget = 0 v = wld.v.addrobot(pos, ang) self.v = v i = wld.v.addrobotinfo(self.n, name) self.i = i
def create_jointDef(jsw_joint, b2_world): joint_type = jsw_joint["type"] # naming #--------------------------------------------------- if joint_type == "revolute": # Done jointDef = b2.b2RevoluteJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "enableLimit", jointDef) setAttr(jsw_joint, "enableMotor", jointDef) setAttr(jsw_joint, "jointSpeed", jointDef, "motorSpeed") setAttr(jsw_joint, "lowerLimit", jointDef, "lowerAngle") setAttr(jsw_joint, "maxMotorTorque", jointDef) setAttr(jsw_joint, "motorSpeed", jointDef) setAttr(jsw_joint, "refAngle", jointDef, "referenceAngle") setAttr(jsw_joint, "upperLimit", jointDef, "upperAngle") #--------------------------------------------------- elif joint_type == "distance": # Done jointDef = b2.b2DistanceJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "dampingRatio", jointDef) setAttr(jsw_joint, "frequency", jointDef, "frequencyHz") setAttr(jsw_joint, "length", jointDef) #--------------------------------------------------- elif joint_type == "prismatic": # Done jointDef = b2.b2PrismaticJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "enableLimit", jointDef) setAttr(jsw_joint, "enableMotor", jointDef) setB2Vec2Attr(jsw_joint, "localAxisA", jointDef, "axis") setAttr(jsw_joint, "lowerLimit", jointDef, "lowerTranslation") setAttr(jsw_joint, "maxMotorForce", jointDef) setAttr(jsw_joint, "motorSpeed", jointDef) setAttr(jsw_joint, "refAngle", jointDef, "referenceAngle") setAttr(jsw_joint, "upperLimit", jointDef, "upperTranslation") #--------------------------------------------------- elif joint_type == "wheel": # Done jointDef = b2.b2WheelJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "enableMotor", jointDef) setB2Vec2Attr(jsw_joint, "localAxisA", jointDef) setAttr(jsw_joint, "maxMotorTorque", jointDef) setAttr(jsw_joint, "motorSpeed", jointDef) setAttr(jsw_joint, "springDampingRatio", jointDef, "dampingRatio") setAttr(jsw_joint, "springFrequency", jointDef, "frequencyHz") #--------------------------------------------------- elif joint_type == "rope": # Done jointDef = b2.b2RopeJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "maxLength", jointDef) #--------------------------------------------------- elif joint_type == "motor": # Done jointDef = b2.b2MotorJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "maxForce", jointDef) setAttr(jsw_joint, "maxTorque", jointDef) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "linearOffset") setAttr(jsw_joint, "correctionFactor", jointDef) #--------------------------------------------------- elif joint_type == "weld": # Done jointDef = b2.b2WeldJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "refAngle", jointDef, "referenceAngle") setAttr(jsw_joint, "dampingRatio", jointDef) setAttr(jsw_joint, "frequency", jointDef, "frequencyHz") #--------------------------------------------------- elif joint_type == "friction": # Done jointDef = b2.b2FrictionJointDef() jointDef.bodyA = get_body(b2_world, jsw_joint["bodyA"]) jointDef.bodyB = get_body(b2_world, jsw_joint["bodyB"]) setB2Vec2Attr(jsw_joint, "anchorA", jointDef, "localAnchorA") setB2Vec2Attr(jsw_joint, "anchorB", jointDef, "localAnchorB") setAttr(jsw_joint, "collideConnected", jointDef) setAttr(jsw_joint, "maxForce", jointDef) setAttr(jsw_joint, "maxTorque", jointDef) else: print ("unsupported joint type") return jointDef
def json_load(self, path, additional_vars = {}): import cjson self.world.GetGroundBody().userData = {"saveid" : 0} f = open(path, 'r') worldmodel = cjson.decode(f.read()) f.close() #clean world for joint in self.world.GetJointList(): self.world.DestroyJoint(joint) for body in self.world.GetBodyList(): if body != self.world.GetGroundBody(): self.world.DestroyBody(body) #load bodys for body in worldmodel['bodylist']: bodyDef = box2d.b2BodyDef() bodyDef.position = body['position'] bodyDef.userData = body['userData'] bodyDef.angle = body['angle'] newBody = self.world.CreateBody(bodyDef) #_logger.debug(newBody) newBody.angularVelocity = body['angularVelocity'] newBody.linearVelocity = body['linearVelocity'] if body.has_key('shapes'): for shape in body['shapes']: if shape['type'] == 'polygon': polyDef = box2d.b2PolygonDef() polyDef.setVertices(shape['vertices']) polyDef.density = shape['density'] polyDef.restitution = shape['restitution'] polyDef.friction = shape['friction'] newBody.CreateShape(polyDef) if shape['type'] == 'circle': circleDef = box2d.b2CircleDef() circleDef.radius = shape['radius'] circleDef.density = shape['density'] circleDef.restitution = shape['restitution'] circleDef.friction = shape['friction'] circleDef.localPosition = shape['localPosition'] newBody.CreateShape(circleDef) newBody.SetMassFromShapes() for joint in worldmodel['jointlist']: if joint['type'] == 'distance': jointDef = box2d.b2DistanceJointDef() body1 = self.getBodyWithSaveId(joint['body1']) anch1 = joint['anchor1'] body2 = self.getBodyWithSaveId(joint['body2']) anch2 = joint['anchor2'] jointDef.collideConnected = joint['collideConnected'] jointDef.Initialize(body1,body2,anch1,anch2) jointDef.SetUserData(joint['userData']) self.world.CreateJoint(jointDef) if joint['type'] == 'revolute': jointDef = box2d.b2RevoluteJointDef() body1 = self.getBodyWithSaveId(joint['body1']) body2 = self.getBodyWithSaveId(joint['body2']) anchor = joint['anchor'] jointDef.Initialize(body1,body2,anchor) jointDef.SetUserData(joint['userData']) jointDef.enableMotor = joint['enableMotor'] jointDef.motorSpeed = joint['motorSpeed'] jointDef.maxMotorTorque = joint['maxMotorTorque'] self.world.CreateJoint(jointDef) for (k,v) in worldmodel['additional_vars'].items(): additional_vars[k] = v for body in self.world.GetBodyList(): del body.userData['saveid'] #remove temporary data
def __init__(self, wld, kind, name, pos, ang): w = wld.w Robot.nrobots += 1 self.n = Robot.nrobots self.alive = True self.health = conf.maxhealth self.kind = kind self.name = name self._pingtype = 'w' self._pingangle = 0 self._pingdist = 0 self._pinged = -5 # Tick most recently pinged by another robot's radar self._cannonheat = 0 self._cannonreload = 0 self._kills = 0 # number of robots killed while this one is still alive self._damage_caused = 0 bodyDef = box2d.b2BodyDef() bodyDef.position = pos bodyDef.angle = ang bodyDef.linearDamping = conf.robot_linearDamping bodyDef.angularDamping = conf.robot_angularDamping bodyDef.userData = {} body = w.CreateBody(bodyDef) shapeDef = box2d.b2PolygonDef() shapeDef.SetAsBox(1, 1) shapeDef.density = conf.robot_density shapeDef.friction = conf.robot_friction shapeDef.restitution = conf.robot_restitution shapeDef.filter.groupIndex = -self.n body.CreateShape(shapeDef) body.SetMassFromShapes() body.userData['actor'] = self body.userData['kind'] = 'robot' self.body = body turretDef = box2d.b2BodyDef() turretDef.position = pos turretDef.angle = ang turretDef.linearDamping = 0 turretDef.angularDamping = 0 turret = w.CreateBody(bodyDef) shapeDef = box2d.b2PolygonDef() shapeDef.SetAsBox(.1, .1) shapeDef.density = 1 shapeDef.friction = 0 shapeDef.restitution = 0 shapeDef.filter.groupIndex = -self.n turret.CreateShape(shapeDef) turret.SetMassFromShapes() self.turret = turret jointDef = box2d.b2RevoluteJointDef() jointDef.Initialize(body, turret, pos) jointDef.maxMotorTorque = conf.turret_maxMotorTorque jointDef.motorSpeed = 0.0 jointDef.enableMotor = True self.turretjoint = w.CreateJoint(jointDef).getAsType() self._turretangletarget = 0 v = wld.v.addrobot(pos, ang) self.v = v i = wld.v.addrobotinfo(self.n, name) self.i = i
def __init__(self): global count # BodyDefs self.LFootDef=box2d.b2BodyDef() self.RFootDef=box2d.b2BodyDef() self.LCalfDef=box2d.b2BodyDef() self.RCalfDef=box2d.b2BodyDef() self.LThighDef=box2d.b2BodyDef() self.RThighDef=box2d.b2BodyDef() self.PelvisDef=box2d.b2BodyDef() self.StomachDef=box2d.b2BodyDef() self.ChestDef=box2d.b2BodyDef() self.NeckDef=box2d.b2BodyDef() self.HeadDef=box2d.b2BodyDef() self.LUpperArmDef=box2d.b2BodyDef() self.RUpperArmDef=box2d.b2BodyDef() self.LForearmDef=box2d.b2BodyDef() self.RForearmDef=box2d.b2BodyDef() self.LHandDef=box2d.b2BodyDef() self.RHandDef=box2d.b2BodyDef() # Polygons self.LFootPoly=box2d.b2PolygonDef() self.RFootPoly=box2d.b2PolygonDef() self.LCalfPoly=box2d.b2PolygonDef() self.RCalfPoly=box2d.b2PolygonDef() self.LThighPoly=box2d.b2PolygonDef() self.RThighPoly=box2d.b2PolygonDef() self.LFootPoly.friction = 0.7 self.RFootPoly.friction = 0.7 self.PelvisPoly=box2d.b2PolygonDef() self.StomachPoly=box2d.b2PolygonDef() self.ChestPoly=box2d.b2PolygonDef() self.NeckPoly=box2d.b2PolygonDef() self.LUpperArmPoly=box2d.b2PolygonDef() self.RUpperArmPoly=box2d.b2PolygonDef() self.LForearmPoly=box2d.b2PolygonDef() self.RForearmPoly=box2d.b2PolygonDef() self.LHandPoly=box2d.b2PolygonDef() self.RHandPoly=box2d.b2PolygonDef() # Circles self.HeadCirc=box2d.b2CircleDef() # Revolute Joints self.LAnkleDef=box2d.b2RevoluteJointDef() self.RAnkleDef=box2d.b2RevoluteJointDef() self.LKneeDef=box2d.b2RevoluteJointDef() self.RKneeDef=box2d.b2RevoluteJointDef() self.LHipDef=box2d.b2RevoluteJointDef() self.RHipDef=box2d.b2RevoluteJointDef() self.LowerAbsDef=box2d.b2RevoluteJointDef() self.UpperAbsDef=box2d.b2RevoluteJointDef() self.LowerNeckDef=box2d.b2RevoluteJointDef() self.UpperNeckDef=box2d.b2RevoluteJointDef() self.LShoulderDef=box2d.b2RevoluteJointDef() self.RShoulderDef=box2d.b2RevoluteJointDef() self.LElbowDef=box2d.b2RevoluteJointDef() self.RElbowDef=box2d.b2RevoluteJointDef() self.LWristDef=box2d.b2RevoluteJointDef() self.RWristDef=box2d.b2RevoluteJointDef() self.iter_polys = ( self.LFootPoly, self.RFootPoly, self.LCalfPoly, self.RCalfPoly, self.LThighPoly, self.RThighPoly, self.PelvisPoly, self.StomachPoly, self.ChestPoly, self.NeckPoly, self.LUpperArmPoly, self.RUpperArmPoly, self.LForearmPoly, self.RForearmPoly, self.LHandPoly, self.RHandPoly , self.HeadCirc ) self.iter_defs = ( self.LFootDef, self.RFootDef, self.LCalfDef, self.RCalfDef, self.LThighDef, self.RThighDef, self.PelvisDef, self.StomachDef, self.ChestDef, self.NeckDef, self.HeadDef, self.LUpperArmDef, self.RUpperArmDef, self.LForearmDef, self.RForearmDef, self.LHandDef, self.RHandDef ) self.iter_joints=( self.LAnkleDef, self.RAnkleDef, self.LKneeDef, self.RKneeDef, self.LHipDef, self.RHipDef, self.LowerAbsDef, self.UpperAbsDef, self.LowerNeckDef, self.UpperNeckDef, self.LShoulderDef, self.RShoulderDef, self.LElbowDef, self.RElbowDef, self.LWristDef, self.RWristDef ) self.SetMotorTorque(2.0) self.SetMotorSpeed(0.0) self.SetDensity(1.0) self.SetRestitution(0.0) self.SetLinearDamping(0.0) self.SetAngularDamping(0.005) count -= 1 self.SetGroupIndex(count) self.EnableMotor() self.EnableLimit() self.DefaultVertices() self.DefaultPositions() self.DefaultJoints() self.LFootPoly.friction = self.RFootPoly.friction = 1.85
] auv_thruster = world.CreateDynamicBody( position=(auv_x - auv_l / 2, auv_y), angle=0, fixtures=b2.b2FixtureDef( shape=b2.b2PolygonShape( vertices=auv_thruster_vertices), density=1, friction=0.1, restitution=0)) thruster_joint = b2.b2RevoluteJointDef( bodyA=auv, bodyB=auv_thruster, localAnchorA=auv_local_anchor_thruster, localAnchorB=thruster_local_anchor_auv, enableMotor=True, enableLimit=True, maxMotorTorque=10) # dont let the thruster collide with the auv body thruster_joint.collideConnected = False # rotate between these angles hopefully joint_angle_limit = 30 / RADTODEG thruster_joint.lowerAngle = -joint_angle_limit thruster_joint.upperAngle = joint_angle_limit # create the joint auv_thruster.joint = world.CreateJoint(thruster_joint) # setup a simple pygame screen
def json_load(self, path, serialized=False): import json self.world.groundBody.userData = {"saveid": 0} f = open(path, 'r') worldmodel = json.loads(f.read()) f.close() # clean world for joint in self.world.joints: self.world.DestroyJoint(joint) for body in self.world.bodies: if body != self.world.groundBody: self.world.DestroyBody(body) # load bodies for body in worldmodel['bodylist']: bodyDef = box2d.b2BodyDef() if body['dynamic']: bodyDef.type = box2d.b2_dynamicBody bodyDef.position = body['position'] bodyDef.userData = body['userData'] bodyDef.angle = body['angle'] newBody = self.world.CreateBody(bodyDef) # _logger.debug(newBody) newBody.angularVelocity = body['angularVelocity'] newBody.linearVelocity = body['linearVelocity'] if 'shapes' in body: for shape in body['shapes']: if shape['type'] == 'polygon': polyDef = box2d.b2FixtureDef() polyShape = box2d.b2PolygonShape() polyShape.vertices = shape['vertices'] polyDef.shape = polyShape polyDef.density = shape['density'] polyDef.restitution = shape['restitution'] polyDef.friction = shape['friction'] newBody.CreateFixture(polyDef) if shape['type'] == 'circle': circleDef = box2d.b2FixtureDef() circleShape = box2d.b2CircleShape() circleShape.radius = shape['radius'] circleShape.pos = shape['localPosition'] circleDef.shape = circleShape circleDef.density = shape['density'] circleDef.restitution = shape['restitution'] circleDef.friction = shape['friction'] newBody.CreateFixture(circleDef) for joint in worldmodel['jointlist']: if joint['type'] == 'distance': jointDef = box2d.b2DistanceJointDef() body1 = self.getBodyWithSaveId(joint['body1']) anch1 = joint['anchor1'] body2 = self.getBodyWithSaveId(joint['body2']) anch2 = joint['anchor2'] jointDef.collideConnected = joint['collideConnected'] jointDef.Initialize(body1, body2, anch1, anch2) jointDef.userData = joint['userData'] self.world.CreateJoint(jointDef) if joint['type'] == 'revolute': jointDef = box2d.b2RevoluteJointDef() body1 = self.getBodyWithSaveId(joint['body1']) body2 = self.getBodyWithSaveId(joint['body2']) anchor = joint['anchor'] jointDef.Initialize(body1, body2, anchor) jointDef.userData = joint['userData'] jointDef.motorEnabled = joint['enableMotor'] jointDef.motorSpeed = joint['motorSpeed'] jointDef.maxMotorTorque = joint['maxMotorTorque'] self.world.CreateJoint(jointDef) self.additional_vars = {} addvars = {} for (k, v) in list(worldmodel['additional_vars'].items()): addvars[k] = v if serialized and 'trackinfo' in addvars: trackinfo = addvars['trackinfo'] for key, info in trackinfo.items(): if not info[3]: addvars['trackinfo'][key][0] = \ self.getBodyWithSaveId(info[0]) addvars['trackinfo'][key][1] = \ self.getBodyWithSaveId(info[1]) else: addvars['trackinfo'][key][0] = None addvars['trackinfo'][key][1] = None self.additional_vars = addvars for body in self.world.bodies: del body.userData['saveid'] # remove temporary data
def __init__(self, wld, kind, name, pos, ang): w = wld.w Robot.nrobots += 1 self.n = Robot.nrobots self.alive = True self.health = conf.maxhealth self.kind = kind self.name = name self._enable_debug = None self._pingtype = 'w' self._pingangle = 0 self._pingdist = 0 self._pinged = -5 # Tick most recently pinged by another robot's radar self._cannonheat = 0 self._cannonreload = 0 self._outlasted = 0 # number of robots killed while this one is still alive self._damage_caused = 0 self._kills = 0 # number of robots this one delivered the final damage to bodyDef = box2d.b2BodyDef(type=box2d.b2_dynamicBody,) bodyDef.position = pos bodyDef.angle = ang bodyDef.linearDamping = conf.robot_linearDamping bodyDef.angularDamping = conf.robot_angularDamping bodyDef.userData = {} body = w.CreateBody(bodyDef) body.CreatePolygonFixture( box=(1, 1), density=conf.robot_density, friction=conf.robot_friction, restitution=conf.robot_restitution, filter = box2d.b2Filter( groupIndex = -self.n,)) body.userData['actor'] = self body.userData['kind'] = 'robot' self.body = body turretDef = box2d.b2BodyDef(type=box2d.b2_dynamicBody,) turretDef.position = pos turretDef.angle = ang turretDef.linearDamping = 0 turretDef.angularDamping = 0 turret = w.CreateBody(bodyDef) turret.CreatePolygonFixture( box=(.1, .1), density=1, friction=0, restitution=0, filter = box2d.b2Filter( groupIndex = -self.n,)) self.turret = turret jointDef = box2d.b2RevoluteJointDef() jointDef.Initialize(body, turret, pos) jointDef.maxMotorTorque = conf.turret_maxMotorTorque jointDef.motorSpeed = 0.0 jointDef.enableMotor = True self.turretjoint = w.CreateJoint(jointDef) self._turret_torque = 0 v = wld.v.addrobot(pos, ang) self.v = v i = wld.v.addrobotinfo(self.n, name) self.i = i