def __init__(self):
super(Pulley, self).__init__()
y, L, a, b = 16.0, 12.0, 1.0, 2.0
# The ground
ground = self.world.create_static_body(
shapes=[edgeShape(vertices=[(-40, 0), (40, 0)]),
circleShape(radius=2, pos=(-10.0, y + b + L)),
circleShape(radius=2, pos=(10.0, y + b + L))]
)
body_a = self.world.create_dynamic_body(
position=(-10, y),
fixtures=fixtureDef(shape=polygonShape(box=(a, b)), density=5.0),
)
body_b = self.world.create_dynamic_body(
position=(10, y),
fixtures=fixtureDef(shape=polygonShape(box=(a, b)), density=5.0),
)
self.pulley = self.world.CreatePulleyJoint(
body_a=body_a,
body_b=body_b,
anchorA=(-10.0, y + b),
anchorB=(10.0, y + b),
groundAnchorA=(-10.0, y + b + L),
groundAnchorB=(10.0, y + b + L),
ratio=1.5,
)
def __init__(self, world, init_angle, init_x, init_y):
self.world = world
self.hull = self.world.CreateDynamicBody(
position = (init_x, init_y),
angle = init_angle,
fixtures = [
fixtureDef(shape = polygonShape(vertices=[ (x*SIZE,y*SIZE) for x,y in HULL_POLY1 ]), density=1.0),
fixtureDef(shape = polygonShape(vertices=[ (x*SIZE,y*SIZE) for x,y in HULL_POLY2 ]), density=1.0),
fixtureDef(shape = polygonShape(vertices=[ (x*SIZE,y*SIZE) for x,y in HULL_POLY3 ]), density=1.0),
fixtureDef(shape = polygonShape(vertices=[ (x*SIZE,y*SIZE) for x,y in HULL_POLY4 ]), density=1.0)
]
)
self.hull.color = (0.8,0.0,0.0)
self.wheels = []
self.fuel_spent = 0.0
WHEEL_POLY = [
(-WHEEL_W,+WHEEL_R), (+WHEEL_W,+WHEEL_R),
(+WHEEL_W,-WHEEL_R), (-WHEEL_W,-WHEEL_R)
]
for wx,wy in WHEELPOS:
front_k = 1.0 if wy > 0 else 1.0
w = self.world.CreateDynamicBody(
position = (init_x+wx*SIZE, init_y+wy*SIZE),
angle = init_angle,
fixtures = fixtureDef(
shape=polygonShape(vertices=[ (x*front_k*SIZE,y*front_k*SIZE) for x,y in WHEEL_POLY ]),
density=0.1,
categoryBits=0x0020,
maskBits=0x001,
restitution=0.0)
)
w.wheel_rad = front_k*WHEEL_R*SIZE
w.color = WHEEL_COLOR
w.gas = 0.0
w.brake = 0.0
w.steer = 0.0
w.phase = 0.0 # wheel angle
w.omega = 0.0 # angular velocity
w.skid_start = None
w.skid_particle = None
rjd = revoluteJointDef(
bodyA=self.hull,
bodyB=w,
localAnchorA=(wx*SIZE,wy*SIZE),
localAnchorB=(0,0),
enableMotor=True,
enableLimit=True,
maxMotorTorque=180*900*SIZE*SIZE,
motorSpeed = 0,
lowerAngle = -0.4,
upperAngle = +0.4,
)
w.joint = self.world.CreateJoint(rjd)
w.tiles = set()
w.userData = w
self.wheels.append(w)
self.drawlist = self.wheels + [self.hull]
self.particles = []
def _create_lander(self, lander, number_of_landers):
initial_y = VIEWPORT_H/SCALE
body_xpos = (lander.player_id + 1) * VIEWPORT_W/SCALE / (number_of_landers + 1)
body_ypos = initial_y
body = self.world.CreateDynamicBody(
position = (body_xpos, body_ypos),
angle=0.0,
fixtures = fixtureDef(
shape=polygonShape(vertices=[ (x/SCALE,y/SCALE) for x,y in LANDER_POLY ]),
density=5.0,
friction=0.1,
categoryBits=0x0010,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
body.color1 = (0.5,0.4,0.9)
body.color2 = (0.3,0.3,0.5)
body.ApplyForceToCenter( (
self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM),
self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM)
), True)
legs = []
for i in [-1,+1]:
leg = self.world.CreateDynamicBody(
position = (body_xpos - i*LEG_AWAY/SCALE, body_ypos),
angle = (i*0.05),
fixtures = fixtureDef(
shape=polygonShape(box=(LEG_W/SCALE, LEG_H/SCALE)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001)
)
leg.ground_contact = False
leg.color1 = (0.5,0.4,0.9)
leg.color2 = (0.3,0.3,0.5)
rjd = revoluteJointDef(
bodyA=body,
bodyB=leg,
localAnchorA=(0, 0),
localAnchorB=(i*LEG_AWAY/SCALE, LEG_DOWN/SCALE),
enableMotor=True,
enableLimit=True,
maxMotorTorque=LEG_SPRING_TORQUE,
motorSpeed=+0.3*i # low enough not to jump back into the sky
)
if i==-1:
rjd.lowerAngle = +0.9 - 0.5 # Yes, the most esoteric numbers here, angles legs have freedom to travel within
rjd.upperAngle = +0.9
else:
rjd.lowerAngle = -0.9
rjd.upperAngle = -0.9 + 0.5
leg.joint = self.world.CreateJoint(rjd)
legs.append(leg)
lander.body, lander.legs = body, legs
def __init__(self):
self.trackWidth = 5.0
self.cartWidth = 0.3
self.cartHeight = 0.2
self.cartMass = 10
self.poleMass = 1
self.force = 0.2
self.trackThickness = self.cartHeight
self.poleLength = 0.5
self.poleThickness = 0.04
self.screenSize = (640,480) #origin upper left
self.worldSize = (float(self.trackWidth),float(self.trackWidth)) #origin at center
self.world = b2.world(gravity=(0,-9.81),doSleep=True)
self.framesPerSecond = 10 # used for dynamics update and for graphics update
self.velocityIterations = 8
self.positionIterations = 6
# Make track bodies and fixtures
self.trackColor = (100,100,100)
poleCategory = 0x0002
f = b2.fixtureDef(shape=b2.polygonShape(box=(self.trackWidth/2,self.trackThickness/2)),
friction=0.000, categoryBits=0x0001, maskBits=(0xFFFF & ~poleCategory))
self.track = self.world.CreateStaticBody(position = (0,0),
fixtures=f, userData={'color': self.trackColor})
self.trackTop = self.world.CreateStaticBody(position = (0,self.trackThickness+self.cartHeight*1.1),
fixtures = f)
f = b2.fixtureDef(shape=b2.polygonShape(box=(self.trackThickness/2,self.trackThickness/2)),
friction=0.000, categoryBits=0x0001, maskBits=(0xFFFF & ~poleCategory))
self.wallLeft = self.world.CreateStaticBody(position = (-self.trackWidth/2+self.trackThickness/2, self.trackThickness),
fixtures=f, userData={'color': self.trackColor})
self.wallRight = self.world.CreateStaticBody(position = (self.trackWidth/2-self.trackThickness/2, self.trackThickness),
fixtures=f,userData={'color': self.trackColor})
# Make cart body and fixture
f = b2.fixtureDef(shape=b2.polygonShape(box=(self.cartWidth/2,self.cartHeight/2)),
density=self.cartMass, friction=0.000, restitution=0.5, categoryBits=0x0001,
maskBits=(0xFFFF & ~poleCategory))
self.cart = self.world.CreateDynamicBody(position=(0,self.trackThickness),
fixtures=f, userData={'color':(20,200,0)})
# Make pole pody and fixture
# Initially pole is hanging down, which defines the zero angle.
f = b2.fixtureDef(shape=b2.polygonShape(box=(self.poleThickness/2, self.poleLength/2)),
density=self.poleMass, categoryBits=poleCategory)
self.pole = self.world.CreateDynamicBody(position=(0,self.trackThickness+self.cartHeight/2+self.poleThickness-self.poleLength/2),
fixtures=f, userData={'color':(200,20,0)})
# Make pole-cart joint
rj = self.world.CreateRevoluteJoint(bodyA = self.pole, bodyB = self.cart,
anchor=(0,self.trackThickness+self.cartHeight/2+self.poleThickness))
def __init__(self):
super(Chain, self).__init__()
# The ground
ground = self.world.CreateBody(
shapes=edgeShape(vertices=[(-40, 0), (40, 0)])
)
plank = fixtureDef(
shape=polygonShape(box=(0.6, 0.125)),
density=20,
friction=0.2,
)
# Create one Chain (Only the left end is fixed)
prevBody = ground
y = 25
numPlanks = 30
for i in range(numPlanks):
body = self.world.CreateDynamicBody(
position=(0.5 + i, y),
fixtures=plank,
)
# You can try a WeldJoint for a slightly different effect.
# self.world.CreateWeldJoint(
self.world.CreateRevoluteJoint(
bodyA=prevBody,
bodyB=body,
anchor=(i, y),
)
prevBody = body
def __init__(self, world):
super(Level1, self).__init__(world)
layer = core.Layer()
self.add_layer(layer)
self.ground = world.CreateStaticBody(
position=(0, -2),
shapes=b2.polygonShape(box=(50, 5))
)
self.im0 = sf.Texture.load_from_file(b'princess.png')
for x in range(100, 800, 200):
sprite = sf.Sprite(self.im0)
sprite.origin = (self.im0.width//2, self.im0.height//2)
# sprite.position = (x, 300)
body = world.CreateDynamicBody()
body.linearDamping = 10.0
body.CreateCircleFixture(radius=2.5, density=10.0, friction=0.3)
body.position = self.convert_coords_to_b2((x, 300))
body.fixedRotation = True
actor = core.Actor(sprite, behavior=box2d.Box2DBehavior(body))
layer.add_actor(actor)
actors = self.layers[0].actors
act = actions.Chain([actions.Move(10.0, 400, -50),
actions.Rotate(3.0, 360),
actions.MoveTo(2.0, 100, 300),
actions.MoveTo(10.0, 700, 300)])
actors[0].add_action(act)
actors[1].add_action(actions.Move(5.0, -400, -50))
actors[2].add_action(actions.MoveTo(15.0, 10, 180))
act2 = actions.Chain([actions.Pause(5.0), actions.Kill()])
actors[3].add_action(act2)
def __init__(self):
self.seed()
self.viewer = None
self.world = Box2D.b2World()
self.terrain = None
self.hull = None
self.prev_shaping = None
self.fd_polygon = fixtureDef(
shape = polygonShape(vertices=
[(0, 0),
(1, 0),
(1, -1),
(0, -1)]),
friction = FRICTION)
self.fd_edge = fixtureDef(
shape = edgeShape(vertices=
[(0, 0),
(1, 1)]),
friction = FRICTION,
categoryBits=0x0001,
)
self.reset()
high = np.array([np.inf]*24)
self.action_space = spaces.Box(np.array([-1,-1,-1,-1]), np.array([+1,+1,+1,+1]))
self.observation_space = spaces.Box(-high, high)
def example_world():
"""Create an example Box2D world for testing
Returns:
tuple(world, bodies, configuration):
- world is a Box2D world with multiple dynamic bodies
- bodies is a dictionary mapping object names to their Box2D
body
- configuration is an example collision-free configuration
"""
get_triangles = metis.geometry.box2d_triangles_from_shapely
obstacle_geometry = shapely.geometry.box(0, 0, 10, 10)
obstacle_geometry = obstacle_geometry.difference(
obstacle_geometry.buffer(-.2))
obstacle_geometry = obstacle_geometry.union(
shapely.geometry.LineString([(5, 0), (5, 10)]).buffer(.1, cap_style=2))
obstacle_geometry = obstacle_geometry.difference(
shapely.geometry.Point(5, 2.5).buffer(1, cap_style=1))
obstacle_geometry = obstacle_geometry.difference(
shapely.geometry.Point(5, 7.5).buffer(1, cap_style=1))
world = b2.world()
obstacles = world.CreateStaticBody()
for triangle in get_triangles(obstacle_geometry):
_ = obstacles.CreateFixture(shape=triangle)
agent = world.CreateDynamicBody()
agent_geometry = shapely.geometry.Polygon([
(2./3., 0.), (-1./3., .4), (-1./3., -.4)])
for triangle in get_triangles(agent_geometry):
_ = agent.CreateFixture(shape=triangle)
boxes = [world.CreateDynamicBody() for _ in xrange(2)]
for box in boxes:
box.CreateFixture(shape=b2.polygonShape(box=(.8, .8)))
bodies = {'robot': agent, 'box1': boxes[0], 'box2': boxes[1]}
sample_configuration = {
'robot': (1, 2, 0), 'box1': (3, 2, -.2), 'box2': (5, 2.5, 0.1)}
return world, bodies, sample_configuration
def addPhysicsObject(self, physObj, color=None):
'''
Adds a PhysicsObject to this SimulationEnvironment,
adding it to the collision manager and to the renderables
'''
if isinstance(physObj, StaticPhysicsObject):
body = self.world.CreateStaticBody(position=physObj.position,
shapes=b2.polygonShape(box=physObj.size))
elif isinstance(physObj,DynamicPhysicsObject):
if len(physObj.size) != 2:
raise ValueError("Size must be a two-tuple")
body = self.world.CreateDynamicBody(position=physObj.position, angle=physObj.angle)
halfSize = tuple(x/2.0 for x in physObj.size) #box takes half size, not full size
body.CreatePolygonFixture(box=halfSize, density=physObj.density, friction=physObj.friction)
else:
raise ValueError("Object was neither a StaticPhysicsObject nor a DynamicPhysicsObject. Unexpected")
self.objectDict[physObj.identifier] = body
return body
def __init__(self, world):
super(Level1, self).__init__(world)
layer = core.Layer()
self.add_layer(layer)
self.ground = world.CreateStaticBody(
position=(0, -2),
shapes=b2.polygonShape(box=(50, 5))
)
self.im0 = sf.Texture.load_from_file(b'princess.png')
for y in range(0, 500, 120):
for x in range(200, 700, 120):
sprite = sf.Sprite(self.im0)
sprite.origin = (self.im0.width//2, self.im0.height//2)
sprite.position = (x, y)
actor = core.Actor(sprite)
body = world.CreateDynamicBody()
body.CreateCircleFixture(radius=2.5, density=1.0, friction=0.3)
actor.add_action(Updater(body), name='box2d')
layer.add_actor(actor)
def __init__(self, world):
super(Level1, self).__init__(world)
layer = core.Layer()
self.add_layer(layer)
self.ground = world.CreateStaticBody(
position=(0, -2),
shapes=b2.polygonShape(box=(50, 5))
)
self.im0 = sf.Texture.load_from_file(b'princess.png')
for x in range(100, 800, 300):
sprite = sf.Sprite(self.im0)
sprite.origin = (self.im0.width//2, self.im0.height//2)
sprite.position = (x, 300)
actor = core.Actor(sprite)
body = world.CreateDynamicBody()
body.CreateCircleFixture(radius=2.5, density=1.0, friction=0.3)
actor.add_action(Updater(body), name='box2d')
layer.add_actor(actor)
actors = self.layers[0].actors
actors[1].obstacle = True
actors[0].add_action(actions.DefferedCall(1.0, self.test_los))
def __init__(self, world, name=None, size=(1, 1), exits=None, floor=None, contents=None):
self.world = world
self.name = name
corners = [
(0, 0),
(0, size[0] + 1),
(size[1] + 1, size[0] + 1),
(size[1] + 1, 0)
]
exit_coords = []
for exit in exits:
exit_pos = exit.values()[0]
exit_coords.append((exit_pos[0]+1, exit_pos[1]))
filled_tiles = []
filled_tiles.extend(tiles_for_wall(corners[0], corners[1], exit_coords))
filled_tiles.extend(tiles_for_wall(corners[1], corners[2], exit_coords))
filled_tiles.extend(tiles_for_wall(corners[2], corners[3], exit_coords))
filled_tiles.extend(tiles_for_wall(corners[3], corners[0], exit_coords))
self.exit_coords = exit_coords
self.filled_tiles = filled_tiles
self.floor = floor
if exits is None:
exits = []
exits.sort()
self.exits = exits
bodies = []
for tile in filled_tiles:
position = (tile[0] + 0.5, tile[1] + 0.5)
shape = b2.polygonShape(box=(0.5, 0.5))
bodies.append(self.world.world.CreateStaticBody(shapes=shape, position=position, userData=self))
self.bodies = bodies
#dictionary of {coords: object}
if contents is None:
contents = {}
for i in contents.values():
i.location = self
self.contents = contents
def _generate_terrain(self, hardcore):
GRASS, STUMP, STAIRS, PIT, _STATES_ = range(5)
state = GRASS
velocity = 0.0
y = TERRAIN_HEIGHT
counter = TERRAIN_STARTPAD
oneshot = False
self.terrain = []
self.terrain_x = []
self.terrain_y = []
for i in range(TERRAIN_LENGTH):
x = i*TERRAIN_STEP
self.terrain_x.append(x)
if state==GRASS and not oneshot:
velocity = 0.8*velocity + 0.01*np.sign(TERRAIN_HEIGHT - y)
if i > TERRAIN_STARTPAD: velocity=0 #+= self.np_random.uniform(-1, 1)/SCALE #1
y += velocity
#y = TERRAIN_HEIGHT
elif state==PIT and oneshot:
counter = self.np_random.randint(3, 5)
poly = [
(x, y),
(x+TERRAIN_STEP, y),
(x+TERRAIN_STEP, y-4*TERRAIN_STEP),
(x, y-4*TERRAIN_STEP),
]
t = self.world.CreateStaticBody(
fixtures = fixtureDef(
shape=polygonShape(vertices=poly),
friction = FRICTION
))
t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
self.terrain.append(t)
t = self.world.CreateStaticBody(
fixtures = fixtureDef(
shape=polygonShape(vertices=[(p[0]+TERRAIN_STEP*counter,p[1]) for p in poly]),
friction = FRICTION
))
t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
self.terrain.append(t)
counter += 2
original_y = y
elif state==PIT and not oneshot:
y = original_y
if counter > 1:
y -= 4*TERRAIN_STEP
elif state==STUMP and oneshot:
counter = self.np_random.randint(1, 3)
poly = [
(x, y),
(x+counter*TERRAIN_STEP, y),
(x+counter*TERRAIN_STEP, y+counter*TERRAIN_STEP),
(x, y+counter*TERRAIN_STEP),
]
t = self.world.CreateStaticBody(
fixtures = fixtureDef(
shape=polygonShape(vertices=poly),
friction = FRICTION
))
t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
self.terrain.append(t)
elif state==STAIRS and oneshot:
stair_height = +1 if self.np_random.rand() > 0.5 else -1
stair_width = self.np_random.randint(4, 5)
stair_steps = self.np_random.randint(3, 5)
original_y = y
for s in range(stair_steps):
poly = [
(x+( s*stair_width)*TERRAIN_STEP, y+( s*stair_height)*TERRAIN_STEP),
(x+((1+s)*stair_width)*TERRAIN_STEP, y+( s*stair_height)*TERRAIN_STEP),
(x+((1+s)*stair_width)*TERRAIN_STEP, y+(-1+s*stair_height)*TERRAIN_STEP),
(x+( s*stair_width)*TERRAIN_STEP, y+(-1+s*stair_height)*TERRAIN_STEP),
]
t = self.world.CreateStaticBody(
fixtures = fixtureDef(
shape=polygonShape(vertices=poly),
friction = FRICTION
))
t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
self.terrain.append(t)
counter = stair_steps*stair_width
elif state==STAIRS and not oneshot:
s = stair_steps*stair_width - counter - stair_height
n = s/stair_width
y = original_y + (n*stair_height)*TERRAIN_STEP
oneshot = False
self.terrain_y.append(y)
counter -= 1
if counter==0:
counter = self.np_random.randint(TERRAIN_GRASS/2, TERRAIN_GRASS)
if state==GRASS and hardcore:
state = self.np_random.randint(1, _STATES_)
oneshot = True
else:
state = GRASS
oneshot = True
self.terrain_poly = []
for i in range(TERRAIN_LENGTH-1):
poly = [
(self.terrain_x[i], self.terrain_y[i]),
(self.terrain_x[i+1], self.terrain_y[i+1])
]
t = self.world.CreateStaticBody(
fixtures = fixtureDef(
shape=edgeShape(vertices=poly),
friction = FRICTION,
categoryBits=0x0001,
))
color = (0.3, 1.0 if i%2==0 else 0.8, 0.3)
t.color1 = color
t.color2 = color
self.terrain.append(t)
color = (0.4, 0.6, 0.3)
poly += [ (poly[1][0], 0), (poly[0][0], 0) ]
self.terrain_poly.append( (poly, color) )
self.terrain.reverse()
def reset(self):
self._destroy()
self.world.contactListener_keepref = ContactDetector(self)
self.world.contactListener = self.world.contactListener_keepref
self.game_over = False
self.prev_shaping = None
self.throttle = 0
self.gimbal = 0.0
self.landed_ticks = 0
self.stepnumber = 0
self.smoke = []
self.episode_number += 1
# self.terrainheigth = self.np_random.uniform(H / 20, H / 10)
self.terrainheigth = H / 20
self.shipheight = self.terrainheigth + SHIP_HEIGHT
# ship_pos = self.np_random.uniform(0, SHIP_WIDTH / SCALE) + SHIP_WIDTH / SCALE
ship_pos = W / 2
self.helipad_x1 = ship_pos - SHIP_WIDTH / 2
self.helipad_x2 = self.helipad_x1 + SHIP_WIDTH
self.helipad_y = self.terrainheigth + SHIP_HEIGHT
self.water = self.world.CreateStaticBody(
fixtures=fixtureDef(
shape=polygonShape(
vertices=(
(0, 0),
(W, 0),
(W, self.terrainheigth),
(0, self.terrainheigth),
)
),
friction=0.1,
restitution=0.0,
)
)
self.water.color1 = rgb(70, 96, 176)
self.ship = self.world.CreateStaticBody(
fixtures=fixtureDef(
shape=polygonShape(
vertices=(
(self.helipad_x1, self.terrainheigth),
(self.helipad_x2, self.terrainheigth),
(self.helipad_x2, self.terrainheigth + SHIP_HEIGHT),
(self.helipad_x1, self.terrainheigth + SHIP_HEIGHT),
)
),
friction=0.5,
restitution=0.0,
)
)
self.containers = []
for side in [-1, 1]:
self.containers.append(
self.world.CreateStaticBody(
fixtures=fixtureDef(
shape=polygonShape(
vertices=(
(
ship_pos + side * 0.95 * SHIP_WIDTH / 2,
self.helipad_y,
),
(
ship_pos + side * 0.95 * SHIP_WIDTH / 2,
self.helipad_y + SHIP_HEIGHT,
),
(
ship_pos
+ side * 0.95 * SHIP_WIDTH / 2
- side * SHIP_HEIGHT,
self.helipad_y + SHIP_HEIGHT,
),
(
ship_pos
+ side * 0.95 * SHIP_WIDTH / 2
- side * SHIP_HEIGHT,
self.helipad_y,
),
)
),
friction=0.2,
restitution=0.0,
)
)
)
self.containers[-1].color1 = rgb(206, 206, 2)
self.ship.color1 = (0.2, 0.2, 0.2)
def initial_rocket_pos(level):
if level > 0:
initial_x = W / 2 + W * np.random.uniform(-0.3, 0.3)
initial_y = H * 0.95
else:
initial_x = W / 2 + W * np.random.uniform(-0.03, 0.03)
initial_y = H * 0.95
return initial_x, initial_y
initial_x, initial_y = initial_rocket_pos(self.level_number)
self.lander = self.world.CreateDynamicBody(
position=(initial_x, initial_y),
angle=0.0,
fixtures=fixtureDef(
shape=polygonShape(
vertices=(
(-ROCKET_WIDTH / 2, 0),
(+ROCKET_WIDTH / 2, 0),
(ROCKET_WIDTH / 2, +ROCKET_HEIGHT),
(-ROCKET_WIDTH / 2, +ROCKET_HEIGHT),
)
),
density=1.0,
friction=0.5,
categoryBits=0x0010,
maskBits=0x001,
restitution=0.0,
),
)
self.lander.color1 = rgb(230, 230, 230)
leg_length_modified = compute_leg_length(LEG_LENGTH, self.level_number)
for i in [-1, +1]:
leg = self.world.CreateDynamicBody(
position=(initial_x - i * LEG_AWAY, initial_y + ROCKET_WIDTH * 0.2),
angle=(i * BASE_ANGLE),
fixtures=fixtureDef(
shape=polygonShape(
vertices=(
(0, 0),
(0, leg_length_modified / 25),
(i * leg_length_modified, 0),
(i * leg_length_modified, -leg_length_modified / 20),
(i * leg_length_modified / 3, -leg_length_modified / 7),
)
),
density=1,
restitution=0.0,
friction=0.2,
categoryBits=0x0020,
maskBits=0x001,
),
)
leg.ground_contact = False
leg.color1 = (0.25, 0.25, 0.25)
rjd = revoluteJointDef(
bodyA=self.lander,
bodyB=leg,
localAnchorA=(i * LEG_AWAY, ROCKET_WIDTH * 0.2),
localAnchorB=(0, 0),
enableLimit=True,
maxMotorTorque=2500.0,
motorSpeed=-0.05 * i,
enableMotor=True,
)
djd = distanceJointDef(
bodyA=self.lander,
bodyB=leg,
anchorA=(i * LEG_AWAY, ROCKET_HEIGHT / 8),
anchorB=leg.fixtures[0].body.transform * (i * leg_length_modified, 0),
collideConnected=False,
frequencyHz=0.01,
dampingRatio=0.9,
)
if i == 1:
rjd.lowerAngle = -SPRING_ANGLE
rjd.upperAngle = 0
else:
rjd.lowerAngle = 0
rjd.upperAngle = +SPRING_ANGLE
leg.joint = self.world.CreateJoint(rjd)
leg.joint2 = self.world.CreateJoint(djd)
self.legs.append(leg)
def random_factor_velocity(INITIAL_RANDOM, level):
if level > 1:
return INITIAL_RANDOM + 0.2
else:
return INITIAL_RANDOM
random_velocity_factor = random_factor_velocity(
INITIAL_RANDOM, self.level_number
)
self.lander.linearVelocity = (
-self.np_random.uniform(0, random_velocity_factor)
* START_SPEED
* (initial_x - W / 2)
/ W,
-START_SPEED,
)
self.lander.angularVelocity = (1 + random_velocity_factor) * np.random.uniform(
-1, 1
)
self.drawlist = (
self.legs + [self.water] + [self.ship] + self.containers + [self.lander]
)
if self.continuous:
return self.step([0, 0, 0])[0]
else:
return self.step(6)[0]
def _create_track(self):
CHECKPOINTS = 12 # 12 = nombre de virages (11) + le départ (1)
# Create checkpoints
checkpoints = []
self.obstacles_positions = []
for c in range(CHECKPOINTS):
noise = self.np_random.uniform(0, 2 * 3.14159 * 1 / CHECKPOINTS)
alpha = 2 * 3.14159 * c / CHECKPOINTS + noise
rad = self.np_random.uniform(TRACK_RAD / 3, TRACK_RAD)
if c == 0:
alpha = 0
rad = 1.5 * TRACK_RAD
if c == CHECKPOINTS - 1:
alpha = 2 * 3.14159 * c / CHECKPOINTS
self.start_alpha = 2 * 3.14159 * (-0.5) / CHECKPOINTS
rad = 1.5 * TRACK_RAD
checkpoints.append(
(alpha, rad * math.cos(alpha), rad * math.sin(alpha)))
self.road = []
# Go from one checkpoint to another to create track
x, y, beta = 1.5 * TRACK_RAD, 0, 0
dest_i = 0
laps = 0
track = []
no_freeze = 2500
visited_other_side = False
while True:
alpha = math.atan2(y, x)
if visited_other_side and alpha > 0:
laps += 1
visited_other_side = False
if alpha < 0:
visited_other_side = True
alpha += 2 * 3.14159
while True: # Find destination from checkpoints
failed = True
while True:
dest_alpha, dest_x, dest_y = checkpoints[dest_i %
len(checkpoints)]
if alpha <= dest_alpha:
failed = False
break
dest_i += 1
if dest_i % len(checkpoints) == 0:
break
if not failed:
break
alpha -= 2 * 3.14159
continue
r1x = math.cos(beta)
r1y = math.sin(beta)
p1x = -r1y
p1y = r1x
dest_dx = dest_x - x # vector towards destination
dest_dy = dest_y - y
# destination vector projected on rad:
proj = r1x * dest_dx + r1y * dest_dy
while beta - alpha > 1.5 * 3.14159:
beta -= 2 * 3.14159
while beta - alpha < -1.5 * 3.14159:
beta += 2 * 3.14159
prev_beta = beta
proj *= SCALE
if proj > 0.3:
beta -= min(TRACK_TURN_RATE, abs(0.001 * proj))
if proj < -0.3:
beta += min(TRACK_TURN_RATE, abs(0.001 * proj))
x += p1x * TRACK_DETAIL_STEP
y += p1y * TRACK_DETAIL_STEP
track.append((alpha, prev_beta * 0.5 + beta * 0.5, x, y))
if laps > 4:
break
no_freeze -= 1
if no_freeze == 0:
break
# Find closed loop range i1..i2, first loop should be ignored, second is OK
i1, i2 = -1, -1
i = len(track)
while True:
i -= 1
if i == 0:
return False # Failed
pass_through_start = track[i][0] > self.start_alpha and track[
i - 1][0] <= self.start_alpha
if pass_through_start and i2 == -1:
i2 = i
elif pass_through_start and i1 == -1:
i1 = i
break
if self.verbose == 1:
print("Track generation: %i..%i -> %i-tiles track" %
(i1, i2, i2 - i1))
assert i1 != -1
assert i2 != -1
track = track[i1:i2 - 1]
first_beta = track[0][1]
first_perp_x = math.cos(first_beta)
first_perp_y = math.sin(first_beta)
# Length of perpendicular jump to put together head and tail
well_glued_together = np.sqrt(
np.square(first_perp_x * (track[0][2] - track[-1][2])) +
np.square(first_perp_y * (track[0][3] - track[-1][3])))
if well_glued_together > TRACK_DETAIL_STEP:
return False
# Red-white border on hard turns
border = [False] * len(track)
for i in range(len(track)):
good = True
oneside = 0
for neg in range(BORDER_MIN_COUNT):
beta1 = track[i - neg - 0][1]
beta2 = track[i - neg - 1][1]
good &= abs(beta1 - beta2) > TRACK_TURN_RATE * 0.2
oneside += np.sign(beta1 - beta2)
good &= abs(oneside) == BORDER_MIN_COUNT
border[i] = good
for i in range(len(track)):
for neg in range(BORDER_MIN_COUNT):
border[i - neg] |= border[i]
# Create tiles and obstacles
last_obstacle = 15 # pour que le début de course se passe sans obstacle
for i in range(len(track)):
alpha1, beta1, x1, y1 = track[i]
alpha2, beta2, x2, y2 = track[i - 1]
road1_l = (x1 - TRACK_WIDTH * math.cos(beta1),
y1 - TRACK_WIDTH * math.sin(beta1))
road1_r = (x1 + TRACK_WIDTH * math.cos(beta1),
y1 + TRACK_WIDTH * math.sin(beta1))
road2_l = (x2 - TRACK_WIDTH * math.cos(beta2),
y2 - TRACK_WIDTH * math.sin(beta2))
road2_r = (x2 + TRACK_WIDTH * math.cos(beta2),
y2 + TRACK_WIDTH * math.sin(beta2))
vertices = [road1_l, road1_r, road2_r, road2_l]
self.fd_tile.shape.vertices = vertices
t = self.world.CreateStaticBody(fixtures=self.fd_tile)
t.userData = t
c = 0.01 * (i % 3)
t.color = [0, 0.128, 0.624, 0.019]
t.road_visited = False
t.road_friction = 1.0
t.fixtures[0].sensor = True
self.road_poly.append(([road1_l, road1_r, road2_r,
road2_l], t.color))
self.road.append(t)
if border[i]:
side = np.sign(beta2 - beta1)
b1_l = (x1 + side * TRACK_WIDTH * math.cos(beta1),
y1 + side * TRACK_WIDTH * math.sin(beta1))
b1_r = (x1 + side * (TRACK_WIDTH + BORDER) * math.cos(beta1),
y1 + side * (TRACK_WIDTH + BORDER) * math.sin(beta1))
b2_l = (x2 + side * TRACK_WIDTH * math.cos(beta2),
y2 + side * TRACK_WIDTH * math.sin(beta2))
b2_r = (x2 + side * (TRACK_WIDTH + BORDER) * math.cos(beta2),
y2 + side * (TRACK_WIDTH + BORDER) * math.sin(beta2))
self.road_poly.append(([b1_l, b1_r, b2_r,
b2_l], (1, 1, 1) if i % 2 == 0 else
(1, 0, 0)))
self.random_obs += 1
self.seed(self.random_obs)
if (self.np_random.uniform(0, 1) < OBSTACLE_PROB) and (
last_obstacle <=
0): # i > 15 pour que la course soit toujours faisable
last_obstacle = 8
deriv_left = self.np_random.uniform(TRACK_WIDTH)
deriv_right = TRACK_WIDTH - deriv_left
obs1_l = (x1 - (TRACK_WIDTH - deriv_left) * math.cos(beta1),
y1 - (TRACK_WIDTH - deriv_left) * math.sin(beta1))
obs1_r = (x1 + (TRACK_WIDTH - deriv_right) * math.cos(beta1),
y1 + (TRACK_WIDTH - deriv_right) * math.sin(beta1))
obs2_l = (x2 - (TRACK_WIDTH - deriv_left) * math.cos(beta2),
y2 - (TRACK_WIDTH - deriv_left) * math.sin(beta2))
obs2_r = (x2 + (TRACK_WIDTH - deriv_right) * math.cos(beta2),
y2 + (TRACK_WIDTH - deriv_right) * math.sin(beta2))
self.obstacles_positions.append(
(obs1_l, obs1_r, obs2_r, obs2_l))
vertices = [obs1_l, obs1_r, obs2_r, obs2_l]
obstacle = fixtureDef(shape=polygonShape(vertices=vertices))
obstacle.userData = obstacle
obstacle.color = [0.1568, 0.598, 0.2862, 0]
self.road_poly.append(([obs1_l, obs1_r, obs2_r,
obs2_l], obstacle.color))
last_obstacle -= 1
self.track = track
return True
def _create_track(self):
CHECKPOINTS = 12
# Create checkpoints
checkpoints = []
for c in range(CHECKPOINTS):
alpha = 2 * math.pi * c / CHECKPOINTS + self.np_random.uniform(
0, 2 * math.pi * 1 / CHECKPOINTS)
rad = self.np_random.uniform(TRACK_RAD / 3, TRACK_RAD)
if c == 0:
alpha = 0
rad = 1.5 * TRACK_RAD
if c == CHECKPOINTS - 1:
alpha = 2 * math.pi * c / CHECKPOINTS
self.start_alpha = 2 * math.pi * (-0.5) / CHECKPOINTS
rad = 1.5 * TRACK_RAD
checkpoints.append(
(alpha, rad * math.cos(alpha), rad * math.sin(alpha)))
#print "\n".join(str(h) for h in checkpoints)
# self.road_poly = [ ( # uncomment this to see checkpoints
# [ (tx,ty) for a,tx,ty in checkpoints ],
# (0.7,0.7,0.9) ) ]
self.road = []
# Go from one checkpoint to another to create track
x, y, beta = 1.5 * TRACK_RAD, 0, 0
dest_i = 0
laps = 0
track = []
no_freeze = 2500
visited_other_side = False
while 1:
alpha = math.atan2(y, x)
if visited_other_side and alpha > 0:
laps += 1
visited_other_side = False
if alpha < 0:
visited_other_side = True
alpha += 2 * math.pi
while True: # Find destination from checkpoints
failed = True
while True:
dest_alpha, dest_x, dest_y = checkpoints[dest_i %
len(checkpoints)]
if alpha <= dest_alpha:
failed = False
break
dest_i += 1
if dest_i % len(checkpoints) == 0: break
if not failed: break
alpha -= 2 * math.pi
continue
r1x = math.cos(beta)
r1y = math.sin(beta)
p1x = -r1y
p1y = r1x
dest_dx = dest_x - x # vector towards destination
dest_dy = dest_y - y
proj = r1x * dest_dx + r1y * dest_dy # destination vector projected on rad
while beta - alpha > 1.5 * math.pi:
beta -= 2 * math.pi
while beta - alpha < -1.5 * math.pi:
beta += 2 * math.pi
prev_beta = beta
proj *= SCALE
if proj > 0.3: beta -= min(TRACK_TURN_RATE, abs(0.001 * proj))
if proj < -0.3: beta += min(TRACK_TURN_RATE, abs(0.001 * proj))
x += p1x * TRACK_DETAIL_STEP
y += p1y * TRACK_DETAIL_STEP
track.append((alpha, prev_beta * 0.5 + beta * 0.5, x, y))
if laps > 4: break
no_freeze -= 1
if no_freeze == 0: break
#print "\n".join([str(t) for t in enumerate(track)])
# Find closed loop range i1..i2, first loop should be ignored, second is OK
i1, i2 = -1, -1
i = len(track)
while True:
i -= 1
if i == 0: return False # Failed
pass_through_start = track[i][0] > self.start_alpha and track[
i - 1][0] <= self.start_alpha
if pass_through_start and i2 == -1:
i2 = i
elif pass_through_start and i1 == -1:
i1 = i
break
print("Track generation: %i..%i -> %i-tiles track" % (i1, i2, i2 - i1))
assert i1 != -1
assert i2 != -1
track = track[i1:i2 - 1]
first_beta = track[0][1]
first_perp_x = math.cos(first_beta)
first_perp_y = math.sin(first_beta)
# Length of perpendicular jump to put together head and tail
well_glued_together = np.sqrt(
np.square(first_perp_x * (track[0][2] - track[-1][2])) +
np.square(first_perp_y * (track[0][3] - track[-1][3])))
if well_glued_together > TRACK_DETAIL_STEP:
return False
# Red-white border on hard turns
border = [False] * len(track)
for i in range(len(track)):
good = True
oneside = 0
for neg in range(BORDER_MIN_COUNT):
beta1 = track[i - neg - 0][1]
beta2 = track[i - neg - 1][1]
good &= abs(beta1 - beta2) > TRACK_TURN_RATE * 0.2
oneside += np.sign(beta1 - beta2)
good &= abs(oneside) == BORDER_MIN_COUNT
border[i] = good
for i in range(len(track)):
for neg in range(BORDER_MIN_COUNT):
border[i - neg] |= border[i]
# Create tiles
for i in range(len(track)):
alpha1, beta1, x1, y1 = track[i]
alpha2, beta2, x2, y2 = track[i - 1]
road1_l = (x1 - TRACK_WIDTH * math.cos(beta1),
y1 - TRACK_WIDTH * math.sin(beta1))
road1_r = (x1 + TRACK_WIDTH * math.cos(beta1),
y1 + TRACK_WIDTH * math.sin(beta1))
road2_l = (x2 - TRACK_WIDTH * math.cos(beta2),
y2 - TRACK_WIDTH * math.sin(beta2))
road2_r = (x2 + TRACK_WIDTH * math.cos(beta2),
y2 + TRACK_WIDTH * math.sin(beta2))
t = self.world.CreateStaticBody(fixtures=fixtureDef(
shape=polygonShape(
vertices=[road1_l, road1_r, road2_r, road2_l])))
t.userData = t
c = 0.1 * (i % 3)
t.color = [ROAD_COLOR[0] + c, ROAD_COLOR[1] + c, ROAD_COLOR[2] + c]
t.road_visited = False
t.road_friction = 1.0
t.fixtures[0].sensor = True
self.road_poly.append(([road1_l, road1_r, road2_r,
road2_l], t.color))
self.road.append(t)
if border[i]:
side = np.sign(beta2 - beta1)
b1_l = (x1 + side * TRACK_WIDTH * math.cos(beta1),
y1 + side * TRACK_WIDTH * math.sin(beta1))
b1_r = (x1 + side * (TRACK_WIDTH + BORDER) * math.cos(beta1),
y1 + side * (TRACK_WIDTH + BORDER) * math.sin(beta1))
b2_l = (x2 + side * TRACK_WIDTH * math.cos(beta2),
y2 + side * TRACK_WIDTH * math.sin(beta2))
b2_r = (x2 + side * (TRACK_WIDTH + BORDER) * math.cos(beta2),
y2 + side * (TRACK_WIDTH + BORDER) * math.sin(beta2))
self.road_poly.append(([b1_l, b1_r, b2_r,
b2_l], (1, 1, 1) if i % 2 == 0 else
(1, 0, 0)))
self.track = track
return True
def __init__(self, world, init_angle, init_x, init_y, userData, robot_id,
group, color):
self.world = world
self.hull = self.world.CreateDynamicBody(
position=(init_x, init_y),
angle=0,
fixtures=[
fixtureDef(
shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in HULL_POLY]),
density=1.0,
restitution=1,
userData=userData,
friction=1),
])
# self.hull.color = (0.8,0.0,0.0)
self.hull.color = color
self.hull.userData = userData
self.wheels = []
for wx, wy in WHEEL_POS:
front_k = 1.0 if wy > 0 else 1.0
w = self.world.CreateDynamicBody(
position=(init_x + wx * SIZE, init_y + wy * SIZE),
angle=0,
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x * front_k * SIZE,
y * front_k * SIZE)
for x, y in WHEEL_POLY]),
density=0.1,
restitution=1,
userData=userData + "_wheel",
friction=1))
rjd = revoluteJointDef(
bodyA=self.hull,
bodyB=w,
localAnchorA=(wx * SIZE, wy * SIZE),
localAnchorB=(0, 0),
enableMotor=False,
enableLimit=True,
lowerAngle=-0.0,
upperAngle=+0.0,
)
w.joint = self.world.CreateJoint(rjd)
w.color = WHEEL_COLOR
w.userData = userData
self.wheels.append(w)
self.gun = self.world.CreateDynamicBody(
position=(init_x, init_y),
angle=0,
fixtures=[
fixtureDef(shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in GUN_POLY]),
categoryBits=0x00,
density=1e-6,
userData=userData)
])
self.gun_joint = self.world.CreateJoint(
revoluteJointDef(
bodyA=self.hull,
bodyB=self.gun,
localAnchorA=(0, 0),
localAnchorB=(0, 0),
enableMotor=True,
enableLimit=False,
maxMotorTorque=180 * 900 * SIZE * SIZE,
motorSpeed=0.0,
#lowerAngle = -math.pi,
#upperAngle = +math.pi,
))
self.gun.color = (0.1, 0.1, 0.1)
self.gun.angle = init_angle
self.hull.angle = init_angle * 1.04
self.drawlist = self.wheels + [self.hull, self.gun]
self.group = group
self.robot_id = robot_id
self.health = 1000.0
self.buffLeftTime = 0
self.command = {"ahead": 0, "rotate": 0, "transverse": 0}
self.bullets_num = 40
self.opportuniy_to_add_bullets = 2
def draw(self, world, init_x, init_y, force_to_center):
head = world.CreateDynamicBody(
position=(init_x, init_y + self.BODY_HEIGHT / self.SCALE / 2 +
self.HEAD_HEIGHT / self.SCALE / 2 + 0.2),
fixtures=fixtureDef(shape=polygonShape(
vertices=[(x / self.SCALE, y / self.SCALE)
for x, y in [(-5, +10), (+5, +10), (+5,
-10), (-5,
-10)]]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x1))
head.color1 = (0.5, 0.4, 0.9)
head.color2 = (0.3, 0.3, 0.5)
head.ApplyForceToCenter((force_to_center, 0), True)
head.userData = CustomBodyUserData(True,
is_contact_critical=True,
name="head")
self.body_parts.append(head)
self.reference_head_object = head
body = world.CreateDynamicBody(
position=(init_x, init_y),
fixtures=fixtureDef(
shape=polygonShape(
vertices=[(x / self.SCALE, y / self.SCALE)
for x, y in [(-12, +25), (+12,
+25), (+8,
-20), (-8,
-20)]]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x1 # collide only with ground
))
body.color1 = (0.5, 0.4, 0.9)
body.color2 = (0.3, 0.3, 0.5)
body.userData = CustomBodyUserData(True,
is_contact_critical=True,
name="body")
self.body_parts.append(body)
rjd = revoluteJointDef(
bodyA=head,
bodyB=body,
anchor=(init_x, init_y + self.BODY_HEIGHT / self.SCALE / 2),
enableMotor=False,
enableLimit=True,
lowerAngle=-0.1 * np.pi,
upperAngle=0.1 * np.pi,
)
world.CreateJoint(rjd)
UPPER_LIMB_FD = fixtureDef(shape=polygonShape(box=(self.LIMB_W / 2,
self.LIMB_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x1)
LOWER_LIMB_FD = fixtureDef(
shape=polygonShape(box=(0.8 * self.LIMB_W / 2, self.LIMB_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x1)
HAND_PART_FD = fixtureDef(
shape=polygonShape(box=(self.HAND_PART_W / 2,
self.HAND_PART_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x1)
# ARMS
for j in [-1, -1]:
upper = world.CreateDynamicBody(
position=(init_x, init_y + self.LIMB_H / 2 + self.ARM_UP),
# angle=(i * 0.05),
fixtures=UPPER_LIMB_FD)
upper.color1 = (0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
upper.color2 = (0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
rjd = revoluteJointDef(
bodyA=body,
bodyB=upper,
anchor=(init_x, init_y + self.ARM_UP),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.75 * 2 * np.pi,
upperAngle=0,
)
upper.userData = CustomBodyUserData(False, name="upper_arm")
self.body_parts.append(upper)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
lower = world.CreateDynamicBody(
position=(init_x, init_y + self.LIMB_H * 3 / 2 + self.ARM_UP),
# angle=(i * 0.05),
fixtures=LOWER_LIMB_FD)
lower.color1 = (0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
lower.color2 = (0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
rjd = revoluteJointDef(
bodyA=upper,
bodyB=lower,
anchor=(init_x, init_y + self.LIMB_H + self.ARM_UP),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=0,
upperAngle=0.75 * np.pi,
)
lower.userData = CustomBodyUserData(False, name="lower_arm")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
# hand
prev_part = lower
initial_y = init_y + self.LIMB_H * 2 + self.ARM_UP
angle_boundaries = [[-0.5, 0.5]]
nb_hand_parts = 1
for u in range(nb_hand_parts):
hand_part = world.CreateDynamicBody(
position=(init_x, initial_y + self.HAND_PART_H / 2 +
self.HAND_PART_H * u),
fixtures=HAND_PART_FD)
hand_part.color1 = (0.6 - j / 10., 0.3 - j / 10.,
0.5 - j / 10.)
hand_part.color2 = (0.4 - j / 10., 0.2 - j / 10.,
0.3 - j / 10.)
rjd = revoluteJointDef(
bodyA=prev_part,
bodyB=hand_part,
anchor=(init_x, initial_y + self.HAND_PART_H * u),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=angle_boundaries[u][0] * np.pi,
upperAngle=angle_boundaries[u][1] * np.pi,
)
hand_part.userData = CustomBodyUserData(True, name="hand")
self.body_parts.append(hand_part)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(
SPEED_HAND, True, contact_body=hand_part)
self.motors.append(joint_motor)
prev_part = hand_part
hand_sensor_position = (init_x, initial_y +
self.HAND_PART_H * nb_hand_parts)
hand_sensor_part = world.CreateDynamicBody(
position=hand_sensor_position,
fixtures=self.SENSOR_FD,
userData=CustomBodySensorUserData(True, False, "hand_sensor"))
hand_sensor_part.color1 = (
1, 0, 0) #(0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
hand_sensor_part.color2 = (
1, 0, 0) #(0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
self.sensors.append(hand_sensor_part)
world.CreateJoint(
weldJointDef(bodyA=prev_part,
bodyB=hand_sensor_part,
anchor=hand_sensor_position))
def box2d_shapes_from_shapely(geometry):
"""Create Box2D shapes from shapely geometry"""
return [b2.polygonShape(vertices=list(poly.exterior.coords))
for poly in getattr(geometry, "geoms", [geometry])]
pass
def PreSolve(self, contact, oldManifold):
pass
def PostSolve(self, contact, impulse):
pass
# construct world
world = world(gravity=(0, 0),
doSleep=True,
contactListener=myContactListener())
# add static body
env_shape = polygonShape(vertices=metadata)
# env_body = world.CreateStaticBody(position=(0,0), shapes=env_shape)
# add borders (for debugging)
border_shape_h = polygonShape(vertices=[(-0.75, 0), (-0.75,
0.01), (0.75,
0.01), (0.75, 0)])
border_shape_v = polygonShape(vertices=[(0, -0.6), (0.01, -0.6), (0.01,
0.6), (0,
0.6)])
# border_body1 = world.CreateStaticBody(position=(0,0.39), shapes=border_shape_h)
# border_body2 = world.CreateStaticBody(position=(0,-0.4), shapes=border_shape_h)
# border_body3 = world.CreateStaticBody(position=(0.59,0), shapes=border_shape_v)
SCREEN_WIDTH, SCREEN_HEIGHT=640,480
# --- pygame setup ---
screen=pygame.display.set_mode((SCREEN_WIDTH,SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption('Simple pygame example')
clock=pygame.time.Clock()
# --- pybox2d world setup ---
# Create the world
world=b2.world(gravity=(0,-0),doSleep=True)
# And a static body to hold the ground shape
ground_body=world.CreateStaticBody(
position=(0,1),
shapes=b2.polygonShape(box=(50,1)),
)
# Create a dynamic body
box1=world.CreateDynamicBody(position=(10,3+5), angle=0.0)
box2=world.CreateDynamicBody(position=(14,5+5), angle=0.0)
rj=world.CreateRevoluteJoint(
bodyA=box1,
bodyB=box2,
anchor=(12,4+5),
lowerAngle = -.5 * b2.pi, # -90 degrees
upperAngle = .5 * b2.pi, # 45 degrees
enableLimit = True,
maxMotorTorque = -200.0,
]
LEG_DOWN = -8 / SCALE
LEG_W, LEG_H = 8 / SCALE, 34 / SCALE
VIEWPORT_W = 600
VIEWPORT_H = 400
TERRAIN_STEP = 14 / SCALE
TERRAIN_LENGTH = 200 # in steps
TERRAIN_HEIGHT = VIEWPORT_H / SCALE / 4
TERRAIN_GRASS = 10 # low long are grass spots, in steps
TERRAIN_STARTPAD = 20 # in steps
FRICTION = 2.5
HULL_FD = fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in HULL_POLY]),
density=5.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
LEG_FD = fixtureDef(
shape=polygonShape(box=(LEG_W / 2, LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001)
LOWER_FD = fixtureDef(
shape=polygonShape(box=(0.8 * LEG_W / 2, LEG_H / 2)),
PAD_RADIUS = 5
n_balls = -1
frames = 0
pygame.init()
font = pygame.font.SysFont("monospace", 15)
screen = pygame.display.set_mode((WIDTH, HEIGHT), 0, 32)
pygame.display.set_caption("SuperKoolt spel")
clock = pygame.time.Clock()
world = b2d.world(gravity=(0, -GRAVITY), doSleep=True)
world.CreateStaticBody(position=(0, 0),
shapes=b2d.polygonShape(box=(0.01, 100))) #Left wall
world.CreateStaticBody(position=(float(WIDTH / PPM), 0),
shapes=b2d.polygonShape(box=(0.01, 100))) #Right wall
world.CreateStaticBody(position=(0, float(HEIGHT / PPM)),
shapes=b2d.polygonShape(box=(100, 0.01))) #Ceiling
pad_body = world.CreateKinematicBody(position=(WIDTH / (PPM * 2),
0.3 - PAD_RADIUS),
shapes=b2d.circleShape(radius=PAD_RADIUS))
pad_body.mass = 10000
pad_body.fixedRotation = True
pygame.key.set_repeat(10, 10)
balls = []
def reset(self):
if self.scores is None:
self.scores = deque(maxlen=100)
else:
if self.achieve_goal:
self.scores.append(1)
else:
self.scores.append(0)
if self.verbose:
self.verbosing()
self.game_over = False
self.prev_shaping = None
self.achieve_goal = False
self.strike_by_obstacle = False
self.speed_table = np.zeros(len(OBSTACLE_POSITIONS))
# timer
self.energy = 1
# clean up objects in the Box 2D world
self._destroy()
# create lidar objects
self.lidar = [LidarCallback() for _ in range(self.num_beams)]
self.world.contactListener_keepref = ContactDetector(self)
self.world.contactListener = self.world.contactListener_keepref
# create new world
self.moon = self.world.CreateStaticBody(shapes=edgeShape(
vertices=[(0, 0), (W, 0)]))
p1 = (1, 1)
p2 = (W - 1, 1)
self.moon.CreateEdgeFixture(
vertices=[p1, p2],
density=100,
friction=0,
restitution=1.0,
)
self._build_wall()
self.moon.color1 = (0.0, 0.0, 0.0)
self.moon.color2 = (0.0, 0.0, 0.0)
# create obstacles
self._build_obstacles()
# create controller object
while True:
drone_pos = (int(np.random.randint(1, 10)),
int(np.random.randint(1, 5)))
self.drone = self.world.CreateDynamicBody(
position=drone_pos,
angle=0.0,
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in DRONE_POLY]),
density=5.0,
friction=0.1,
categoryBits=0x0010,
maskBits=0x003, # collide all but obs range object
restitution=0.0) # 0.99 bouncy
)
self.drone.color1 = (0.5, 0.4, 0.9)
self.drone.color2 = (0.3, 0.3, 0.5)
self.world.Step(1.0 / FPS, 6 * 30, 2 * 30)
if self.game_over:
self.world.DestroyBody(self.drone)
self.game_over = False
else:
break
# create goal
goal_pos = GOAL_POS[np.random.randint(len(GOAL_POS))]
self.goal = self.world.CreateStaticBody(
position=goal_pos,
angle=0.0,
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in DRONE_POLY]),
density=5.0,
friction=0.1,
categoryBits=0x002,
maskBits=0x0010, # collide only with control device
restitution=0.0) # 0.99 bouncy
)
self.goal.color1 = (0., 0.5, 0)
self.goal.color2 = (0., 0.5, 0)
self.obs_range_plt = self.world.CreateKinematicBody(
position=(self.drone.position[0], self.drone.position[1]),
angle=0.0,
fixtures=fixtureDef(
shape=circleShape(radius=np.float64(self.max_obs_range),
pos=(0, 0)),
density=0,
friction=0,
categoryBits=0x0100,
maskBits=0x000, # collide with nothing
restitution=0.3))
self.obs_range_plt.color1 = (0.2, 0.2, 0.4)
self.obs_range_plt.color2 = (0.6, 0.6, 0.6)
self.drawlist = [self.obs_range_plt, self.drone, self.goal
] + self.walls + self.obstacles
self._observe_lidar(drone_pos)
self.world.Step(1.0 / FPS, 6 * 30, 2 * 30)
return np.copy(self.array_observation())
def _reset(self):
self._destroy()
self.world.contactListener_bug_workaround = ContactDetector(self)
self.world.contactListener = self.world.contactListener_bug_workaround
self.game_over = False
self.prev_shaping = None
self.scroll = 0.0
self.lidar_render = 0
# Create world
self._generate_terrain(hardcore=False)
self._generate_clouds()
# Create body
init_x = TERRAIN_STEP * TERRAIN_STARTPAD / 2
init_y = TERRAIN_HEIGHT + 2 * LEG_H
self.hull = self.world.CreateDynamicBody(
position=(init_x, init_y),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in HULL_POLY]),
density=1.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001,
restitution=0.0))
self.hull.color1 = (0.3, 0.4, 0.9)
self.hull.color2 = (0.3, 0.4, 0.7)
# Create legs
self.legs = []
self.joints = []
for i in [-1, 1]:
# Upper leg
leg = self.world.CreateDynamicBody(
position=(init_x, init_y - LEG_H / 2 - LEG_DOWN),
angle=(i * 0.01),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in LEG_POLY]),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001))
leg.color1 = (0.3 - i / 10, 0.4 - i / 10, 0.8 - i / 10)
leg.color2 = (0.3, 0.4, 0.5)
rjd = revoluteJointDef(
bodyA=self.hull,
bodyB=leg,
localAnchorA=(0, LEG_DOWN),
localAnchorB=(0, LEG_H / 2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed=i,
lowerAngle=-1.1,
upperAngle=1.3,
)
self.legs.append(leg)
self.joints.append(self.world.CreateJoint(rjd))
# Lower leg
lower = self.world.CreateDynamicBody(
position=(init_x, init_y - LEG_H * 3 / 2 - LEG_DOWN),
angle=(i * 0.01),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in LOWERLEG_POLY]),
density=1.0,
restitution=0.00,
categoryBits=0x0020,
maskBits=0x001))
lower.color1 = (0.3 - i / 10., 0.4 - i / 10., 0.8 - i / 10.)
lower.color2 = (0.3 - i / 10., 0.4 - i / 10., 0.5 - i / 10.)
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
localAnchorA=(0, -LEG_H / 2),
localAnchorB=(0, LEG_H / 2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-2.8,
upperAngle=0.2,
)
lower.ground_contact = False
self.legs.append(lower)
self.joints.append(self.world.CreateJoint(rjd))
# Feet
foot = self.world.CreateDynamicBody(
position=(init_x, init_y),
angle=0,
fixtures=fixtureDef(shape=polygonShape(box=(LEG_H, LEG_W)),
density=1.0,
restitution=0.05,
categoryBits=0x0020,
maskBits=0x001))
foot.color1 = (0.3 - i / 10., 0.3 - i / 10., 0.8 - i / 10.)
foot.color2 = (0.3 - i / 10., 0.3 - i / 10., 0.8 - i / 10.)
rjd = revoluteJointDef(
bodyA=lower,
bodyB=foot,
localAnchorA=(0, -LEG_H / 2),
localAnchorB=(-0.05, 0),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-1.0,
upperAngle=1.0,
)
foot.ground_contact = False
self.legs.append(foot)
self.joints.append(self.world.CreateJoint(rjd))
# We render this
self.drawlist = self.terrain + self.legs + [self.hull]
# Lidar
class LidarCallback(Box2D.b2.rayCastCallback):
def ReportFixture(self, fixture, point, normal, fraction):
if (fixture.filterData.categoryBits & 1) == 0:
return 1
self.p2 = point
self.fraction = fraction
return 0
self.lidar = [LidarCallback() for _ in range(10)]
# Start
return self._step(np.array([0, 0, 0, 0, 0, 0]))[0]
def _reset(self):
self.trajectory = []
self.actions = []
self.curr_step = 0
self.curr_obs = None
self.fan = False
self.fan_confs = []
self.at_site = [False for _ in range(LANDED_SLACK)]
self.grounded = [False for _ in range(LANDED_SLACK)]
self._destroy()
self.world.contactListener_keepref = ContactDetector(self)
self.world.contactListener = self.world.contactListener_keepref
self.game_over = False
self.prev_shaping = None
W = VIEWPORT_W / SCALE
H = VIEWPORT_H / SCALE
# terrain
CHUNKS = 11
height = self.np_random.uniform(0, H / 2, size=(CHUNKS + 1, ))
chunk_x = [W / (CHUNKS - 1) * i for i in range(CHUNKS)]
#helipad_chunk = CHUNKS // 2
# randomize helipad x coord
helipad_chunk = self.goal if self.goal is not None else np.random.choice(
range(1, CHUNKS - 1))
self.helipad_x1 = chunk_x[helipad_chunk - 1]
self.helipad_x2 = chunk_x[helipad_chunk + 1]
self.helipad_y = H / 4
height[helipad_chunk - 2] = self.helipad_y
height[helipad_chunk - 1] = self.helipad_y
height[helipad_chunk + 0] = self.helipad_y
height[helipad_chunk + 1] = self.helipad_y
height[helipad_chunk + 2] = self.helipad_y
smooth_y = [
0.33 * (height[i - 1] + height[i + 0] + height[i + 1])
for i in range(CHUNKS)
]
self.moon = self.world.CreateStaticBody(shapes=edgeShape(
vertices=[(0, 0), (W, 0)]))
self.sky_polys = []
for i in range(CHUNKS - 1):
p1 = (chunk_x[i], smooth_y[i])
p2 = (chunk_x[i + 1], smooth_y[i + 1])
self.moon.CreateEdgeFixture(vertices=[p1, p2],
density=0,
friction=0.1)
self.sky_polys.append([p1, p2, (p2[0], H), (p1[0], H)])
self.moon.color1 = (0.0, 0.0, 0.0)
self.moon.color2 = (0.0, 0.0, 0.0)
initial_y = VIEWPORT_H / SCALE #*0.75
self.lander = self.world.CreateDynamicBody(
position=(VIEWPORT_W / SCALE / 2, initial_y),
angle=0.0,
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in LANDER_POLY]),
density=5.0,
friction=0.1,
categoryBits=0x0010,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
self.lander.color1 = (0.5, 0.4, 0.9)
self.lander.color2 = (0.3, 0.3, 0.5)
self.lander.ApplyForceToCenter(
(self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM),
self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM)), True)
self.legs = []
for i in [-1, +1]:
leg = self.world.CreateDynamicBody(
position=(VIEWPORT_W / SCALE / 2 - i * LEG_AWAY / SCALE,
initial_y),
angle=(i * 0.05),
fixtures=fixtureDef(shape=polygonShape(box=(LEG_W / SCALE,
LEG_H / SCALE)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001))
leg.ground_contact = False
leg.color1 = (0.5, 0.4, 0.9)
leg.color2 = (0.3, 0.3, 0.5)
rjd = revoluteJointDef(
bodyA=self.lander,
bodyB=leg,
localAnchorA=(0, 0),
localAnchorB=(i * LEG_AWAY / SCALE, LEG_DOWN / SCALE),
enableMotor=True,
enableLimit=True,
maxMotorTorque=LEG_SPRING_TORQUE,
motorSpeed=+0.3 * i # low enough not to jump back into the sky
)
if i == -1:
rjd.lowerAngle = +0.9 - 0.5 # Yes, the most esoteric numbers here, angles legs have freedom to travel within
rjd.upperAngle = +0.9
else:
rjd.lowerAngle = -0.9
rjd.upperAngle = -0.9 + 0.5
leg.joint = self.world.CreateJoint(rjd)
self.legs.append(leg)
self.drawlist = [self.lander] + self.legs
return self._step(np.array([0, 0]) if self.continuous else 0)[0]
def _reset(self):
self._destroy()
self.world.contactListener = ContactDetector(self)
self.game_over = False
self.prev_shaping = None
self.scroll = 0.0
self.lidar_render = 0
W = VIEWPORT_W/SCALE
H = VIEWPORT_H/SCALE
self._generate_terrain(self.hardcore)
self._generate_clouds()
init_x = TERRAIN_STEP*TERRAIN_STARTPAD/2
init_y = TERRAIN_HEIGHT+2*LEG_H
self.hull = self.world.CreateDynamicBody(
position = (init_x, init_y),
fixtures = fixtureDef(
shape=polygonShape(vertices=[ (x/SCALE,y/SCALE) for x,y in HULL_POLY ]),
density=5.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
self.hull.color1 = (0.5,0.4,0.9)
self.hull.color2 = (0.3,0.3,0.5)
self.hull.ApplyForceToCenter((np.random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)
self.legs = []
self.joints = []
for i in [-1,+1]:
leg = self.world.CreateDynamicBody(
position = (init_x, init_y - LEG_H/2 - LEG_DOWN),
angle = (i*0.05),
fixtures = fixtureDef(
shape=polygonShape(box=(LEG_W/2, LEG_H/2)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001)
)
leg.color1 = (0.6-i/10., 0.3-i/10., 0.5-i/10.)
leg.color2 = (0.4-i/10., 0.2-i/10., 0.3-i/10.)
rjd = revoluteJointDef(
bodyA=self.hull,
bodyB=leg,
localAnchorA=(0, LEG_DOWN),
localAnchorB=(0, LEG_H/2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed = i,
lowerAngle = -0.8,
upperAngle = 1.1,
)
self.legs.append(leg)
self.joints.append(self.world.CreateJoint(rjd))
lower = self.world.CreateDynamicBody(
position = (init_x, init_y - LEG_H*3/2 - LEG_DOWN),
angle = (i*0.05),
fixtures = fixtureDef(
shape=polygonShape(box=(0.8*LEG_W/2, LEG_H/2)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001)
)
lower.color1 = (0.6-i/10., 0.3-i/10., 0.5-i/10.)
lower.color2 = (0.4-i/10., 0.2-i/10., 0.3-i/10.)
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
localAnchorA=(0, -LEG_H/2),
localAnchorB=(0, LEG_H/2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed = 1,
lowerAngle = -1.6,
upperAngle = -0.1,
)
lower.ground_contact = False
self.legs.append(lower)
self.joints.append(self.world.CreateJoint(rjd))
self.drawlist = self.terrain + self.legs + [self.hull]
class LidarCallback(Box2D.b2.rayCastCallback):
def ReportFixture(self, fixture, point, normal, fraction):
if (fixture.filterData.categoryBits & 1) == 0:
return 1
self.p2 = point
self.fraction = fraction
return 0
self.lidar = [LidarCallback() for _ in range(10)]
return self._step(np.array([0,0,0,0]))[0]
def _create_track(self):
# Offset parameters to get the track correctly on the playfield
self.off_params = (-WINDOW_H//4, -WINDOW_W//2.5)
# Construct a lattice using a random node initialization followed
# by a random edge deletion, ensuring everything is connected and
# lattice constraints are met
if "lattice" not in self.__dict__:
self.lattice = construct_lattice(GRID_ROWS, GRID_COLS, PROB_EDGE,
LATTICE_CONSTRAINTS, RANDOM_DELETIONS, self.pre_provided_lattice, self.delete_edges)
# Create polygons for the lattice road pieces and lane separators
# Also store directions for each of the road piece
if "track" not in self.__dict__:
self.normal_polygons, self.special_polygons, self.ls_polygons, self.directions, \
self.relevant_nodes, self.special_relevant_nodes = \
construct_grid(self.lattice, LANE_WIDTH, EDGE_WIDTH, self.off_params, LANE_SEP)
self.track = self.normal_polygons+self.special_polygons
self.directions += "n"*len(self.special_polygons) # Each special_polygon is junction
self.relevant_nodes += self.special_relevant_nodes
assert(len(self.track) == len(self.directions) == len(self.relevant_nodes))
# Start with a blank list of road objects
self.road = []
# Draw track with varying shade so road pieces can be differentiated
i = 0
for polygon in self.track:
# Create static body
t = self.world.CreateStaticBody(fixtures = fixtureDef(
shape=polygonShape(vertices=list(polygon))
))
t.userData = t
# Assign direction
t.direction = self.directions[i]
# Assign colors
# USEFUL: Change colors when debugging directions!
c = 0.01*(i%3)
i += 1
t.color = [ROAD_COLOR[0] + c, ROAD_COLOR[1] + c, ROAD_COLOR[2] + c]
# Assign properties and set contact true
t.boxtype = "road"
t.road_friction = 1.0
t.fixtures[0].sensor = True
# Add to list for rendering, and keep track by adding to self.road
self.road_poly.append(( list(polygon), t.color ))
self.road.append(t)
# Which vertices are in the lattice?
h, w = self.lattice.shape[:2]
self.which_points = []
self.neighbors = []
for i in range(h):
for j in range(w):
if self.lattice[i, j, 0]:
self.which_points += [(i, j)]
self.neighbors += [self.lattice[i, j, 1:]]
# Construct traffic lights
self.lights = []
for neighbor in self.neighbors:
self.lights += [construct_traffic_lights(neighbor, LANE_WIDTH, TRAFFIC_LIGHT_R, TRAFFIC_LIGHT_R2)]
return True
# --- pybox2d world setup ---
# Create the world
world = b2.world(gravity=(0, -10), doSleep=True)
# --- pybox2d static body setup ---
bodyDef = Box2D.b2BodyDef()
bodyDef.position = (22.5, 12) # the center of object
body = world.CreateBody(
bodyDef) # The default bodies are static. # The default density is zero.
groundBody = Box2D.b2PolygonShape(box=(1, 7))
groundBoxFixture = Box2D.b2FixtureDef(shape=groundBody)
body.CreateFixture(groundBoxFixture)
# above is equal in this：
ground_body = world.CreateStaticBody(
position=(3, 1),
shapes=b2.polygonShape(box=(10, 1)),
)
# circle shape
body1 = world.CreateStaticBody(position=(30, 12))
circle = body1.CreateCircleFixture(radius=0.1, density=1, friction=0.3)
# other shape:create by offering vertices
body = world.CreateStaticBody(position=(30, 12), angle=math.pi * 90 / 180)
box = body.CreatePolygonFixture(vertices=[(1, 1), (2, 3), (1, 0), (0, 2)],
density=1,
friction=0.3,
restitution=0.9)
# --- pygame draw function setup ---
colors = {
b2.staticBody: (255, 255, 255), # white
def box2d_triangles_from_shapely(geometry):
"""Create Box2D shapes by triangulating shapely geometry"""
for face in triangulate(geometry):
yield b2.polygonShape(vertices=list(face.exterior.coords))
def _reset(self):
self._destroy()
init_x = self.init_x
init_y = self.init_y
self.hull = self.world.CreateDynamicBody(
position=(init_x, init_y),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in HULL_POLY]),
density=5.0,
friction=0.1,
categoryBits=0x002,
# maskBits=(0x001 & 0x002), # collide only with ground
restitution=0.0) # 0.99 bouncy
)
self.hull.color1 = (0.5, 0.4, 0.9)
self.hull.color2 = (0.3, 0.3, 0.5)
self.hull.ApplyForceToCenter(
(self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)
self.legs = []
self.joints = []
for i in [-1, +1]:
leg = self.world.CreateDynamicBody(
position=(init_x, init_y - LEG_H / 2 - LEG_DOWN),
angle=(i * 0.05),
fixtures=fixtureDef(shape=polygonShape(box=(LEG_W / 2,
LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x002,
maskBits=0x001) # collide with ground only
)
leg.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
leg.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=self.hull,
bodyB=leg,
localAnchorA=(0, LEG_DOWN),
localAnchorB=(0, LEG_H / 2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed=i,
lowerAngle=-0.8,
upperAngle=1.1,
)
self.legs.append(leg)
self.joints.append(self.world.CreateJoint(rjd))
lower = self.world.CreateDynamicBody(
position=(init_x, init_y - LEG_H * 3 / 2 - LEG_DOWN),
angle=(i * 0.05),
fixtures=fixtureDef(shape=polygonShape(box=(0.8 * LEG_W / 2,
LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001))
lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
localAnchorA=(0, -LEG_H / 2),
localAnchorB=(0, LEG_H / 2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-1.6,
upperAngle=-0.1,
)
lower.ground_contact = False
self.legs.append(lower)
self.joints.append(self.world.CreateJoint(rjd))
self.drawlist = self.legs + [self.hull]
class LidarCallback(Box2D.b2.rayCastCallback):
def ReportFixture(self, fixture, point, normal, fraction):
if (fixture.filterData.categoryBits & 1) == 0:
return 1
self.p2 = point
self.fraction = fraction
return 0
self.lidar = [LidarCallback() for _ in range(10)]
class World():
def create_world_grid():
##---Create dictionary to store world cells---##
my_world = {}
key = 0
for x in range(600):
my_world[key] = [-100, -100]
key += 1
##---Store x and y coordinates for each cell---##
key = 0
for y in xrange(1, 40, 2):
for x in xrange(1, 60, 2):
my_world[key] = [x, y]
key += 1
return my_world
def create_graph():
##---BUILD A FULLY CONNECTED GRAPH OF THE WORLD: AT THE BEGINNING EVERY CELL IS---##
##---CONSIDERED TO BE FREE---##
my_graph = {}
for key in range(600):
my_graph[key] = {
key + 30: 1,
key + 31: 1.4,
key + 1: 1,
key - 29: 1.4,
key - 30: 1,
key - 31: 1.4,
key - 1: 1,
key + 29: 1.4
}
##---Define list of cells for each side (corner cells not included!)---##
left_side = [
30, 60, 90, 120, 150, 180, 210, 240, 270, 300, 330, 360, 390, 420,
450, 480, 510, 540
]
right_side = [
59, 89, 119, 149, 179, 209, 239, 269, 299, 329, 359, 389, 419, 449,
479, 509, 539, 569
]
bottom_side = [
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19,
20, 21, 22, 23, 24, 25, 26, 27, 28
]
top_side = [
571, 572, 573, 574, 575, 576, 577, 578, 579, 580, 581, 582, 583,
584, 585, 586, 587, 588, 589, 590, 591, 592, 593, 594, 595, 596,
597, 598
]
for key in range(600):
##---Set connected cells of side cells (corner cells not included)---##
if (key in left_side):
my_graph[key] = {
key + 30: 1,
key + 31: 1.4,
key + 1: 1,
key - 29: 1.4,
key - 30: 1
}
if (key in right_side):
my_graph[key] = {
key + 30: 1,
key - 30: 1,
key - 31: 1.4,
key - 1: 1,
key + 29: 1.4
}
if (key in bottom_side):
my_graph[key] = {
key + 30: 1,
key + 31: 1.4,
key + 1: 1,
key - 1: 1,
key + 29: 1.4
}
if (key in top_side):
my_graph[key] = {
key + 1: 1,
key - 29: 1.4,
key - 30: 1,
key - 31: 1.4,
key - 1: 1
}
##---Set connections of corner cells---##
if (key == 0):
my_graph[key] = {key + 30: 1, key + 31: 1.4, key + 1: 1}
if (key == 29):
my_graph[key] = {key + 30: 1, key - 1: 1, key + 29: 1.4}
if (key == 570):
my_graph[key] = {key + 1: 1, key - 29: 1.4, key - 30: 1}
if (key == 599):
my_graph[key] = {key - 30: 1, key - 31: 1.4, key - 1: 1}
return my_graph
##---Constants---##
PPM = 20.0 # pixels per meter
TARGET_FPS = 60
TIME_STEP = 1.0 / TARGET_FPS
vel_iters, pos_iters = 6, 2
SCREEN_WIDTH, SCREEN_HEIGHT = 1200, 800
##---pygame setup---##
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption('Path finding: robot body of any shape')
clock = pygame.time.Clock()
start_time = datetime.now()
##---Create the world---##
my_world = world(gravity=(0, 0), doSleep=True)
# ##---Static obstacles of the environment---##
# wall_bottom = my_world.CreateStaticBody(position=(30, 0.1), shapes=polygonShape(box=(30, 0.1)), )
# wall_top = my_world.CreateStaticBody(position=(30, 40), shapes=polygonShape(box=(30, 0.1)), )
# wall_left = my_world.CreateStaticBody(position=(0, 20), shapes=polygonShape(box=(20, 0.1)), angle=math.pi/2)
# wall_right = my_world.CreateStaticBody(position=(60, 20), shapes=polygonShape(box=(20, 0.1)), angle=math.pi/2)
# wall_bottom.userData = {'color': 'obstacle'}
# wall_top.userData = {'color': 'obstacle'}
# wall_left.userData = {'color': 'obstacle'}
# wall_right.userData = {'color': 'obstacle'}
obstacle_1 = my_world.CreateStaticBody(position=(5, 12.5),
shapes=polygonShape(box=(5, 2.5)))
obstacle_2 = my_world.CreateStaticBody(position=(40, 25),
shapes=polygonShape(box=(6, 2)),
angle=math.pi / 4)
obstacle_3 = my_world.CreateStaticBody(position=(30, 25),
shapes=polygonShape(box=(6, 2)),
angle=-math.pi / 4)
obstacle_1.userData = {'color': 'obstacle'}
obstacle_2.userData = {'color': 'obstacle'}
obstacle_3.userData = {'color': 'obstacle'}
# ##---TO BE USED TO TEST AND COMPARE DIJKSTRA'S AND A* ALGORITHMS---##
# obstacle_1 = my_world.CreateStaticBody(position=(12, 12), shapes=polygonShape(box=(8, 0.5)), angle=-math.pi/4)
# obstacle_1.userData = {'color': 'obstacle'}
# obstacle_2 = my_world.CreateStaticBody(position=(24, 8), shapes=polygonShape(box=(8, 0.5)), angle=-math.pi/4)
# obstacle_2.userData = {'color': 'obstacle'}
# obstacle_3 = my_world.CreateStaticBody(position=(18, 20), shapes=polygonShape(box=(8, 0.5)), angle=-math.pi/4)
# obstacle_3.userData = {'color': 'obstacle'}
# obstacle_4 = my_world.CreateStaticBody(position=(22, 31), shapes=polygonShape(box=(14, 1.0)), angle=-math.pi/4)
# obstacle_4.userData = {'color': 'obstacle'}
# obstacle_5 = my_world.CreateStaticBody(position=(37, 25), shapes=polygonShape(box=(12, 1.0)), angle=-math.pi/2)
# obstacle_5.userData = {'color': 'obstacle'}
# obstacle_6 = my_world.CreateStaticBody(position=(44, 14), shapes=polygonShape(box=(6, 3.0)), angle=-math.pi/4)
# obstacle_6.userData = {'color': 'obstacle'}
# obstacle_7 = my_world.CreateStaticBody(position=(54, 26), shapes=polygonShape(box=(10, 1.0)))
# obstacle_7.userData = {'color': 'obstacle'}
##---Data structures---##
my_grid = create_world_grid()
my_graph = create_graph()
##---Check if cells are free or occupied---##
key = 0
occupied_counter = 0
occupied_list = []
free_cell_list = []
for y in xrange(1, 40, 2):
for x in xrange(1, 60, 2):
start_position = (x, y)
##---Create robot---##
body = my_world.CreateDynamicBody(position=start_position)
box = body.CreatePolygonFixture(box=(0.98, 1.98),
density=1,
friction=0.3)
free_cell_list.append(key)
for i in range(3):
##---Instruct the world to perform a single step of simulation---##
my_world.Step(TIME_STEP, vel_iters, pos_iters)
##---If the position of the robot changes, the cell must be occupied---##
if (start_position != body.position):
occupied_counter += 1
occupied_list.append(key)
##---Add some fictional high values for occupied cell x and y coordinates---##
my_grid[key] = [1000, 1000]
del my_graph[key]
##---Destroy robot body---##
free_cell_list.remove(key)
my_world.DestroyBody(body)
key += 1
print("occupied_counter: %d" % occupied_counter)
##---count number of cells in the graph---##
counter = 0
for key, value in my_graph.items():
counter += 1
print("Number of free cells in graph: %d" % counter)
print("occupied cells: %s" % occupied_list)
##---Count edges in graph so far---##
edge_count = 0
for key, value in my_graph.items():
for x in value:
edge_count += 1
print("edgecount: %d" % edge_count)
##---Delete edges which are connected to occupied cells---##
for number in occupied_list:
for key, value in my_graph.items():
for x, y in value.items():
if (x == number):
del value[x]
##---Delete diagonal edges at the corners of occupied cells---##
##---Those edges which connect two free cell diagonally but there is an occupied cells next to them---##
for number in occupied_list:
##---Delete edges at the top-right corner of the occupied cell---##
if (number + 1 in my_graph.keys() and number + 30 in my_graph.keys()):
if (number + 30 in my_graph[number + 1].keys()):
del my_graph[number + 1][number + 30]
if (number + 1 in my_graph[number + 30].keys()):
del my_graph[number + 30][number + 1]
##---Delete edges at the top-left corner of the occupied cell---##
if (number - 1 in my_graph.keys() and number + 30 in my_graph.keys()):
if (number + 30 in my_graph[number - 1].keys()):
del my_graph[number - 1][number + 30]
if (number - 1 in my_graph[number + 30].keys()):
del my_graph[number + 30][number - 1]
##---Delete edges at the bottom-left corner of the occupied cell---##
if (number - 1 in my_graph.keys() and number - 30 in my_graph.keys()):
if (number - 30 in my_graph[number - 1].keys()):
del my_graph[number - 1][number - 30]
if (number - 1 in my_graph[number - 30].keys()):
del my_graph[number - 30][number - 1]
##---Delete edges at the bottom-right corner of the occupied cell---##
if (number + 1 in my_graph.keys() and number - 30 in my_graph.keys()):
if (number - 30 in my_graph[number + 1].keys()):
del my_graph[number + 1][number - 30]
if (number + 1 in my_graph[number - 30].keys()):
del my_graph[number - 30][number + 1]
##---Turn the robot counterclockwise and check for collision at every 9 degrees---##
start_key = 0
for y in xrange(1, 40, 2):
for x in xrange(1, 60, 2):
start_position3 = (x, y)
##---40 iterations, 9 degrees/iteration = 360 degrees checked---##
for i in xrange(0, 39, 1):
body3 = my_world.CreateDynamicBody(
position=start_position3,
angle=i * math.pi / 20) ##Step size: 9 degrees (180/20)
box = body3.CreatePolygonFixture(box=(0.98, 1.98),
density=1,
friction=0.3)
for j in range(3):
##---Instruct the world to perform a single step of simulation---##
my_world.Step(TIME_STEP, vel_iters, pos_iters)
##---If the robot body was in collision with any object of the environment---##
if (start_position3 != body3.position):
##---Check how much the robot was rotated when the collision happened---##
##---And delete all the edges which come after this rotation---##
if (i < 35): # 35*9=315 degrees
for key, value in my_graph.items():
if (key == start_key):
for inner_key, inner_value in value.items():
if (key + 31 == inner_key):
del value[key + 31]
if (i < 30): # 30*9=270 degrees
for key, value in my_graph.items():
if (key == start_key):
for inner_key, inner_value in value.items():
if (key + 1 == inner_key):
del value[key + 1]
if (i < 25): # 25*9=225 degrees
for key, value in my_graph.items():
if (key == start_key):
for inner_key, inner_value in value.items():
if (key - 29 == inner_key):
del value[key - 29]
if (i < 20): # 20*9=180 degrees
for key, value in my_graph.items():
if (key == start_key):
for inner_key, inner_value in value.items():
if (key - 30 == inner_key):
del value[key - 30]
if (i < 15): # 15*9=135 degrees
for key, value in my_graph.items():
if (key == start_key):
for inner_key, inner_value in value.items():
if (key - 31 == inner_key):
del value[key - 31]
if (i < 10): # 10*9=00 degrees
for key, value in my_graph.items():
if (key == start_key):
for inner_key, inner_value in value.items():
if (key - 1 == inner_key):
del value[key - 1]
if (i < 5): # 5*9=45 degrees
for key, value in my_graph.items():
if (key == start_key):
for inner_key, inner_value in value.items():
if (key + 29 == inner_key):
del value[key + 29]
my_world.DestroyBody(body3)
start_key += 1
##---count edges again---##
new_edge_count = 0
for key, value in my_graph.items():
for x in value:
new_edge_count += 1
print("new edgecount: %d" % new_edge_count)
##---Set start and goal node numbers---##
start_number = 31
goal_number = 437
##---Use Dijkstra's algorithm to find shortest path---##
start_time_dijkstra = datetime.now()
shortest_path_1, cost_1, counter_1 = dijkstra(my_graph, start_number,
goal_number, 0)
shortest_path_1.reverse()
print("Shortest path using Dijkstra's algorithm: ")
print(shortest_path_1)
print("cost: %f" % cost_1)
print("Number of nodes expanded: %d" % counter_1)
end_time_dijkstra = datetime.now()
time_needed_dijkstra = end_time_dijkstra - start_time_dijkstra
print("Time needed for Dijkstra's algorithm: %s s." %
str(time_needed_dijkstra))
##---Use A* algorithm to find shortest path---##
start_time_A_star = datetime.now()
shortest_path_2, cost_2, counter_2 = A_star(my_grid, my_graph,
start_number, goal_number, 0)
shortest_path_2.reverse()
print("Shortest path using A* algorithm: ")
print(shortest_path_2)
print("cost: %f" % cost_2)
print("Number of nodes expanded: %d" % counter_2)
end_time_A_star = datetime.now()
time_needed_A_star = end_time_A_star - start_time_A_star
print("Time needed for A* algorithm: %s s." % str(time_needed_A_star))
##---Create static bodies to mark the nodes in the tree with small "dots"---##
for key, value in my_grid.items():
x = value[0]
y = value[1]
body3 = my_world.CreateStaticBody(position=(x, y))
box3 = body3.CreatePolygonFixture(box=(0.098, 0.098), density=1)
box3.sensor = True
body3.userData = {'color': 'node_marker'}
##---Draw small 'dots' along the shortest path---##
for number in shortest_path_1: ##---To be used with Dijkstra's algorithm---##
#for number in shortest_path_2: ##---To be used with the A* algorithm---##
x = my_grid[number][0]
y = my_grid[number][1]
body = my_world.CreateKinematicBody(position=(x, y))
box = body.CreatePolygonFixture(box=(0.098, 0.098),
density=1,
friction=0.3)
box.sensor = True
body.userData = {'color': 'shortest_path'}
##---Define vertices for start and goal positions and for the robot---##
vertices2 = [(-0.5, -0.2), (-0.2, -0.5), (0.2, -0.5), (0.5, -0.2),
(0.5, 0.2), (0.2, 0.5), (-0.2, 0.5), (-0.5, 0.2)]
##---Create static body to mark the start position---##
body1 = my_world.CreateStaticBody(position=(my_grid[start_number][0],
my_grid[start_number][1]))
box1 = body1.CreatePolygonFixture(vertices=vertices2, density=1)
box1.sensor = True
body1.userData = {'color': 'start_position'}
##---Create static body to mark the goal position---##
body2 = my_world.CreateStaticBody(position=(my_grid[goal_number][0],
my_grid[goal_number][1]))
box2 = body2.CreatePolygonFixture(vertices=vertices2, density=1)
box2.sensor = True
body2.userData = {'color': 'goal_position'}
# Create a dynamic body for the robot
dynamic_body = my_world.CreateDynamicBody(
position=(my_grid[start_number][0], my_grid[start_number][1]),
angle=0 * math.pi / 2)
box = dynamic_body.CreatePolygonFixture(box=(0.5, 0.8),
density=1,
friction=0.3)
dynamic_body.userData = {'color': 'robot'}
##---Define colours for robot, obstacles, start- and goal positions, node position markers---##
colors = {
'obstacle': (115, 115, 115),
'robot': (165, 0, 0),
'moveable_obstacle': (77, 166, 255),
'start_position': (255, 140, 26),
'goal_position': (204, 255, 153),
'node_marker': (0, 77, 0),
'shortest_path': (255, 128, 223),
}
##---Calculate and print runtime---##
end_time = datetime.now()
total_runtime = end_time - start_time
print("total_runtime: %s" % (total_runtime))
def draw(self, world, init_x, init_y, force_to_center):
MAIN_BODY_FIXTURES = [
fixtureDef(shape=polygonShape(vertices=[(x / self.SCALE,
y / self.SCALE)
for x, y in polygon]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x000F) for polygon in MAIN_BODY_POLYGONS
]
LEG_FD = fixtureDef(shape=polygonShape(box=(self.LEG_W / 2,
self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * self.LEG_W / 2,
self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
init_x = init_x - MAIN_BODY_BOTTOM_WIDTH / self.SCALE * self.nb_of_bodies / 2 # initial init_x is the middle of the whole body
previous_main_body = None
for j in range(self.nb_of_bodies):
main_body_x = init_x + j * (MAIN_BODY_BOTTOM_WIDTH / self.SCALE)
main_body = world.CreateDynamicBody(position=(main_body_x, init_y),
fixtures=MAIN_BODY_FIXTURES)
main_body.color1 = (0.5, 0.4, 0.9)
main_body.color2 = (0.3, 0.3, 0.5)
main_body.ApplyForceToCenter((force_to_center, 0), True)
# self.body_parts.append(BodyPart(main_body, True, True))
main_body.userData = CustomBodyUserData(True,
is_contact_critical=False,
name="body")
self.body_parts.append(main_body)
for i in [-1, +1]:
leg = world.CreateDynamicBody(
position=(main_body_x,
init_y - self.LEG_H / 2 - self.LEG_DOWN),
#angle=(i * 0.05),
fixtures=LEG_FD)
leg.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
leg.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=main_body,
bodyB=leg,
anchor=(main_body_x, init_y - self.LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=i,
lowerAngle=-0.8,
upperAngle=1.1,
)
leg.userData = CustomBodyUserData(False, name="leg")
self.body_parts.append(leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
lower = world.CreateDynamicBody(
position=(main_body_x,
init_y - self.LEG_H * 3 / 2 - self.LEG_DOWN),
#angle=(i * 0.05),
fixtures=LOWER_FD)
lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
anchor=(main_body_x, init_y - self.LEG_DOWN - self.LEG_H),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-1.6,
upperAngle=-0.1,
)
lower.userData = CustomBodyUserData(True, name="lower")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(
SPEED_KNEE, True, contact_body=lower, angle_correction=1.0)
self.motors.append(joint_motor)
if previous_main_body is not None:
rjd = revoluteJointDef(
bodyA=previous_main_body,
bodyB=main_body,
anchor=(main_body_x -
MAIN_BODY_BOTTOM_WIDTH / self.SCALE / 2, init_y),
enableMotor=False,
enableLimit=True,
lowerAngle=-0.05 * np.pi,
upperAngle=0.05 * np.pi,
)
world.CreateJoint(rjd)
previous_main_body = main_body
self.reference_head_object = previous_main_body # set as head the rightmost body
self.reference_head_object.is_contact_critical = self.reference_head_object
def _create_rocket(self, initial_coordinates=(W / 2, H / 1.2)):
body_color = (1, 1, 1)
# ----------------------------------------------------------------------------------------
# LANDER
initial_x, initial_y = initial_coordinates
self.lander = self.world.CreateDynamicBody(
position=(initial_x, initial_y),
angle=0,
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in LANDER_POLY]),
density=5.0,
friction=0.1,
categoryBits=0x0010,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
self.lander.color1 = body_color
self.lander.color2 = (0, 0, 0)
if isinstance(self.settings['Initial Force'], str):
self.lander.ApplyForceToCenter(
(self.np_random.uniform(-INITIAL_RANDOM * 0.3,
INITIAL_RANDOM * 0.3),
self.np_random.uniform(-1.3 * INITIAL_RANDOM,
-INITIAL_RANDOM)), True)
else:
self.lander.ApplyForceToCenter(self.settings['Initial Force'],
True)
# COG is set in the middle of the polygon by default. x = 0 = middle.
# self.lander.mass = 25
# self.lander.localCenter = (0, 3) # COG
# ----------------------------------------------------------------------------------------
# LEGS
self.legs = []
for i in [-1, +1]:
leg = self.world.CreateDynamicBody(
position=(initial_x - i * LEG_AWAY / SCALE, initial_y),
angle=(i * 0.05),
fixtures=fixtureDef(shape=polygonShape(box=(LEG_W / SCALE,
LEG_H / SCALE)),
density=5.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x005))
leg.ground_contact = False
leg.color1 = body_color
leg.color2 = (0, 0, 0)
rjd = revoluteJointDef(
bodyA=self.lander,
bodyB=leg,
localAnchorA=(-i * 0.3 / LANDER_CONSTANT, 0),
localAnchorB=(i * 0.5 / LANDER_CONSTANT, LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=LEG_SPRING_TORQUE,
motorSpeed=+0.3 * i # low enough not to jump back into the sky
)
if i == -1:
rjd.lowerAngle = 40 * DEGTORAD
rjd.upperAngle = 45 * DEGTORAD
else:
rjd.lowerAngle = -45 * DEGTORAD
rjd.upperAngle = -40 * DEGTORAD
leg.joint = self.world.CreateJoint(rjd)
self.legs.append(leg)
# ----------------------------------------------------------------------------------------
# NOZZLE
self.nozzle = self.world.CreateDynamicBody(
position=(initial_x, initial_y),
angle=0.0,
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in NOZZLE_POLY]),
density=5.0,
friction=0.1,
categoryBits=0x0040,
maskBits=0x003, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
self.nozzle.color1 = (0, 0, 0)
self.nozzle.color2 = (0, 0, 0)
rjd = revoluteJointDef(
bodyA=self.lander,
bodyB=self.nozzle,
localAnchorA=(0, 0),
localAnchorB=(0, 0.2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=NOZZLE_TORQUE,
motorSpeed=0,
referenceAngle=0,
lowerAngle=-13 *
DEGTORAD, # +- 15 degrees limit applied in practice
upperAngle=13 * DEGTORAD)
# The default behaviour of a revolute joint is to rotate without resistance.
self.nozzle.joint = self.world.CreateJoint(rjd)
# ----------------------------------------------------------------------------------------
# self.drawlist = [self.nozzle] + [self.lander] + self.legs
self.drawlist = self.legs + [self.nozzle] + [self.lander]
self.initial_mass = self.lander.mass
self.remaining_fuel = INITIAL_FUEL_MASS_PERCENTAGE * self.initial_mass
return
]
LEG_DOWN = -8/SCALE
LEG_W, LEG_H = 8/SCALE, 34/SCALE
VIEWPORT_W = 600
VIEWPORT_H = 400
TERRAIN_STEP = 14/SCALE
TERRAIN_LENGTH = 200 # in steps
TERRAIN_HEIGHT = VIEWPORT_H/SCALE/4
TERRAIN_GRASS = 10 # low long are grass spots, in steps
TERRAIN_STARTPAD = 20 # in steps
FRICTION = 2.5
HULL_FD = fixtureDef(
shape=polygonShape(box=(28/SCALE/2, 28/SCALE/2)),
density=5.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
NECK1_FD = fixtureDef(
shape=polygonShape(box=(20/SCALE/2, 20/SCALE/2)),
density=5.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
NECK2_FD = fixtureDef(
shape=polygonShape(box=(10/SCALE/2, 15/SCALE/2)),
def _reset(self):
self._destroy()
# self.world.contactListener_bug_workaround = ContactDetector(self)
# self.world.contactListener = self.world.contactListener_bug_workaround
self.world.contactListener_keepref = ContactDetector(self)
self.world.contactListener = self.world.contactListener_keepref
self.game_over = False
self.prev_shaping = None
self.scroll = 0.0
self.lidar_render = 0
W = VIEWPORT_W / SCALE
H = VIEWPORT_H / SCALE
self._generate_terrain(self.hardcore)
self._generate_clouds()
init_x = TERRAIN_STEP * TERRAIN_STARTPAD / 2
init_y = TERRAIN_HEIGHT + 2 * LEG_H
self.hull = self.world.CreateDynamicBody(
position=(init_x, init_y),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in HULL_POLY]),
density=5.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
self.hull.color1 = (0.5, 0.4, 0.9)
self.hull.color2 = (0.3, 0.3, 0.5)
self.hull.ApplyForceToCenter(
(self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)
self.head = self.world.CreateDynamicBody(
position=(init_x - 0 / SCALE, init_y + (HULL_H + 10) / SCALE),
fixtures=fixtureDef(
shape=circleShape(pos=[0, 0], radius=7.0 / SCALE),
density=5.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
self.head.color1 = (1.0, 0.4, 0.9)
self.head.color2 = (0.3, 1.0, 0.5)
#self.head.ApplyForceToCenter((self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)
self.legs = []
self.arms = []
self.joints = []
rjd1 = revoluteJointDef(
bodyA=self.head,
bodyB=self.hull,
localAnchorA=(0, 0),
localAnchorB=(0, +(HULL_H) / SCALE),
# enableMotor=True,
# enableLimit=True,
# maxMotorTorque=MOTORS_TORQUE,
# motorSpeed = -1,
# lowerAngle = -0.8,
# upperAngle = 1.1,
)
for i in [-1, +1]:
leg = self.world.CreateDynamicBody(
position=(init_x, init_y - LEG_H / 2 - LEG_DOWN),
angle=(i * 0.05),
fixtures=fixtureDef(shape=polygonShape(box=(LEG_W / 2,
LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001))
leg.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
leg.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=self.hull,
bodyB=leg,
localAnchorA=(0, LEG_DOWN),
localAnchorB=(0, LEG_H / 2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed=i,
lowerAngle=-0.8,
upperAngle=1.1,
)
self.legs.append(leg)
self.joints.append(self.world.CreateJoint(rjd))
lower = self.world.CreateDynamicBody(
position=(init_x, init_y - LEG_H * 3 / 2 - LEG_DOWN),
angle=(i * 0.05),
fixtures=fixtureDef(shape=polygonShape(box=(0.8 * LEG_W / 2,
LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001))
lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
localAnchorA=(0, -LEG_H / 2),
localAnchorB=(0, LEG_H / 2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-1.6,
upperAngle=-0.1,
)
lower.ground_contact = False
self.legs.append(lower)
self.joints.append(self.world.CreateJoint(rjd))
for i in [-1, +1]:
arm = self.world.CreateDynamicBody(
position=(init_x, init_y - arm_H / 2 + (HULL_H) / SCALE),
angle=(i * 0.05),
fixtures=fixtureDef(shape=polygonShape(box=(arm_W / 2,
arm_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001))
arm.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
arm.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=self.hull,
bodyB=arm,
localAnchorA=(0, (HULL_H) / SCALE),
localAnchorB=(0, arm_H / 2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed=-i,
lowerAngle=-0.8,
upperAngle=0.8,
)
self.arms.append(arm)
self.joints.append(self.world.CreateJoint(rjd))
# lower_arm = self.world.CreateDynamicBody(
# position = (init_x, init_y - arm_H*3/2 + (HULL_H)/SCALE),
# angle = (i*0.05),
# fixtures = fixtureDef(
# shape=polygonShape(box=(0.8*arm_W/2, arm_H/2)),
# density=1.0,
# restitution=0.0,
# categoryBits=0x0020,
# maskBits=0x001)
# )
# lower_arm.color1 = (0.6-i/10., 0.3-i/10., 0.5-i/10.)
# lower_arm.color2 = (0.4-i/10., 0.2-i/10., 0.3-i/10.)
# rjd = revoluteJointDef(
# bodyA=arm,
# bodyB=lower_arm,
# localAnchorA=(0, -arm_H/2),
# localAnchorB=(0, arm_H/2),
# enableMotor=True,
# enableLimit=True,
# maxMotorTorque=MOTORS_TORQUE,
# motorSpeed = 1,
# lowerAngle = -1.6,
# upperAngle = -0.1,
# )
# lower_arm.ground_contact = False
# self.arms.append(lower_arm)
# self.joints.append(self.world.CreateJoint(rjd))
self.joints.append(self.world.CreateJoint(rjd1))
self.drawlist = self.terrain + self.legs + self.arms + [self.hull] + [
self.head
]
class LidarCallback(Box2D.b2.rayCastCallback):
def ReportFixture(self, fixture, point, normal, fraction):
if (fixture.filterData.categoryBits & 1) == 0:
return 1
self.p2 = point
self.fraction = fraction
return 0
self.lidar = [LidarCallback() for _ in range(10)]
self.a = np.array([0, 0, 0, 0])
return self._step(0)
def draw(self, world, init_x, init_y, force_to_center):
HULL_FIXTURES = [
fixtureDef(shape=polygonShape(vertices=[(x / self.SCALE,
y / self.SCALE)
for x, y in polygon]),
density=self.DENSITY * 1.25,
friction=0.1,
categoryBits=0x20,
maskBits=0x000F) # 0.99 bouncy
for polygon in HULL_POLYGONS
]
LEG_FD = fixtureDef(shape=polygonShape(box=(self.LEG_W / 2,
self.LEG_H / 2)),
density=self.DENSITY * 0.25,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * self.LEG_W / 2,
self.LEG_H / 2)),
density=self.DENSITY * 0.25,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
hull = world.CreateDynamicBody(position=(init_x, init_y),
fixtures=HULL_FIXTURES)
hull.color1 = (0.44, 0.81, 0.14)
hull.color2 = (0.36, 0.66, 0.11)
hull.ApplyForceToCenter((force_to_center, 0), True)
hull.userData = CustomBodyUserData(True,
is_contact_critical=False,
name="hull")
self.body_parts.append(hull)
self.reference_head_object = hull
for i in [-1, +1]:
leg = world.CreateDynamicBody(
position=(init_x, init_y - self.LEG_H / 2 - self.LEG_DOWN),
#angle=(i * 0.05),#2°
fixtures=LEG_FD)
leg.color1 = (0.44, 0.81, 0.14)
leg.color2 = (0.36, 0.66, 0.11)
rjd = revoluteJointDef(
bodyA=hull,
bodyB=leg,
anchor=(init_x, init_y - self.LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=i,
lowerAngle=-0.8,
upperAngle=1.1,
)
leg.userData = CustomBodyUserData(False, name="leg")
self.body_parts.append(leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
lower = world.CreateDynamicBody(
position=(init_x, init_y - self.LEG_H * 3 / 2 - self.LEG_DOWN),
#angle=(i * 0.05), #2°
fixtures=LOWER_FD)
lower.color1 = (1.0, 0.25, 0.04)
lower.color2 = (0.86, 0.29, 0.12)
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
anchor=(init_x, init_y - self.LEG_DOWN - self.LEG_H),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-1.6,
upperAngle=-0.1,
)
lower.userData = CustomBodyUserData(True, name="lower")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE,
True,
contact_body=lower,
angle_correction=1.0)
self.motors.append(joint_motor)
def add_static_body(self, p, size):
return self.world.CreateStaticBody(position=self.to_pybox2d(p), \
shapes=polygonShape(box=self.size_to_pybox2d(size)))
def _create_decoration():
objs = []
objs.append(
self.world.CreateStaticBody(
position=(W / 2, H / 2),
angle=0.0,
fixtures=fixtureDef(shape=circleShape(radius=100 / SCALE,
pos=(0, 0)),
categoryBits=0x0,
maskBits=0x0)))
objs[-1].color1 = (0.8, 0.8, 0.8)
objs[-1].color2 = (0.8, 0.8, 0.8)
objs.append(
self.world.CreateStaticBody(
position=(W / 2, H / 2),
angle=0.0,
fixtures=fixtureDef(shape=circleShape(radius=100 / SCALE,
pos=(0, 0)),
categoryBits=0x0,
maskBits=0x0)))
objs[-1].color1 = (0.8, 0.8, 0.8)
objs[-1].color2 = (0.8, 0.8, 0.8)
objs.append(
self.world.CreateStaticBody(
position=(W / 2 - 250 / SCALE, H / 2),
angle=0.0,
fixtures=fixtureDef(shape=circleShape(radius=70 / SCALE,
pos=(0, 0)),
categoryBits=0x0,
maskBits=0x0)))
objs[-1].color1 = (255. / 255, 204. / 255, 191. / 255)
objs[-1].color2 = (255. / 255, 204. / 255, 191. / 255)
poly = [(0, 100), (100, 100), (100, -100), (0, -100)]
objs.append(
self.world.CreateStaticBody(
position=(W / 2 - 240 / SCALE, H / 2),
angle=0.0,
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in poly]),
categoryBits=0x0,
maskBits=0x0)))
objs[-1].color1 = (1, 1, 1)
objs[-1].color2 = (1, 1, 1)
objs.append(
self.world.CreateStaticBody(
position=(W / 2 + 250 / SCALE, H / 2),
angle=0.0,
fixtures=fixtureDef(shape=circleShape(radius=70 / SCALE,
pos=(0, 0)),
categoryBits=0x0,
maskBits=0x0)))
objs[-1].color1 = (255. / 255, 204. / 255, 191. / 255)
objs[-1].color2 = (255. / 255, 204. / 255, 191. / 255)
poly = [(100, 100), (0, 100), (0, -100), (100, -100)]
objs.append(
self.world.CreateStaticBody(
position=(W / 2 + 140 / SCALE, H / 2),
angle=0.0,
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in poly]),
categoryBits=0x0,
maskBits=0x0)))
objs[-1].color1 = (1, 1, 1)
objs[-1].color2 = (1, 1, 1)
return objs
def draw(self, world, init_x, init_y, force_to_center):
head = world.CreateDynamicBody(
position=(init_x, init_y + self.BODY_HEIGHT / self.SCALE / 2 + self.HEAD_HEIGHT / self.SCALE / 2 + 0.2),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / self.SCALE, y / self.SCALE) for x, y in [
(-5, +10), (+5, +10),
(+5, -10), (-5, -10)]]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x1
)
)
head.color1 = (0.5, 0.4, 0.9)
head.color2 = (0.3, 0.3, 0.5)
head.ApplyForceToCenter((force_to_center, 0), True)
head.userData = CustomBodyUserData(True, is_contact_critical=self.reset_on_hull_critical_contact, name="head")
self.body_parts.append(head)
self.reference_head_object = head
body = world.CreateDynamicBody(
position=(init_x, init_y),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / self.SCALE, y / self.SCALE) for x, y in [
(-8, +25), (+8, +25),
(+8, -20), (-8, -20)]]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x1 # collide only with ground
)
)
body.color1 = (0.5, 0.4, 0.9)
body.color2 = (0.3, 0.3, 0.5)
body.userData = CustomBodyUserData(True, is_contact_critical=True, name="body")
self.body_parts.append(body)
rjd = revoluteJointDef(
bodyA=head,
bodyB=body,
anchor=(init_x, init_y + self.BODY_HEIGHT / self.SCALE / 2),
enableMotor=False,
enableLimit=True,
lowerAngle=-0.1 * np.pi,
upperAngle=0.1 * np.pi,
)
world.CreateJoint(rjd)
UPPER_LIMB_FD = fixtureDef(
shape=polygonShape(box=(self.LIMB_W / 2, self.LIMB_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x1
)
LOWER_LIMB_FD = fixtureDef(
shape=polygonShape(box=(0.8 * self.LIMB_W / 2, self.LIMB_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x1
)
# LEGS
for i in [+1, +1]:
upper = world.CreateDynamicBody(
position=(init_x, init_y - self.LIMB_H / 2 - self.LEG_DOWN),
# angle=(i * 0.05),
fixtures=UPPER_LIMB_FD
)
upper.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
upper.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=body,
bodyB=upper,
anchor=(init_x, init_y - self.LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.3 * np.pi,
upperAngle=0.6 * np.pi,
)
upper.userData = CustomBodyUserData(False, name="upper_leg")
self.body_parts.append(upper)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
lower = world.CreateDynamicBody(
position=(init_x, init_y - self.LIMB_H * 3 / 2 - self.LEG_DOWN),
# angle=(i * 0.05),
fixtures=LOWER_LIMB_FD
)
lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=upper,
bodyB=lower,
anchor=(init_x, init_y - self.LIMB_H - self.LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.75 * np.pi,
upperAngle=-0.1,
)
lower.userData = CustomBodyUserData(True, name="lower_leg")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE, True, angle_correction=1.0, contact_body=lower)
self.motors.append(joint_motor)
# ARMS
for j in [-1, -1]:
upper = world.CreateDynamicBody(
position=(init_x, init_y - self.LIMB_H / 2 + self.ARM_UP),
# angle=(i * 0.05),
fixtures=UPPER_LIMB_FD
)
upper.color1 = (0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
upper.color2 = (0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
rjd = revoluteJointDef(
bodyA=body,
bodyB=upper,
anchor=(init_x, init_y + self.ARM_UP),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.5 * np.pi,
upperAngle=0.8 * np.pi,
)
upper.userData = CustomBodyUserData(False, name="upper_arm")
self.body_parts.append(upper)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
lower = world.CreateDynamicBody(
position=(init_x, init_y - self.LIMB_H * 3 / 2 + self.ARM_UP),
# angle=(i * 0.05),
fixtures=LOWER_LIMB_FD
)
lower.color1 = (0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
lower.color2 = (0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
rjd = revoluteJointDef(
bodyA=upper,
bodyB=lower,
anchor=(init_x, init_y - self.LIMB_H + self.ARM_UP),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=0,
upperAngle=0.75 * np.pi,
)
lower.userData = CustomBodyUserData(True, name="lowar_arm")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE, True, contact_body=lower)
self.motors.append(joint_motor)
def reset(self):
self.obj_type = 5
self.timer = 0
self.trigger = 'stop'
self.curr_grab = 1000
self.curr_right = 0
self.consecutive_wait = 0
self.dist_from_obj = 0
self.dist_from_pos = 0
self.colour_list = []
self.obj_type = random.randint(1, 5)
self.lift_reward = [0.1] * 10
self.max_height = 200
for i_body in range(len(self.body_list)):
if i_body != 0:
self.world.DestroyBody(self.body_list[i_body])
self.body_list = []
self.ground_body = self.world.CreateStaticBody(
position=(160, 0), shapes=polygonShape(box=(160, 7.5)), angle=0)
self.wall_body1 = self.world.CreateStaticBody(
position=(320, 80), shapes=polygonShape(box=(7.5, 320)), angle=0)
self.wall_body2 = self.world.CreateStaticBody(
position=(0, 80), shapes=polygonShape(box=(7.5, 320)), angle=0)
self.wall_body3 = self.world.CreateStaticBody(
position=(100000, 0), shapes=polygonShape(box=(5, 70)), angle=0)
self.roof = self.world.CreateStaticBody(position=(160, 320),
shapes=polygonShape(box=(160,
7.5)),
angle=0)
pos1 = np.random.uniform(15, 130)
pos2 = np.random.uniform(280, 310)
self.arm = self.world.CreateDynamicBody(position=(pos1, 315),
shapes=polygonShape(box=(6,
12)),
angle=0)
self.grab = self.world.CreateDynamicBody(
position=(pos1, pos2),
shapes=polygonShape(box=(6, 6)),
angle=0,
fixedRotation=True,
)
self.grab.CreatePolygonFixture(box=(6, 6), density=0.1, friction=0)
self.stick = self.world.CreateDynamicBody(
position=(pos1, pos2),
shapes=polygonShape(box=(40, 6)),
angle=0,
)
self.stick.CreatePolygonFixture(box=(40, 6), density=0.1, friction=0)
offset = 2.5
DENSITY_2 = 0.1
FRICTION = 10
if self.obj_type in [2, 3, 5]:
OBJ_2 = [(-40, 30), (-10, 30), (-10, 20), (-40, 20)]
OBJ_3 = [(-40, -20), (-10, -20), (-10, -30), (-40, -30)]
else:
OBJ_2 = [(-40, 30), (-10, 30), (-10, 10), (-40, 10)]
OBJ_3 = [(-40, -10), (-10, -10), (-10, -30), (-40, -30)]
OBJ_1 = [(-10, 30), (10, 30), (10, -30), (-10, -30)]
OBJ_4 = [(10, 10), (40, 10), (40, -10), (10, -10)]
OBJ_5 = [(-40, -5), (-10, -5), (-10, 5), (-40, 5)]
definedFixturesObj_1 = fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in OBJ_1]),
density=DENSITY_2,
friction=FRICTION,
restitution=0.0)
definedFixturesObj_2 = fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in OBJ_2]),
density=DENSITY_2,
friction=FRICTION,
restitution=0.0)
definedFixturesObj_3 = fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in OBJ_3]),
density=DENSITY_2,
friction=FRICTION,
restitution=0.0)
definedFixturesObj_4 = fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in OBJ_4]),
density=DENSITY_2,
friction=FRICTION,
restitution=0.0)
definedFixturesObj_5 = fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in OBJ_5]),
density=DENSITY_2,
friction=FRICTION,
restitution=0.0)
if self.obj_type == 1:
fixturesListObj = [
definedFixturesObj_1, definedFixturesObj_2,
definedFixturesObj_3, definedFixturesObj_4
]
elif self.obj_type == 2:
fixturesListObj = [
definedFixturesObj_1, definedFixturesObj_3,
definedFixturesObj_5
]
elif self.obj_type == 3:
fixturesListObj = [
definedFixturesObj_1, definedFixturesObj_2,
definedFixturesObj_5
]
elif self.obj_type == 4:
fixturesListObj = [
definedFixturesObj_1, definedFixturesObj_2,
definedFixturesObj_3
]
elif self.obj_type == 5:
fixturesListObj = [
definedFixturesObj_1, definedFixturesObj_2,
definedFixturesObj_3, definedFixturesObj_5
]
self.object = self.world.CreateDynamicBody(position=(random.randint(
140, 280), 40),
angle=0,
angularDamping=10,
linearDamping=1,
fixtures=fixturesListObj)
self.object.mass = 1000
self.prev_grab_pos = self.grab.position.copy()
self.h_pj = self.world.CreatePrismaticJoint(
bodyA=self.wall_body2,
bodyB=self.arm,
anchor=(self.wall_body2.worldCenter[0],
self.wall_body2.worldCenter[1]),
axis=(1, 0),
lowerTranslation=-100000,
upperTranslation=1000000,
enableLimit=True,
motorSpeed=0,
maxMotorForce=5000000000000000000000,
enableMotor=True,
)
self.v_pj = self.world.CreatePrismaticJoint(
bodyA=self.arm,
bodyB=self.grab,
anchor=(self.grab.worldCenter[0], self.grab.worldCenter[1]),
axis=(0, 1),
lowerTranslation=-100000,
upperTranslation=0,
enableLimit=True,
motorSpeed=0,
maxMotorForce=5000000000000000000000,
enableMotor=True,
)
self.a_rj = self.world.CreateRevoluteJoint(
bodyA=self.grab,
bodyB=self.stick,
anchor=self.stick.worldCenter,
maxMotorTorque=100000000,
motorSpeed=0,
enableMotor=True,
collideConnected=False,
)
self.colour_list.append((255, 255, 255, 255))
self.colour_list.append((255, 255, 255, 255))
self.colour_list.append((255, 255, 255, 255))
self.colour_list.append((255, 255, 255, 255))
self.colour_list.append((255, 255, 255, 255))
self.colour_list.append((150, 200, 150, 200))
self.colour_list.append((200, 150, 200, 150))
self.colour_list.append((200, 150, 200, 150))
self.body_list = [
self.ground_body, self.wall_body1, self.wall_body2, self.roof,
self.arm, self.grab, self.object, self.stick
]
self.reward_list = [1] * len(self.body_list)
self.update_screen()
self.state1 = get_game_screen(screen)
return self.state1
def _reset(self):
self._destroy()
self.world.contactListener_bug_workaround = ContactDetector(self)
self.world.contactListener = self.world.contactListener_bug_workaround
self.game_over = False
self.prev_shaping = None
self.scroll = 0.0
self.lidar_render = 0
W = VIEWPORT_W/SCALE
H = VIEWPORT_H/SCALE
self._generate_terrain(self.hardcore)
self._generate_clouds()
self.hull = self.world.CreateDynamicBody(
position = (hull_x, hull_y),
fixtures = fixtureDef(
shape=polygonShape(vertices=[ (x/SCALE,y/SCALE) for x,y in HULL_POLY ]),
density=5.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
self.hull.color1 = (0.5,0.4,0.9)
self.hull.color2 = (0.3,0.3,0.5)
#self.hull.ApplyForceToCenter((self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)
#self.hull.ApplyForceToCenter((0, 10*10.0*5.0*(20/SCALE)**2), False)
self.legs = []
self.joints = []
for i in [-1,+1]:
leg = self.world.CreateDynamicBody(
position = (leg_x, leg_y),
angle = gamma_i,
fixtures = fixtureDef(
shape=polygonShape(box=(LEG_W/2, LEG_H/2)),
density=1.0,
restitution=0.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001)
)
leg.color1 = (0.6-i/10., 0.3-i/10., 0.5-i/10.)
leg.color2 = (0.4-i/10., 0.2-i/10., 0.3-i/10.)
rjd = revoluteJointDef(
bodyA=self.hull,
bodyB=leg,
localAnchorA=(0, 0),
localAnchorB=(0, LEG_H/2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed = 0.0,
referenceAngle = 0.0,
lowerAngle = -1.4, # -80 degree
upperAngle = 1.4, # 80 degree
)
self.legs.append(leg)
self.joints.append(self.world.CreateJoint(rjd))
lower = self.world.CreateDynamicBody(
position = (lower_x, lower_y),
angle = (-math.pi/2+a_i),
fixtures = fixtureDef(
shape=polygonShape(box=(LEG_W/2, LEG_H/2)),
density=1.0,
friction=0.1,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001)
)
lower.color1 = (0.6-i/10., 0.3-i/10., 0.5-i/10.)
lower.color2 = (0.4-i/10., 0.2-i/10., 0.3-i/10.)
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
localAnchorA=(0, -LEG_H/2),
localAnchorB=(0, LEG_H/2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed = 0.0,
referenceAngle = -1*math.pi,
lowerAngle = 0.35, # 20 degree
upperAngle = 3.14 # 180 degree
)
lower.ground_contact = False
# foot.ground_contact = False
self.legs.append(lower)
# self.legs.append(foot)
self.joints.append(self.world.CreateJoint(rjd))
# self.joints.append(self.world.CreateJoint(foot_jd))
self.drawlist = self.terrain + self.legs + [self.hull]
class LidarCallback(Box2D.b2.rayCastCallback):
def ReportFixture(self, fixture, point, normal, fraction):
if (fixture.filterData.categoryBits & 1) == 0:
return 1
self.p2 = point
self.fraction = fraction
return 0
self.lidar = [LidarCallback() for _ in range(10)]
#self.hull.ApplyForceToCenter((0, 10.0 * (5.0 * (20 / SCALE) ** 2)), True)
#self.legs[0].ApplyForceToCenter((0, 10.0 * 1.0 * LEG_H * LEG_W), True)
#self.legs[1].ApplyForceToCenter((0, 10.0 * 1.0 * LEG_H * LEG_W), True)
#self.legs[2].ApplyForceToCenter((0, 10.0 * 1.0 * LEG_H * LEG_W), True)
#self.legs[3].ApplyForceToCenter((0, 10.0 * 1.0 * LEG_H * LEG_W), True)
return self._step(np.array([0.0,0.0,0.0,0.0]))[0]
# --- constants ---
# Box2D deals with meters, but we want to display pixels,
# so define a conversion factor:
PPM=20.0 # pixels per meter
TARGET_FPS=30
TIME_STEP=1.0/TARGET_FPS
# --- pybox2d world setup ---
# Create the world
world=world(gravity=(0,-10),doSleep=True)
# And a static body to hold the ground shape
ground_body=world.CreateStaticBody(
position=(0,0),
shapes=polygonShape(box=(70,1)),
)
for idx, poly in enumerate(polys):
print idx
vecs = [vec2(p[0], HT-p[1]) / PPM for p in poly]
mn = sum(vecs, vec2(0,0)) / len(vecs)
if mn[1] > 17:
continue
try:
ps = polygonShape(vertices=[1.02 * (v-mn) for v in vecs])
except:
vecs.reverse()
try:
ps = polygonShape(vertices=[v-mn for v in vecs])
except:
TIME_STEP = 1.0 / TARGET_FPS
SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480
# --- pygame setup ---
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption('Simple pygame example')
clock = pygame.time.Clock()
# --- pybox2d world setup ---
# Create the world
world = world(gravity=(0, -10), doSleep=True)
# And a static body to hold the ground shape
ground_body = world.CreateStaticBody(
position=(0, 0),
shapes=polygonShape(box=(50, 1)),
)
# Create a couple dynamic bodies
body = world.CreateDynamicBody(position=(20, 15))
circle = body.CreateCircleFixture(radius=0.5, density=1, friction=0.3)
colors = {
staticBody: (255, 255, 255, 255),
dynamicBody: (127, 127, 127, 255),
}
# Let's play with extending the shape classes to draw for us.
def simulate(cmd, trj):
import Box2D # The main library
# Box2D.b2 maps Box2D.b2Vec2 to vec2 (and so on)
from Box2D.b2 import (world, polygonShape, circleShape, staticBody, dynamicBody, vec2)
# --- constants ---
# Box2D deals with meters, but we want to display pixels,
# so define a conversion factor:
PPM = 20.0 # pixels per meter
TIME_STEP = 1.0 / TARGET_FPS
SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480
# --- pygame setup ---
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption('Simple pygame example')
clock = pygame.time.Clock()
# --- pybox2d world setup ---
# Create the world
world = world(gravity=(0, 0), doSleep=True)
# Add a static body to hold the ground shape
ground_body = world.CreateStaticBody(
position=(0, 1),
shapes=polygonShape(box=(50, 1)),
)
# Create dynamic bodies
des_body = world.CreateDynamicBody(position=(15, 12), angle=0,
linearDamping = 0.5*4*9.8, angularDamping = 0.3*1/12*9.8)
obs1_body = world.CreateDynamicBody(position=(18, 12), angle=0,
linearDamping = 0.5*16*9.8, angularDamping = 0.3*4/12*9.8)
obs2_body = world.CreateDynamicBody(position=(11,11), angle=0,
linearDamping = 0.5*36*9.8, angularDamping = 0.3*4/12*9.8)
# Create fingers as kinematic bodies (infinite masses and directly controls velocity)
width = 2.5
fng1 = world.CreateKinematicBody(position=(16, 5), angle = 0)
fng2 = world.CreateKinematicBody(position=(16+width,5), angle = 0)
# And add box fixtures onto it (with a nonzero density, so it will move)
des_box = des_body.CreatePolygonFixture(box=(1, 1), density=1, friction=0.3, restitution = 0.8)
obs1_box = obs1_body.CreatePolygonFixture(box=(2, 2), density=1, friction=0.3, restitution = 0.8)
obs2_box = obs2_body.CreatePolygonFixture(box=(3, 3), density=1, friction=0.3, restitution = 0.8)
# Add sensors for the contact points
# print vec2(-1,0)
cnt1 = des_body.CreatePolygonFixture(box=(0.05, 0.05, vec2(-1,0), 0), density=0, isSensor = True)
cnt2 = des_body.CreatePolygonFixture(box=(0.05, 0.05, vec2( 1,0), 0), density=0, isSensor = True)
printflag = True
# Model fingers as small circular cross sections
# circle = circleShape(radius=0.1)
fng1_cir = fng1.CreatePolygonFixture(box = (0.1, 0.1), density = 5, friction = 0.3)
fng2_cir = fng2.CreatePolygonFixture(box = (0.1, 0.1), density = 5, friction = 0.3)
# Mass and Moment of Inertia data
# print "des_body: " + str(des_body.mass) + " kg , " + str(des_body.inertia) + " kg*m^2"
# print "obs_body1: " + str(obs_body1.mass) + " kg , " + str(obs_body1.inertia) + " kg*m^2"
# print fng1.linearVelocity
colors = [(255, 255, 255, 255), (255, 50, 50, 255), (124,252,0), (124,252,0),
(50, 50, 255, 255), (50, 50, 255, 255), (255, 255, 255, 255), (255, 255, 255, 255)]
bodies = [ground_body, des_body, obs1_body, obs2_body, fng1, fng2]
# LOAD DESIRED VELOCITY PROFILE
if cmd == 'naive':
v_prof = np.loadtxt('examples/vel_prof1.txt', delimiter=';')
v_x = v_prof[:,0]
v_y = v_prof[:,1]
psi_prof = np.loadtxt('examples/pos_prof1.txt', delimiter=';')
xfng = np.reshape(np.matrix(psi_prof[:,0]),(N,1))
yfng = np.reshape(np.matrix(psi_prof[:,1]),(N,1))
# print xfng
# print v_y/TARGET_FPS
else:
v_prof = np.gradient(trj, axis=0)*TARGET_FPS
v_x = v_prof[:,0]
v_y = v_prof[:,1]
xfng = trj[:,0]
yfng = trj[:,1]
# print 'something else', v_x
# GATHER ACTUAL FNG POSITIONS
# xfng = np.zeros((N, 1))
# yfng = np.zeros((N, 1))
# INTIALIZE THE COST FUNCTION
fx = 0
fy = 0
LQ = 1
xdes = np.zeros((N,1))
ydes = np.zeros((N,1))
hits = 0
cnt_reward = 0
# --- main game loop ---
# while True:
for k in range(N):
# Check the event queue
for event in pygame.event.get():
if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE):
# The user closed the window or pressed escape
running = False
screen.fill((0, 0, 0, 0))
# Draw the world
i = 0
# Cast a ray from the center of the fingers to the desired object
x_ray = fng1.worldCenter[0] + width/2
y_ray = fng1.worldCenter[1]
x_db = des_body.worldCenter[0]
y_db = des_body.worldCenter[1]
input = rayCastInput(p1=(x_ray,y_ray), p2=(x_db,y_db), maxFraction=1)
output = rayCastOutput()
for body in bodies: # or: world.bodies
# The body gives us the position and angle of its shapes
for fixture in body.fixtures:
# The fixture holds information like density and friction,
# and also the shape.
shape = fixture.shape
# Naively assume that this is a polygon shape. (not good normally!)
# We take the body's transform and multiply it with each
# vertex, and then convert from meters to pixels with the scale
# factor.
vertices = [(body.transform * v) * PPM for v in shape.vertices]
# But wait! It's upside-down! Pygame and Box2D orient their
# axes in different ways. Box2D is just like how you learned
# in high school, with positive x and y directions going
# right and up. Pygame, on the other hand, increases in the
# right and downward directions. This means we must flip
# the y components.
vertices = [(v[0], SCREEN_HEIGHT - v[1]) for v in vertices]
if body != des_body:
hit = shape.RayCast(output, input, body.transform, 0)
if hit:
hits = 1 + hits
pygame.draw.polygon(screen, colors[i], vertices)
i = i + 1
# print i
vd = vec2((float)(v_x[k]), (float)(v_y[k]))
fng1.linearVelocity = vd
fng2.linearVelocity = vd
# print fng1.linearVelocity
# print fng2.linearVelocity
if hits > 0:
cnt_reward = 0
else:
cnt_reward = 1*LQ
print cnt_reward
# Collect data from these bodies
KEx_des, KEy_des = KE(des_body)
KEx_obs1, KEy_obs1 = KE(obs1_body)
KEx_obs2, KEy_obs2 = KE(obs2_body)
KEx_obs = KEx_obs1 + KEx_obs2
KEy_obs = KEy_obs1 + KEy_obs2
psi_des = des_body.GetWorldPoint(des_body.localCenter + [-width/2,0])
xdes[k] = psi_des[0]
ydes[k] = psi_des[1]
# xdes[k] = 13.75 # (CONSTANT)
# ydes[k] = 12 # (CONSTANT)12
# Collect data from the fingers
KEx_fng1, KEy_fng1 = KE_fng(fng1, mfng)
KEx_fng2, KEy_fng2 = KE_fng(fng2, mfng)
# xfng[k] = fng1.worldCenter[0]
# yfng[k] = fng1.worldCenter[1]
# # Check contacts
# cnt_reward = 0
# for c in des_body.contacts:
# # if printflag:
# # print c.contact.touching
# # printflag = False
# if c.contact.touching :
# # print "sensor triggered"
# cnt_reward = 0
# else:
# # print "contacts points are free"
# cnt_reward = 1*LQ
# Integrate the Cost function
# print cnt_reward
fx = C1*KEx_fng1+ C2*mfng*np.abs(xfng[k]-xdes[k])**2 + C3*KEx_des + C4*KEx_obs - C5*cnt_reward + fx
fy = C1*KEy_fng1+ C2*mfng*np.abs(yfng[k]-ydes[k])**2 + C3*KEy_des + C4*KEy_obs - C5*cnt_reward + fy
# print "KEx: " + str(KEx_fng1) + ", KEy: " + str(KEy_fng1)
# Make Box2D simulate the physics of our world for one step.
# Instruct the world to perform a single step of simulation. It is
# generally best to keep the time step and iterations fixed.
# See the manual (Section "Simulating the World") for further discussion
# on these parameters and their implications.
world.Step(TIME_STEP, 10, 10)
# Flip the screen and try to keep at the target FPS
if cmd == "naive" or cmd == "show":
pygame.display.flip()
clock.tick(TARGET_FPS)
else:
pass
pygame.quit()
print('Simulation Done!')
# print 'xdes', xdes
# print 'xfng', xfng
return xfng, yfng, xdes, ydes, fx, fy
def create_wall_tile(self, position, size=(0.5, 0.5), **kwargs): shape = b2.polygonShape(box=size) self.bodies.append(self.world.CreateStaticBody(shapes=shape, position=position, userData=self, **kwargs))
def draw(self, world, init_x, init_y, force_to_center):
''' Circular body
MAIN_BODY_FIXTURES = fixtureDef(
shape=circleShape(radius=0.4),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F
)
'''
MAIN_BODY_FIXTURES = [
fixtureDef(shape=polygonShape(vertices=[(x / self.SCALE,
y / self.SCALE)
for x, y in polygon]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x000F) for polygon in MAIN_BODY_POLYGONS
]
LEG_FD = fixtureDef(shape=polygonShape(box=(self.LEG_W / 2,
self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * self.LEG_W / 2,
self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
main_body = world.CreateDynamicBody(position=(init_x, init_y),
fixtures=MAIN_BODY_FIXTURES)
basis_color1 = (0.6, 0.3, 0.5)
basis_color2 = (0.4, 0.2, 0.3)
main_body.color1 = tuple([c - 0.1 for c in basis_color1])
main_body.color2 = tuple([c - 0.1 for c in basis_color2])
leg_color1 = tuple([c + 0.1 for c in basis_color1])
leg_color2 = tuple([c + 0.1 for c in basis_color2])
main_body.ApplyForceToCenter((force_to_center, 0), True)
main_body.userData = CustomBodyUserData(True,
is_contact_critical=True,
name="main_body")
self.body_parts.append(main_body)
self.reference_head_object = main_body
for i in [+1, -1] * self.nb_pairs_of_legs:
##### First part of the leg #####
upper_leg_angle = 0.25 * np.pi * i
upper_leg_x_distance = np.sin(upper_leg_angle) * self.LEG_H / 2
upper_leg_y_distance = np.cos(upper_leg_angle) * self.LEG_H / 2
upper_leg_x = init_x - i * MAIN_BODY_BOTTOM_WIDTH / self.SCALE / 2 - upper_leg_x_distance
upper_leg_y = init_y + upper_leg_y_distance - self.LEG_DOWN
upper_leg = world.CreateDynamicBody(position=(upper_leg_x,
upper_leg_y),
angle=upper_leg_angle,
fixtures=LEG_FD)
upper_leg.color1 = leg_color1
upper_leg.color2 = leg_color2
rjd = revoluteJointDef(
bodyA=main_body,
bodyB=upper_leg,
anchor=(init_x - i * MAIN_BODY_BOTTOM_WIDTH / self.SCALE / 2,
init_y - self.LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.1 * np.pi,
upperAngle=0.1 * np.pi,
)
upper_leg.userData = CustomBodyUserData(False, name="upper_leg")
self.body_parts.append(upper_leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
##### Second part of the leg #####
middle_leg_angle = 0.7 * np.pi * i
middle_leg_x_distance = np.sin(middle_leg_angle) * self.LEG_H / 2
middle_leg_y_distance = -np.cos(middle_leg_angle) * self.LEG_H / 2
middle_leg_x = upper_leg_x - upper_leg_x_distance - middle_leg_x_distance
middle_leg_y = upper_leg_y + upper_leg_y_distance - middle_leg_y_distance
middle_leg = world.CreateDynamicBody(position=(middle_leg_x,
middle_leg_y),
angle=middle_leg_angle,
fixtures=LEG_FD)
middle_leg.color1 = leg_color1
middle_leg.color2 = leg_color2
rjd = revoluteJointDef(
bodyA=upper_leg,
bodyB=middle_leg,
anchor=(upper_leg_x - upper_leg_x_distance,
upper_leg_y + upper_leg_y_distance),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.15 * np.pi,
upperAngle=0.15 * np.pi,
)
middle_leg.userData = CustomBodyUserData(False, name="middle_leg")
self.body_parts.append(middle_leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
##### Third part of the leg #####
lower_leg_angle = 0.9 * np.pi * i
lower_leg_x_distance = np.sin(lower_leg_angle) * self.LEG_H / 2
lower_leg_y_distance = -np.cos(lower_leg_angle) * self.LEG_H / 2
lower_leg_x = middle_leg_x - middle_leg_x_distance - lower_leg_x_distance
lower_leg_y = middle_leg_y - middle_leg_y_distance - lower_leg_y_distance
lower_leg = world.CreateDynamicBody(position=(lower_leg_x,
lower_leg_y),
angle=lower_leg_angle,
fixtures=LOWER_FD)
lower_leg.color1 = leg_color1
lower_leg.color2 = leg_color2
rjd = revoluteJointDef(
bodyA=middle_leg,
bodyB=lower_leg,
anchor=(middle_leg_x - middle_leg_x_distance,
middle_leg_y - middle_leg_y_distance),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.2 * np.pi,
upperAngle=0.2 * np.pi,
)
lower_leg.userData = CustomBodyUserData(True, name="lower_leg")
self.body_parts.append(lower_leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE,
True,
contact_body=lower_leg)
self.motors.append(joint_motor)
def _reset(self):
self._destroy()
self.game_over = False
self.prev_shaping = None
W = VIEWPORT_W/SCALE
H = VIEWPORT_H/SCALE
# terrain
CHUNKS = 11
height = np.random.uniform(0, H/2, size=(CHUNKS+1,) )
chunk_x = [W/(CHUNKS-1)*i for i in xrange(CHUNKS)]
self.helipad_x1 = chunk_x[CHUNKS//2-1]
self.helipad_x2 = chunk_x[CHUNKS//2+1]
self.helipad_y = H/4
height[CHUNKS//2-2] = self.helipad_y
height[CHUNKS//2-1] = self.helipad_y
height[CHUNKS//2+0] = self.helipad_y
height[CHUNKS//2+1] = self.helipad_y
height[CHUNKS//2+2] = self.helipad_y
smooth_y = [0.33*(height[i-1] + height[i+0] + height[i+1]) for i in xrange(CHUNKS)]
self.moon = self.world.CreateStaticBody( shapes=edgeShape(vertices=[(0, 0), (W, 0)]) )
self.sky_polys = []
for i in xrange(CHUNKS-1):
p1 = (chunk_x[i], smooth_y[i])
p2 = (chunk_x[i+1], smooth_y[i+1])
self.moon.CreateEdgeFixture(
vertices=[p1,p2],
density=0,
friction=0.1)
self.sky_polys.append( [p1, p2, (p2[0],H), (p1[0],H)] )
self.moon.color1 = (0.0,0.0,0.0)
self.moon.color2 = (0.0,0.0,0.0)
initial_y = VIEWPORT_H/SCALE
self.lander = self.world.CreateDynamicBody(
position = (VIEWPORT_W/SCALE/2, initial_y),
angle=0.0,
fixtures = fixtureDef(
shape=polygonShape(vertices=[ (x/SCALE,y/SCALE) for x,y in LANDER_POLY ]),
density=5.0,
friction=0.1,
categoryBits=0x0010,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
self.lander.color1 = (0.5,0.4,0.9)
self.lander.color2 = (0.3,0.3,0.5)
self.lander.ApplyForceToCenter( (
np.random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM),
np.random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM)
), True)
self.legs = []
for i in [-1,+1]:
leg = self.world.CreateDynamicBody(
position = (VIEWPORT_W/SCALE/2 - i*LEG_AWAY/SCALE, initial_y),
angle = (i*0.05),
fixtures = fixtureDef(
shape=polygonShape(box=(LEG_W/SCALE, LEG_H/SCALE)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001)
)
leg.color1 = (0.5,0.4,0.9)
leg.color2 = (0.3,0.3,0.5)
rjd = revoluteJointDef(
bodyA=self.lander,
bodyB=leg,
localAnchorA=(0, 0),
localAnchorB=(i*LEG_AWAY/SCALE, LEG_DOWN/SCALE),
enableMotor=True,
enableLimit=True,
maxMotorTorque=LEG_SPRING_TORQUE,
motorSpeed=+0.3*i # low enough not to jump back into the sky
)
if i==-1:
rjd.lowerAngle = +0.9 - 0.5 # Yes, the most esoteric numbers here, angles legs have freedom to travel within
rjd.upperAngle = +0.9
else:
rjd.lowerAngle = -0.9
rjd.upperAngle = -0.9 + 0.5
leg.joint = self.world.CreateJoint(rjd)
self.legs.append(leg)
self.drawlist = [self.lander] + self.legs
return self._step(0)[0]
def __init__(self, world, init_angle, init_x, init_y):
self.world = world
self.hull = self.world.CreateDynamicBody(
position=(init_x, init_y),
angle=init_angle,
fixtures=[
fixtureDef(
shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in HULL_POLY1]),
density=1.0,
),
fixtureDef(
shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in HULL_POLY2]),
density=1.0,
),
fixtureDef(
shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in HULL_POLY3]),
density=1.0,
),
fixtureDef(
shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in HULL_POLY4]),
density=1.0,
),
],
)
self.hull.color = (0.8, 0.0, 0.0)
self.wheels = []
self.fuel_spent = 0.0
WHEEL_POLY = [
(-WHEEL_W, +WHEEL_R),
(+WHEEL_W, +WHEEL_R),
(+WHEEL_W, -WHEEL_R),
(-WHEEL_W, -WHEEL_R),
]
for wx, wy in WHEELPOS:
front_k = 1.0 if wy > 0 else 1.0
w = self.world.CreateDynamicBody(
position=(init_x + wx * SIZE, init_y + wy * SIZE),
angle=init_angle,
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x * front_k * SIZE,
y * front_k * SIZE)
for x, y in WHEEL_POLY]),
density=0.1,
categoryBits=0x0020,
maskBits=0x001,
restitution=0.0,
),
)
w.wheel_rad = front_k * WHEEL_R * SIZE
w.color = WHEEL_COLOR
w.gas = 0.0
w.brake = 0.0
w.steer = 0.0
w.phase = 0.0 # wheel angle
w.omega = 0.0 # angular velocity
w.skid_start = None
w.skid_particle = None
rjd = revoluteJointDef(
bodyA=self.hull,
bodyB=w,
localAnchorA=(wx * SIZE, wy * SIZE),
localAnchorB=(0, 0),
enableMotor=True,
enableLimit=True,
maxMotorTorque=180 * 900 * SIZE * SIZE,
motorSpeed=0,
lowerAngle=-0.4,
upperAngle=+0.4,
)
w.joint = self.world.CreateJoint(rjd)
w.tiles = set()
w.userData = w
self.wheels.append(w)
self.drawlist = self.wheels + [self.hull]
self.particles = []
def convex_box2d_shape_from_shapely(geometry):
"""Create a Box2D shape from the convex hull of shapely geometry"""
return b2.polygonShape(
vertices=list(geometry.convex_hull.exterior.coords))
VIEWPORT_W = 600
VIEWPORT_H = 400
TERRAIN_STEP = 14 / SCALE
TERRAIN_LENGTH = 200 # in steps
TERRAIN_HEIGHT = VIEWPORT_H / SCALE / 4
TERRAIN_GRASS = 10 # low long are grass spots, in steps
TERRAIN_STARTPAD = 20 # in steps
FRICTION = 2.5
BIPED_LIMIT = 1600
BIPED_HARDCORE_LIMIT = 2000
HULL_FD = fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in HULL_POLY]),
density=5.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
LEG_FD = fixtureDef(shape=polygonShape(box=(LEG_W / 2, LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001)
LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * LEG_W / 2, LEG_H / 2)),
density=1.0,
restitution=0.0,
def _generate_terrain(self, hardcore):
GRASS, STUMP, STAIRS, PIT, _STATES_ = range(5)
state = GRASS
velocity = 0.0
y = TERRAIN_HEIGHT
counter = TERRAIN_STARTPAD
oneshot = False
self.terrain = []
self.terrain_x = []
self.terrain_y = []
for i in range(TERRAIN_LENGTH):
x = i*TERRAIN_STEP
self.terrain_x.append(x)
if state==GRASS and not oneshot:
velocity = 0.8*velocity + 0.01*np.sign(TERRAIN_HEIGHT - y)
if i > TERRAIN_STARTPAD: velocity += np.random.uniform(-1, 1)/SCALE #1
y += velocity
elif state==PIT and oneshot:
counter = np.random.randint(3, 5)
poly = [
(x, y),
(x+TERRAIN_STEP, y),
(x+TERRAIN_STEP, y-4*TERRAIN_STEP),
(x, y-4*TERRAIN_STEP),
]
t = self.world.CreateStaticBody(
fixtures = fixtureDef(
shape=polygonShape(vertices=poly),
friction = FRICTION
))
t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
self.terrain.append(t)
t = self.world.CreateStaticBody(
fixtures = fixtureDef(
shape=polygonShape(vertices=[(p[0]+TERRAIN_STEP*counter,p[1]) for p in poly]),
friction = FRICTION
))
t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
self.terrain.append(t)
counter += 2
original_y = y
elif state==PIT and not oneshot:
y = original_y
if counter > 1:
y -= 4*TERRAIN_STEP
elif state==STUMP and oneshot:
counter = np.random.randint(1, 3)
poly = [
(x, y),
(x+counter*TERRAIN_STEP, y),
(x+counter*TERRAIN_STEP, y+counter*TERRAIN_STEP),
(x, y+counter*TERRAIN_STEP),
]
t = self.world.CreateStaticBody(
fixtures = fixtureDef(
shape=polygonShape(vertices=poly),
friction = FRICTION
))
t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
self.terrain.append(t)
elif state==STAIRS and oneshot:
stair_height = +1 if np.random.ranf() > 0.5 else -1
stair_width = np.random.randint(4, 5)
stair_steps = np.random.randint(3, 5)
original_y = y
for s in range(stair_steps):
poly = [
(x+( s*stair_width)*TERRAIN_STEP, y+( s*stair_height)*TERRAIN_STEP),
(x+((1+s)*stair_width)*TERRAIN_STEP, y+( s*stair_height)*TERRAIN_STEP),
(x+((1+s)*stair_width)*TERRAIN_STEP, y+(-1+s*stair_height)*TERRAIN_STEP),
(x+( s*stair_width)*TERRAIN_STEP, y+(-1+s*stair_height)*TERRAIN_STEP),
]
t = self.world.CreateStaticBody(
fixtures = fixtureDef(
shape=polygonShape(vertices=poly),
friction = FRICTION
))
t.color1, t.color2 = (1,1,1), (0.6,0.6,0.6)
self.terrain.append(t)
counter = stair_steps*stair_width
elif state==STAIRS and not oneshot:
s = stair_steps*stair_width - counter - stair_height
n = s/stair_width
y = original_y + (n*stair_height)*TERRAIN_STEP
oneshot = False
self.terrain_y.append(y)
counter -= 1
if counter==0:
counter = np.random.randint(TERRAIN_GRASS/2, TERRAIN_GRASS)
if state==GRASS and hardcore:
state = np.random.randint(1, _STATES_)
oneshot = True
else:
state = GRASS
oneshot = True
self.terrain_poly = []
for i in range(TERRAIN_LENGTH-1):
poly = [
(self.terrain_x[i], self.terrain_y[i]),
(self.terrain_x[i+1], self.terrain_y[i+1])
]
t = self.world.CreateStaticBody(
fixtures = fixtureDef(
shape=edgeShape(vertices=poly),
friction = FRICTION,
categoryBits=0x0001,
))
color = (0.3, 1.0 if i%2==0 else 0.8, 0.3)
t.color1 = color
t.color2 = color
self.terrain.append(t)
color = (0.4, 0.6, 0.3)
poly += [ (poly[1][0], 0), (poly[0][0], 0) ]
self.terrain_poly.append( (poly, color) )
self.terrain.reverse()
def __init__(self,
world,
init_angle,
init_x,
init_y,
variable_speed=False):
self.world = world
self.grass_friction = 1.0
self.variable_speed = variable_speed
# Create car hull
self.hull = self.world.CreateDynamicBody(
position=(init_x, init_y),
angle=(init_angle),
fixtures=[
fixtureDef(
shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in HULL_POLY1]),
density=1.0),
fixtureDef(
shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in HULL_POLY2]),
density=1.0),
fixtureDef(
shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in HULL_POLY3]),
density=1.0),
fixtureDef(
shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in HULL_POLY4]),
density=1.0)
])
self.hull.color = (0.8, 0, 0)
# Create wheels
self.wheels = []
for wx, wy in WHEELPOS:
# First two wheels are front wheels
# Next two are rear wheels
w = self.world.CreateDynamicBody(
position=(init_x + wx * SIZE, init_y + wy * SIZE),
angle=init_angle,
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x * SIZE, y * SIZE)
for x, y in WHEEL_POLY]),
density=0.1,
categoryBits=0x0020,
maskBits=0x001,
restitution=0.0))
w.front = True if wy > 0 else False
w.wheel_rad = WHEEL_R * SIZE
w.color = WHEEL_COLOR
w.speed = 0.0 # linear speed
w.steering_angle = 0.0 # steering angle
w.omega = 0.0 # angular speed
# Skid
w.skid_particle = None
w.skid_start = None
# Revolute joint to model steering
rjd = revoluteJointDef(bodyA=self.hull,
bodyB=w,
localAnchorA=(wx * SIZE, wy * SIZE),
localAnchorB=(0, 0),
enableMotor=True,
enableLimit=True,
maxMotorTorque=180 * 900 * SIZE**2,
motorSpeed=0,
lowerAngle=-0.4,
upperAngle=+0.4)
w.joint = self.world.CreateJoint(rjd)
w.tiles = set()
w.userData = w
self.wheels.append(w)
self.drawlist = self.wheels + [self.hull]
self.particles = []
def _create_track(self):
CHECKPOINTS = 12
# Create checkpoints
checkpoints = []
for c in range(CHECKPOINTS):
alpha = 2*math.pi*c/CHECKPOINTS + self.np_random.uniform(0, 2*math.pi*1/CHECKPOINTS)
rad = self.np_random.uniform(TRACK_RAD/3, TRACK_RAD)
if c==0:
alpha = 0
rad = 1.5*TRACK_RAD
if c==CHECKPOINTS-1:
alpha = 2*math.pi*c/CHECKPOINTS
self.start_alpha = 2*math.pi*(-0.5)/CHECKPOINTS
rad = 1.5*TRACK_RAD
checkpoints.append( (alpha, rad*math.cos(alpha), rad*math.sin(alpha)) )
#print "\n".join(str(h) for h in checkpoints)
#self.road_poly = [ ( # uncomment this to see checkpoints
# [ (tx,ty) for a,tx,ty in checkpoints ],
# (0.7,0.7,0.9) ) ]
self.road = []
# Go from one checkpoint to another to create track
x, y, beta = 1.5*TRACK_RAD, 0, 0
dest_i = 0
laps = 0
track = []
no_freeze = 2500
visited_other_side = False
while 1:
alpha = math.atan2(y, x)
if visited_other_side and alpha > 0:
laps += 1
visited_other_side = False
if alpha < 0:
visited_other_side = True
alpha += 2*math.pi
while True: # Find destination from checkpoints
failed = True
while True:
dest_alpha, dest_x, dest_y = checkpoints[dest_i % len(checkpoints)]
if alpha <= dest_alpha:
failed = False
break
dest_i += 1
if dest_i % len(checkpoints) == 0: break
if not failed: break
alpha -= 2*math.pi
continue
r1x = math.cos(beta)
r1y = math.sin(beta)
p1x = -r1y
p1y = r1x
dest_dx = dest_x - x # vector towards destination
dest_dy = dest_y - y
proj = r1x*dest_dx + r1y*dest_dy # destination vector projected on rad
while beta - alpha > 1.5*math.pi: beta -= 2*math.pi
while beta - alpha < -1.5*math.pi: beta += 2*math.pi
prev_beta = beta
proj *= SCALE
if proj > 0.3: beta -= min(TRACK_TURN_RATE, abs(0.001*proj))
if proj < -0.3: beta += min(TRACK_TURN_RATE, abs(0.001*proj))
x += p1x*TRACK_DETAIL_STEP
y += p1y*TRACK_DETAIL_STEP
track.append( (alpha,prev_beta*0.5 + beta*0.5,x,y) )
if laps > 4: break
no_freeze -= 1
if no_freeze==0: break
#print "\n".join([str(t) for t in enumerate(track)])
# Find closed loop range i1..i2, first loop should be ignored, second is OK
i1, i2 = -1, -1
i = len(track)
while True:
i -= 1
if i==0: return False # Failed
pass_through_start = track[i][0] > self.start_alpha and track[i-1][0] <= self.start_alpha
if pass_through_start and i2==-1:
i2 = i
elif pass_through_start and i1==-1:
i1 = i
break
if self.verbose == 1:
print("Track generation: %i..%i -> %i-tiles track" % (i1, i2, i2-i1))
assert i1!=-1
assert i2!=-1
track = track[i1:i2-1]
first_beta = track[0][1]
first_perp_x = math.cos(first_beta)
first_perp_y = math.sin(first_beta)
# Length of perpendicular jump to put together head and tail
well_glued_together = np.sqrt(
np.square( first_perp_x*(track[0][2] - track[-1][2]) ) +
np.square( first_perp_y*(track[0][3] - track[-1][3]) ))
if well_glued_together > TRACK_DETAIL_STEP:
return False
# Red-white border on hard turns
border = [False]*len(track)
for i in range(len(track)):
good = True
oneside = 0
for neg in range(BORDER_MIN_COUNT):
beta1 = track[i-neg-0][1]
beta2 = track[i-neg-1][1]
good &= abs(beta1 - beta2) > TRACK_TURN_RATE*0.2
oneside += np.sign(beta1 - beta2)
good &= abs(oneside) == BORDER_MIN_COUNT
border[i] = good
for i in range(len(track)):
for neg in range(BORDER_MIN_COUNT):
border[i-neg] |= border[i]
# Create tiles
for i in range(len(track)):
alpha1, beta1, x1, y1 = track[i]
alpha2, beta2, x2, y2 = track[i-1]
road1_l = (x1 - TRACK_WIDTH*math.cos(beta1), y1 - TRACK_WIDTH*math.sin(beta1))
road1_r = (x1 + TRACK_WIDTH*math.cos(beta1), y1 + TRACK_WIDTH*math.sin(beta1))
road2_l = (x2 - TRACK_WIDTH*math.cos(beta2), y2 - TRACK_WIDTH*math.sin(beta2))
road2_r = (x2 + TRACK_WIDTH*math.cos(beta2), y2 + TRACK_WIDTH*math.sin(beta2))
t = self.world.CreateStaticBody( fixtures = fixtureDef(
shape=polygonShape(vertices=[road1_l, road1_r, road2_r, road2_l])
))
t.userData = t
c = 0.01*(i%3)
t.color = [ROAD_COLOR[0] + c, ROAD_COLOR[1] + c, ROAD_COLOR[2] + c]
t.road_visited = False
t.road_friction = 1.0
t.fixtures[0].sensor = True
self.road_poly.append(( [road1_l, road1_r, road2_r, road2_l], t.color ))
self.road.append(t)
if border[i]:
side = np.sign(beta2 - beta1)
b1_l = (x1 + side* TRACK_WIDTH *math.cos(beta1), y1 + side* TRACK_WIDTH *math.sin(beta1))
b1_r = (x1 + side*(TRACK_WIDTH+BORDER)*math.cos(beta1), y1 + side*(TRACK_WIDTH+BORDER)*math.sin(beta1))
b2_l = (x2 + side* TRACK_WIDTH *math.cos(beta2), y2 + side* TRACK_WIDTH *math.sin(beta2))
b2_r = (x2 + side*(TRACK_WIDTH+BORDER)*math.cos(beta2), y2 + side*(TRACK_WIDTH+BORDER)*math.sin(beta2))
self.road_poly.append(( [b1_l, b1_r, b2_r, b2_l], (1,1,1) if i%2==0 else (1,0,0) ))
self.track = track
return True
]
LEG_DOWN = -8/SCALE
LEG_W, LEG_H = 8/SCALE, 34/SCALE
VIEWPORT_W = 600
VIEWPORT_H = 400
TERRAIN_STEP = 14/SCALE
TERRAIN_LENGTH = 200 # in steps
TERRAIN_HEIGHT = VIEWPORT_H/SCALE/4
TERRAIN_GRASS = 10 # low long are grass spots, in steps
TERRAIN_STARTPAD = 20 # in steps
FRICTION = 2.5
HULL_FD = fixtureDef(
shape=polygonShape(vertices=[ (x/SCALE,y/SCALE) for x,y in HULL_POLY ]),
density=5.0,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
LEG_FD = fixtureDef(
shape=polygonShape(box=(LEG_W/2, LEG_H/2)),
density=1.0,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001)
LOWER_FD = fixtureDef(
shape=polygonShape(box=(0.8*LEG_W/2, LEG_H/2)),