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
