def create_vehicle(world, chassis_vertices, wheels_data):
chassis_vertices = sorted(chassis_vertices, key=lambda cord: cord[0])
shapes = [
b2PolygonShape(
vertices=[
chassis_vertices[i],
chassis_vertices[i + 1],
chassis_vertices[i + 2],
chassis_vertices[i + 3],
]
)
for i in range(0, len(chassis_vertices) - 2, 2)
]
chassis = world.CreateDynamicBody(
position=(0, 10),
shapes=shapes,
shapeFixture=b2FixtureDef(density=1, filter=b2Filter(groupIndex=-1)),
)
wheels, springs = [], []
for wheel_spec in wheels_data:
wheel = world.CreateDynamicBody(
position=(wheel_spec["position_x"], wheel_spec["position_y"]),
fixtures=b2FixtureDef(
shape=b2CircleShape(radius=wheel_spec["radius"]),
density=wheel_spec["density"],
filter=b2Filter(groupIndex=-1),
),
)
spring = world.CreateWheelJoint(
bodyA=chassis,
bodyB=wheel,
anchor=wheel.position,
axis=(wheel_spec["wheel_axis_x"], wheel_spec["wheel_axis_y"]),
motorSpeed=wheel_spec["speed"],
maxMotorTorque=wheel_spec["torque"],
enableMotor=wheel_spec["enable_motor"],
dampingRatio=wheel_spec["damping_ratio"],
frequencyHz=4.0,
)
wheels.append(wheel)
springs.append(spring)
return chassis, wheels, springs
def set_box(self, body, size, group=-2):
while len(body.fixtures) > 0:
body.DestroyFixture(body.fixtures[0])
size = self.size_to_pybox2d(size)
body.CreatePolygonFixture(box=size,
density=1,
friction=1.0,
filter=b2Filter(groupIndex=group))
body.userData['size'] = size
def createTarget(self, position):
filt = b2Filter(
categoryBits=Rocket.ROCKET_FILTER,
maskBits=Rocket.NO_MASK_FILTER)
frame_fixture = b2FixtureDef(
shape=b2CircleShape(pos=(0, 0), radius=0.5),
density=1,
filter=filt)
return self.world.CreateStaticBody(
position=position,
fixtures=frame_fixture)
def createObject(self, size, position, angle):
filt = b2Filter(
categoryBits=self.ROCKET_FILTER,
maskBits=self.NO_MASK_FILTER)
fixture = b2FixtureDef(
shape=b2PolygonShape(box=size),
density=1,
filter=filt)
obj = self.world.CreateDynamicBody(
position=position,
fixtures=fixture)
jnt = self.world.CreateRevoluteJoint(
bodyA=self.frame,
bodyB=obj,
anchor=position,
collideConnected=False,
lowerAngle=angle,
upperAngle=angle,
enableLimit=True,
enableMotor=False)
return obj, jnt
def createSimpleBox(self,
screenBoxPosition,
screenBoxSize,
categoryBits,
maskBits=settings.B2D_CAT_BITS_GROUND):
screenBoxWidth = screenBoxSize[0]
screenBoxHeight = screenBoxSize[1]
boxWidth = screenBoxWidth / settings.B2D_PPM
boxHeight = screenBoxHeight / settings.B2D_PPM
boxPosition = self.convertScreenToWorld(screenBoxPosition)
shape = b2PolygonShape()
shape.SetAsBox(boxWidth / 2, boxHeight / 2)
fixture = b2FixtureDef()
fixture.density = 1
fixture.friction = 0.0
fixture.shape = shape
fixture.filter = b2Filter(
groupIndex=0,
categoryBits=categoryBits, # I am...
maskBits=maskBits # I collide with...
)
# body definition
bodyDef = b2BodyDef()
bodyDef.position.Set(boxPosition[0], boxPosition[1])
bodyDef.type = b2_dynamicBody
bodyDef.fixedRotation = False
return self.addActor(ActorB2D(screenBoxPosition,
(screenBoxWidth, screenBoxHeight)),
bodyDef=bodyDef,
fixture=fixture)
def __init__(self, args=None):
"""
Initialize all of your objects here.
Be sure to call the Framework's initializer first.
"""
if args == None:
args = {
"start_y": 10,
"len_torso": 6,
"len_crank_arm": 2,
"len_leg": 14,
"motor_torque": 400,
"motor_speed": 5
}
super(Robot, self).__init__()
self.count = 0
self.points = []
self.group = -1
self.start_y = args["start_y"]
self.len_torso = args["len_torso"]
self.len_crank_arm = args["len_crank_arm"]
self.len_leg = args["len_leg"]
self.motor_torque = args["motor_torque"]
self.motor_speed = args["motor_speed"]
#Non controlled constants
torso_width = .001
other_width = .001
leg_angle = numpy.arctan(self.len_torso / self.len_crank_arm)
ground = self.world.CreateStaticBody(
position=(0, 0),
shapes=[b2EdgeShape(vertices=[(-1000, 0), (1000, 0)])])
self.torso = self.world.CreateDynamicBody(
position=(0, self.start_y),
fixedRotation=False,
fixtures=b2FixtureDef(
shape=b2PolygonShape(box=(torso_width, self.len_torso / 2)),
density=1.3,
filter=b2Filter(groupIndex=self.group, )),
)
tail_angle = -numpy.pi / 3
len_tail = .035
self.tail = self.world.CreateDynamicBody(
position=(len_tail * numpy.sin(tail_angle),
self.start_y - len_tail * numpy.cos(tail_angle)),
fixtures=b2FixtureDef(shape=b2PolygonShape(box=(torso_width,
len_tail)),
density=1,
filter=b2Filter(groupIndex=self.group, )),
angle=tail_angle,
)
self.crank_arm = self.world.CreateDynamicBody(
position=(0, self.start_y + self.len_torso / 2),
fixtures=b2FixtureDef(shape=b2PolygonShape(box=(self.len_crank_arm,
other_width)),
density=.1,
filter=b2Filter(groupIndex=self.group, )),
)
#Creating the legs
l = numpy.sqrt(self.len_torso**2 + self.len_crank_arm**2)
x = (self.len_leg - 2 * l) * numpy.cos(leg_angle) / 2
y = (self.len_leg - 2 * l) * numpy.sin(leg_angle) / 2
self.right_leg = self.world.CreateDynamicBody(
position=(x, self.start_y - y - self.len_torso / 2),
angle=(-1 * leg_angle + numpy.pi / 2),
fixtures=b2FixtureDef(shape=b2PolygonShape(box=(other_width,
self.len_leg / 2)),
density=.50,
filter=b2Filter(groupIndex=self.group),
friction=1,
restitution=0),
)
self.left_leg = self.world.CreateDynamicBody(
position=(-x, self.start_y - y - self.len_torso / 2),
angle=(leg_angle + numpy.pi / 2),
fixtures=b2FixtureDef(shape=b2PolygonShape(box=(other_width,
self.len_leg / 2)),
density=1.0,
filter=b2Filter(groupIndex=self.group),
friction=1,
restitution=0),
)
self.right_foot = self.world.CreateDynamicBody(
position=(self.len_leg / 2 * numpy.cos(leg_angle),
self.len_leg * numpy.sin(leg_angle)),
fixtures=b2FixtureDef(shape=b2CircleShape(radius=.003),
density=0.1,
filter=b2Filter(groupIndex=self.group, )),
)
self.left_foot = self.world.CreateDynamicBody(
position=(-1 * self.len_leg / 2 * numpy.cos(leg_angle),
self.len_leg * numpy.sin(leg_angle)),
fixtures=b2FixtureDef(shape=b2CircleShape(radius=.003),
density=0.1,
filter=b2Filter(groupIndex=self.group, )),
)
slot_joint_right = self.world.CreateDynamicBody(
position=(0, self.torso.worldCenter[1] - self.len_torso / 2),
angle=(-1 * leg_angle + numpy.pi / 2),
fixtures=b2FixtureDef(shape=b2CircleShape(radius=.0005),
density=1.0,
filter=b2Filter(groupIndex=self.group, )),
)
slot_joint_left = self.world.CreateDynamicBody(
position=(0, self.torso.worldCenter[1] - self.len_torso / 2),
angle=(leg_angle + numpy.pi / 2),
fixtures=b2FixtureDef(shape=b2CircleShape(radius=.0005),
density=1.0,
filter=b2Filter(groupIndex=self.group, )),
)
#Attach Feat to legs
self.right_foot_weld = self.world.CreateWeldJoint(
bodyA=self.right_foot,
bodyB=self.right_leg,
anchor=(self.torso.worldCenter[0] +
self.len_leg / 2 * numpy.cos(leg_angle),
self.torso.worldCenter[1] +
self.len_leg * numpy.sin(leg_angle)),
)
self.left_foot_weld = self.world.CreateWeldJoint(
bodyA=self.left_foot,
bodyB=self.left_leg,
anchor=(self.torso.worldCenter[0] -
self.len_leg / 2 * numpy.cos(leg_angle),
self.torso.worldCenter[1] +
self.len_leg * numpy.sin(leg_angle)),
)
#Motor Joint
self.motor = self.world.CreateRevoluteJoint(
bodyA=self.torso,
bodyB=self.crank_arm,
anchor=(self.torso.worldCenter[0],
self.torso.worldCenter[1] + self.len_torso / 2),
motorSpeed=self.motor_speed,
maxMotorTorque=self.motor_torque,
enableMotor=True,
)
#Joints at end of pivot
self.right_joint = self.world.CreateRevoluteJoint(
bodyA=self.crank_arm,
bodyB=self.right_leg,
anchor=(self.crank_arm.worldCenter[0] - self.len_crank_arm,
self.crank_arm.worldCenter[1]),
)
self.left_joint = self.world.CreateRevoluteJoint(
bodyA=self.crank_arm,
bodyB=self.left_leg,
anchor=(self.crank_arm.worldCenter[0] + self.len_crank_arm,
self.crank_arm.worldCenter[1]),
)
#Making the slot joint composed of rev joint and prismatic joints
self.left_joint_rev = self.world.CreateRevoluteJoint(
bodyA=slot_joint_right,
bodyB=self.torso,
anchor=(0, self.start_y - self.len_torso / 2),
)
self.right_joint_rev = self.world.CreateRevoluteJoint(
bodyA=slot_joint_left,
bodyB=self.torso,
anchor=(0, self.start_y - self.len_torso / 2),
)
self.right_slide_pris = self.world.CreatePrismaticJoint(
bodyA=slot_joint_right,
bodyB=self.right_leg,
anchor=(self.torso.worldCenter[0],
self.torso.worldCenter[0] - self.len_torso / 2),
localAxisA=(0, 1),
)
self.left_slide_pris = self.world.CreatePrismaticJoint(
bodyA=slot_joint_left,
bodyB=self.left_leg,
anchor=(self.torso.worldCenter[0],
self.torso.worldCenter[0] - self.len_torso / 2),
localAxisA=(0, 1),
)
self.tail_joint = self.world.CreateWeldJoint(
bodyA=self.tail,
bodyB=self.torso,
anchor=(0, self.start_y - self.len_torso / 2),
)
def set_box(self, body, size, group=-2):
while len(body.fixtures) > 0:
body.DestroyFixture(body.fixtures[0])
size = self.size_to_pybox2d(size)
body.CreatePolygonFixture(box=size, density=1, friction=1.0, filter=b2Filter(groupIndex=group))
body.userData['size'] = size
def __init__(self):
super(CollisionFiltering, self).__init__()
# Ground body
world = self.world
ground = world.CreateBody(shapes=b2EdgeShape(vertices=[(-40, 0), (40,
0)]))
# Define the groups that fixtures can fall into
# Note that negative groups never collide with other negative ones.
smallGroup = 1
largeGroup = -1
# And the categories
# Note that these are bit-locations, and as such are written in
# hexadecimal.
# defaultCategory = 0x0001
triangleCategory = 0x0002
boxCategory = 0x0004
circleCategory = 0x0008
# And the masks that define which can hit one another
# A mask of 0xFFFF means that it will collide with everything else in
# its group. The box mask below uses an exclusive OR (XOR) which in
# effect toggles the triangleCategory bit, making boxMask = 0xFFFD.
# Such a mask means that boxes never collide with triangles. (if
# you're still confused, see the implementation details below)
triangleMask = 0xFFFF
boxMask = 0xFFFF ^ triangleCategory
circleMask = 0xFFFF
# The actual implementation determining whether or not two objects
# collide is defined in the C++ source code, but it can be overridden
# in Python (with b2ContactFilter).
# The default behavior goes like this:
# if (filterA.groupIndex == filterB.groupIndex and filterA.groupIndex != 0):
# collide if filterA.groupIndex is greater than zero (negative groups never collide)
# else:
# collide if (filterA.maskBits & filterB.categoryBits) != 0 and (filterA.categoryBits & filterB.maskBits) != 0
#
# So, if they have the same group index (and that index isn't the
# default 0), then they collide if the group index is > 0 (since
# negative groups never collide)
# (Note that a body with the default filter settings will always
# collide with everything else.)
# If their group indices differ, then only if their bitwise-ANDed
# category and mask bits match up do they collide.
#
# For more help, some basics of bit masks might help:
# -> http://en.wikipedia.org/wiki/Mask_%28computing%29
# Small triangle
triangle = b2FixtureDef(
shape=b2PolygonShape(vertices=[(-1, 0), (1, 0), (0, 2)]),
density=1,
filter=b2Filter(
groupIndex=smallGroup,
categoryBits=triangleCategory,
maskBits=triangleMask,
))
world.CreateDynamicBody(
position=(-5, 2),
fixtures=triangle,
)
# Large triangle (recycle definitions)
triangle.shape.vertices = [
2.0 * b2Vec2(v) for v in triangle.shape.vertices
]
triangle.filter.groupIndex = largeGroup
trianglebody = world.CreateDynamicBody(
position=(-5, 6),
fixtures=triangle,
fixedRotation=True, # <--
)
# note that the large triangle will not rotate
# Small box
box = b2FixtureDef(shape=b2PolygonShape(box=(1, 0.5)),
density=1,
restitution=0.1,
filter=b2Filter(
groupIndex=smallGroup,
categoryBits=boxCategory,
maskBits=boxMask,
))
world.CreateDynamicBody(
position=(0, 2),
fixtures=box,
)
# Large box
box.shape.box = (2, 1)
box.filter.groupIndex = largeGroup
world.CreateDynamicBody(
position=(0, 6),
fixtures=box,
)
# Small circle
circle = b2FixtureDef(shape=b2CircleShape(radius=1),
density=1,
filter=b2Filter(
groupIndex=smallGroup,
categoryBits=circleCategory,
maskBits=circleMask,
))
world.CreateDynamicBody(
position=(5, 2),
fixtures=circle,
)
# Large circle
circle.shape.radius *= 2
circle.filter.groupIndex = largeGroup
world.CreateDynamicBody(
position=(5, 6),
fixtures=circle,
)
# Create a joint for fun on the big triangle
# Note that it does not inherit or have anything to do with the
# filter settings of the attached triangle.
box = b2FixtureDef(shape=b2PolygonShape(box=(0.5, 1)), density=1)
testbody = world.CreateDynamicBody(
position=(-5, 10),
fixtures=box,
)
world.CreatePrismaticJoint(
bodyA=trianglebody,
bodyB=testbody,
enableLimit=True,
localAnchorA=(0, 4),
localAnchorB=(0, 0),
localAxisA=(0, 1),
lowerTranslation=-1,
upperTranslation=1,
)
def __init__(self):
super(CollisionFiltering, self).__init__()
# Ground body
world = self.world
ground = world.CreateBody(
shapes=b2EdgeShape(vertices=[(-40, 0), (40, 0)])
)
# Define the groups that fixtures can fall into
# Note that negative groups never collide with other negative ones.
smallGroup = 1
largeGroup = -1
# And the categories
# Note that these are bit-locations, and as such are written in
# hexadecimal.
# defaultCategory = 0x0001
triangleCategory = 0x0002
boxCategory = 0x0004
circleCategory = 0x0008
# And the masks that define which can hit one another
# A mask of 0xFFFF means that it will collide with everything else in
# its group. The box mask below uses an exclusive OR (XOR) which in
# effect toggles the triangleCategory bit, making boxMask = 0xFFFD.
# Such a mask means that boxes never collide with triangles. (if
# you're still confused, see the implementation details below)
triangleMask = 0xFFFF
boxMask = 0xFFFF ^ triangleCategory
circleMask = 0xFFFF
# The actual implementation determining whether or not two objects
# collide is defined in the C++ source code, but it can be overridden
# in Python (with b2ContactFilter).
# The default behavior goes like this:
# if (filterA.groupIndex == filterB.groupIndex and filterA.groupIndex != 0):
# collide if filterA.groupIndex is greater than zero (negative groups never collide)
# else:
# collide if (filterA.maskBits & filterB.categoryBits) != 0 and (filterA.categoryBits & filterB.maskBits) != 0
#
# So, if they have the same group index (and that index isn't the
# default 0), then they collide if the group index is > 0 (since
# negative groups never collide)
# (Note that a body with the default filter settings will always
# collide with everything else.)
# If their group indices differ, then only if their bitwise-ANDed
# category and mask bits match up do they collide.
#
# For more help, some basics of bit masks might help:
# -> http://en.wikipedia.org/wiki/Mask_%28computing%29
# Small triangle
triangle = b2FixtureDef(
shape=b2PolygonShape(vertices=[(-1, 0), (1, 0), (0, 2)]),
density=1,
filter=b2Filter(
groupIndex=smallGroup,
categoryBits=triangleCategory,
maskBits=triangleMask,
)
)
world.CreateDynamicBody(
position=(-5, 2),
fixtures=triangle,
)
# Large triangle (recycle definitions)
triangle.shape.vertices = [
2.0 * b2Vec2(v) for v in triangle.shape.vertices]
triangle.filter.groupIndex = largeGroup
trianglebody = world.CreateDynamicBody(
position=(-5, 6),
fixtures=triangle,
fixedRotation=True, # <--
)
# note that the large triangle will not rotate
# Small box
box = b2FixtureDef(
shape=b2PolygonShape(box=(1, 0.5)),
density=1,
restitution=0.1,
filter = b2Filter(
groupIndex=smallGroup,
categoryBits=boxCategory,
maskBits=boxMask,
)
)
world.CreateDynamicBody(
position=(0, 2),
fixtures=box,
)
# Large box
box.shape.box = (2, 1)
box.filter.groupIndex = largeGroup
world.CreateDynamicBody(
position=(0, 6),
fixtures=box,
)
# Small circle
circle = b2FixtureDef(
shape=b2CircleShape(radius=1),
density=1,
filter=b2Filter(
groupIndex=smallGroup,
categoryBits=circleCategory,
maskBits=circleMask,
)
)
world.CreateDynamicBody(
position=(5, 2),
fixtures=circle,
)
# Large circle
circle.shape.radius *= 2
circle.filter.groupIndex = largeGroup
world.CreateDynamicBody(
position=(5, 6),
fixtures=circle,
)
# Create a joint for fun on the big triangle
# Note that it does not inherit or have anything to do with the
# filter settings of the attached triangle.
box = b2FixtureDef(shape=b2PolygonShape(box=(0.5, 1)), density=1)
testbody = world.CreateDynamicBody(
position=(-5, 10),
fixtures=box,
)
world.CreatePrismaticJoint(
bodyA=trianglebody,
bodyB=testbody,
enableLimit=True,
localAnchorA=(0, 4),
localAnchorB=(0, 0),
localAxisA=(0, 1),
lowerTranslation=-1,
upperTranslation=1,
)
def __init__(self, world, chromosome):
super(Rocket, self).__init__()
self.world = world
self.chromosome = chromosome
if isinstance(self.chromosome, SHACTChromosome):
self.updateFunction = self.updateSHACT
self.brain = Brain(self.chromosome)
elif isinstance(self.chromosome, PControlChromosome):
self.updateFunction = self.updatePController
elif isinstance(self.chromosome, PDControlChromosome):
self.updateFunction = self.updatePDController
elif isinstance(self.chromosome, PIDControlChromosome):
self.updateFunction = self.updatePIDController
elif isinstance(self.chromosome, IslandChromosome):
self.updateFunction = self.updateIslandGA
self.brain = Brain(self.chromosome)
r_w = self.chromosome['width']
r_h = self.chromosome['height']
l_w, l_h = 0.3, 1.6
r_o = r_h * 0.5 + 1.0
filt = b2Filter(
categoryBits=self.ROCKET_FILTER,
maskBits=self.NO_MASK_FILTER)
frame_fixture = b2FixtureDef(
shape=b2PolygonShape(box=(r_w / 2, r_h / 2)),
density=1,
filter=filt)
self.frame = self.world.CreateDynamicBody(
position=(0, r_o),
fixtures=frame_fixture)
# Landing legs.
left_angle = radians(self.chromosome['left.leg.angle'])
right_angle = radians(self.chromosome['right.leg.angle'])
left_origin = (
-(r_w * 0.5) + sin(left_angle) * l_h * 0.5,
r_o - r_h * 0.5 + 0.4 - cos(left_angle) * l_h * 0.5)
right_origin = (
-(r_w * -0.5) + sin(right_angle) * l_h * 0.5,
r_o - r_h * 0.5 + 0.4 - cos(right_angle) * l_h * 0.5)
self.leftLeg, self.leftNULL = self.createObject((0.15, 0.8),
left_origin, left_angle)
self.rightLeg, self.rightNULL = self.createObject((0.15, 0.8),
right_origin, right_angle)
# Bottom thruster.
bottom_origin = (0, r_o - r_h * 0.5)
self.bottomThruster, self.bottomThrusterJoint = self.createObject(
(0.30, 0.30), bottom_origin, radians(
self.chromosome['bottom.thrust.angle']))
# Top thrusters.
left_origin = (r_w * -0.5, r_o + r_h * 0.5 - 0.2)
right_origin = (r_w * 0.5, r_o + r_h * 0.5 - 0.2)
self.leftThruster, self.leftNULL = self.createObject((0.075, 0.15),
left_origin, radians(self.chromosome['left.thrust.angle']))
self.rightThruster, self.rightNULL = self.createObject((0.075, 0.15),
right_origin, radians(self.chromosome['right.thrust.angle']))
