def __init__(self,
world,
img,
pos,
rotation,
shape,
static=False,
scale=1):
img = pygame.transform.rotozoom(img, 0, scale)
self.img = img
angle = rotation * (pi / 180)
self.shape = shape
if static:
self.body = world.CreateStaticBody(position=(pos), angle=angle)
else:
self.body = world.CreateDynamicBody(position=(pos),
fixedRotation=False,
angle=angle)
if shape == CIRCLE:
radius = self.img.get_rect().width * .8 / 2 / PPM
self.fix = self.body.CreateCircleFixture(radius=radius,
density=5,
friction=0.3,
restitution=.5)
self.radius = radius
elif shape == BOX:
dimensions = (self.img.get_rect().width / (2 * PPM),
self.img.get_rect().height / (2 * PPM))
self.fix = self.body.CreatePolygonFixture(box=dimensions,
density=4,
friction=0.3,
restitution=.5)
def __init__(self, *group):
super().__init__(*group)
self.radius = radius
self.rect = self.image.get_rect()
body = world.CreateDynamicBody(position=(20, 20))
self.circle = body.CreateCircleFixture(radius=1,
density=1,
friction=0.3)
self.rect = pygame.Rect(20, 20, 260, 260)
def __init__(self, world, x=10, y=15, color=None):
if color is None:
color = (255, 255, 0, 255)
self.body = world.CreateDynamicBody(position=(x, y), linearDamping=1)
self.fixture = self.body.CreateCircleFixture(radius=0.35, density=1, friction=0.3)
self.circle = self.fixture.shape
self.base_color = color
self.color = color
self.prev_force = None
def __init__(self, all_sprites, y, post):
super().__init__(all_sprites)
self.post1 = post
self.post = world.CreateDynamicBody(position=(33, 40 - y - 5),
shapes=polygonShape(box=(2, 2)),
linearVelocity=(-5, 0),
gravityScale=0)
self.image = load_image("plume.png", -1)
self.rect = pygame.Rect(630, (y + 3) * PPM, 40, 40)
self.mask = pygame.mask.from_surface(self.image)
def load_chain(world, data, dynamic):
chain = []
for ind in range(len(data)):
row = data[ind]
circle = circleShape(pos=(0,0), radius=0.001)
if dynamic:
new_body = world.CreateDynamicBody(position=row, userData=ind)
new_body.CreateFixture(fixtureDef(shape=circle, density=1, friction=0.3))
# if ind > 0:
# world.CreateRevoluteJoint(bodyA=chain[ind-1], bodyB=new_body, anchor=chain[ind-1].worldCenter)
else:
new_body = world.CreateStaticBody(position=row, shapes=[circle])
chain.append(new_body)
return chain
def __init__(self, world, showcase, pos, size, angle=0, static=False):
if static:
self.body = world.CreateStaticBody(
position=pos,
shapes=polygonShape(box=size),
)
else:
self.body = world.CreateDynamicBody(position=pos, angle=angle)
self.body.CreatePolygonFixture(box=size, density=1, friction=0.3)
points = self.body.fixtures[0].shape.vertices
index = (0, 1, 3, 1, 2, 3)
self.figure = graphicsHelper.createMesh(points, index)
showcase.add(self.figure)
def __init__(self, all_sprites, y):
"""Инициализация объекта"""
super().__init__(all_sprites)
self.y = y
self.post = world.CreateDynamicBody(position=(33, 40),
shapes=polygonShape(box=(3, y)),
linearVelocity=(-5, 0),
gravityScale=0)
# self.image = pygame.Surface((60, y * PPM), pygame.SRCALPHA, 32)
# pygame.draw.rect(self.image, pygame.Color(0, 255, 0), (0, 0, 60, y * PPM))
self.image = load_image("postdown.png", -1).subsurface(
pygame.Rect(0, 800 - y * PPM, 60, y * PPM))
self.image.set_colorkey("BLACK")
self.rect = pygame.Rect(630, 0, 60, y * PPM)
self.mask = pygame.mask.from_surface(self.image)
self.wascount = False
def __init__(self,
img,
pos,
rotation,
shape,
static=False,
scale=1,
density=1):
img = pygame.transform.rotozoom(img, 0, scale)
self.img = img
angle = rotation * (pi / 180)
self.shape = shape
self.contact_impulse = 0
self.min_impulse = 2.0
if static:
self.body = world.CreateStaticBody(position=(pos),
angle=angle,
userData=self)
else:
self.body = world.CreateDynamicBody(position=(pos),
fixedRotation=False,
angle=angle,
userData=self)
if shape == CIRCLE:
radius = self.img.get_rect().width * .6 / 2 / PPM
self.fix = self.body.CreateCircleFixture(radius=radius,
density=density,
friction=0.3,
restitution=.5)
self.radius = radius
elif shape == BOX:
dimensions = (self.img.get_rect().width / (2 * PPM),
self.img.get_rect().height / (2 * PPM))
self.fix = self.body.CreatePolygonFixture(box=dimensions,
density=density,
friction=0.3,
restitution=.5)
elif shape == RIGHT_TRIANGLE:
rect = self.img.get_rect()
width = rect.width / PPM
height = rect.height / PPM
vertices = [(-width / 2, -height / 2), (width / 2, -height / 2),
(-width / 2, height / 2)]
self.fix = self.body.CreatePolygonFixture(density=density,
vertices=vertices,
friction=0.3,
restitution=.5)
def __init__(self, all_sprites):
"""Инициализация объекта"""
super().__init__(all_sprites)
self.body = world.CreateDynamicBody(position=(5, 20))
circle = self.body.CreateCircleFixture(radius=1,
density=1,
friction=0,
categoryBits=3,
maskBits=2)
self.radius = 20
# self.image = pygame.Surface((62, 48), pygame.SRCALPHA, 32)
self.sprites = [
load_image("bird1.png", -1),
load_image("bird2.png", -1),
load_image("bird3.png", -1),
load_image("bird2.png", -1)
]
self.an = 0
self.image = self.sprites[self.an]
# pygame.draw.circle(self.image, pygame.Color("red"), (20, 20), 20)
self.rect = pygame.Rect(80, 380, 61, 44)
self.mask = pygame.mask.from_surface(self.image)
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 * 1 * 9.8,
angularDamping=0.3 * 1 / 12 * 9.8)
obs_body = world.CreateDynamicBody(position=(18, 12),
angle=0,
linearDamping=0.5 * 4 * 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)
obs_box = obs_body.CreatePolygonFixture(box=(2, 2),
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_body: " + str(obs_body.mass) + " kg , " + str(obs_body.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), (255, 255, 255, 255),
(255, 255, 255, 255)]
bodies = [ground_body, des_body, obs_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))
# --- 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
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]
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
# Collect data from these bodies
KEx_des, KEy_des = KE(des_body)
KEx_obs, KEy_obs = KE(obs_body)
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)
# 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
# And a static body to hold the ground shape
ground_body = world.CreateStaticBody(
position=(0, 0),
shapes=polygonShape(box=(50, 1)),
)
BODY_DIM = (2, 1)
BODY_POS = (10, 11)
LEG_INNER = 0.25
LEG_DIM = (0.25, 2)
LEGA_POS = (BODY_POS[0]-BODY_DIM[0]+LEG_DIM[0], BODY_POS[1]-BODY_DIM[1]+LEG_INNER)
LEGB_POS = (BODY_POS[0]+BODY_DIM[0]-LEG_DIM[0], BODY_POS[1]-BODY_DIM[1]+LEG_INNER)
boxbody = world.CreateDynamicBody(position=BODY_POS, angle=0)
box = boxbody.CreatePolygonFixture(box=BODY_DIM, density=1, friction=0.3)
legabody = world.CreateDynamicBody(position=LEGA_POS, angle=(-45/2)*DEGTORAD)
lega = legabody.CreatePolygonFixture(box=LEG_DIM, density=1, friction=0.3)
lowerlegabody = world.CreateDynamicBody(position=(LEGA_POS[0], LEGA_POS[1]-LEG_DIM[1]))
lowerlega = lowerlegabody.CreatePolygonFixture(box=LEG_DIM, density=1, friction=0.3)
legbbody = world.CreateDynamicBody(position=LEGB_POS, angle=(+45/2)*DEGTORAD)
legb = legbbody.CreatePolygonFixture(box=LEG_DIM, density=1, friction=0.3)
lowerlegbbody = world.CreateDynamicBody(position=(LEGB_POS[0], LEGB_POS[1]-LEG_DIM[1]))
lowerlegb = lowerlegbbody.CreatePolygonFixture(box=LEG_DIM, density=1, friction=0.3)
# legalowerbody = world.CreateDynamicBody(position=(11.5, 8))
# legalower = legalowerbody.CreatePolygonFixture(box=(0.25, 2), density=1, friction=0.3)
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 ---
world = world(gravity=(0, 0), doSleep=True)
bodies = []
#track setup
outer_track = world.CreateBody(shapes=chainShape(vertices=[(3,3), (int(SCREEN_WIDTH/PPM)-3, 3),(int(SCREEN_WIDTH/PPM)-3,int(SCREEN_HEIGHT/PPM)-3),(3,int(SCREEN_HEIGHT/PPM)-3)]))
bodies.append([outer_track,'white'])
inner_track = world.CreateBody(shapes=chainShape(vertices=[(17,int(SCREEN_HEIGHT/PPM)-17),(int(SCREEN_WIDTH/PPM)-17,int(SCREEN_HEIGHT/PPM)-17),(int(SCREEN_WIDTH/PPM)-17, 17),(17,17)]))
bodies.append([inner_track,'gray'])
# Create car
car = world.CreateDynamicBody(position=(10, 15), angle= -np.pi/2, linearDamping=.4, angularDamping = 6)
bodies.append([car,'blue'])
box = car.CreatePolygonFixture(box=(1, .5), density=4, friction=0.2)
box.massData.center = vec2(0,-10) #set center of mass
pressed_keys = []
time_pressed = {
'w' : 0.0,
'a' : 0.0,
's' : 0.0,
'd' : 0.0
}
#for whiskers
# angles, lengths, intercept, normals
rays = [[-np.pi/2, 10, 0, 0],
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, 1),
shapes=polygonShape(box=(50, 5)),
)
# Create a dynamic body
dynamic_body = world.CreateDynamicBody(position=(10, 15), angle=15)
# And add a box fixture onto it (with a nonzero density, so it will move)
box = dynamic_body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
colors = {
staticBody: (255, 255, 255, 255),
dynamicBody: (127, 127, 127, 255),
}
# --- main game loop ---
running = True
while running:
# Check the event queue
for event in pygame.event.get():
if event.type == QUIT or (event.type == KEYDOWN
x0 = 10
y0 = 10
s = 1
total_weight = 10
masses = []
joints = []
min_dists = []
with open("bodies/quad.obj") as f:
for line in f:
if line.startswith("v "):
x, y, _ = [float(loc) for loc in line[2:].split(" ")]
body = world.CreateDynamicBody(
position=(x0 + s*x, y0 + s*y),
fixedRotation=True)
circle = body.CreateCircleFixture(
radius=0.3,
density=1,
friction=0.4)
masses.append(body)
min_dists.append(1)
elif line.startswith("f "):
indices = [int(idx)-1 for idx in line[2:].split(" ")]
for i in range(3):
a = masses[indices[i]]
b = masses[indices[(i+1) % 3]]
l = math.hypot(a.position[0]-b.position[0], a.position[1]-b.position[1])
min_dists[indices[i]] = min(l, min_dists[indices[i]])
min_dists[indices[(i+1) % 3]] = min(l, min_dists[indices[(i+1) % 3]])
# And a static body to hold the ground shape
ground_body = world.CreateStaticBody(
position=(0, 1),
shapes=polygonShape(box=(50, 5)),
)
'''
# Create a dynamic body
dynamic_body = world.CreateDynamicBody(position=(10, 15), angle=15)
# And add a box fixture onto it (with a nonzero density, so it will move)
box = dynamic_body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
'''
#body = world.CreateDynamicBody(position=(20, 45))
#circle = body.CreateCircleFixture(radius=0.5, density=1, friction=0.3)
body = world.CreateDynamicBody(position=(20, 35))
circle = body.CreatePolygonFixture(box=(3, 1), density=1, friction=0.3)
for x in range(0, 10):
body = world.CreateDynamicBody(position=(30 - x * 2, 35 + x * 10),
angle=15)
box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
body = world.CreateDynamicBody(position=(30, 35), angle=15)
box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
body = world.CreateDynamicBody(position=(30, 25), angle=15)
box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
body = world.CreateDynamicBody(position=(10, 15), angle=45)
box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
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, 45))
circle = body.CreateCircleFixture(radius=0.5, density=1, friction=0.3)
body = world.CreateDynamicBody(position=(30, 45), angle=15)
box = body.CreatePolygonFixture(box=(2, 1), 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 my_draw_polygon(polygon, body, fixture):
vertices = [(body.transform * v) * PPM for v in polygon.vertices]
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=(20, 5), angle=0)
fng2 = world.CreateKinematicBody(position=(20 + width, 5), angle=0)
def create_circle():
for i in range(5, 40, 5):
"""Создаем круги"""
body = world.CreateDynamicBody(position=(i, 20))
circle = body.CreateCircleFixture(radius=1, density=1, friction=0.3)
def build(self, world, x0, y0):
width = self.genome['width']
height = self.genome['height']
length1 = self.genome['right_leg']
length2 = self.genome['left_leg']
""" construct body, head, and neck """
body = world.CreateDynamicBody(position=(x0, y0))
body.fixedRotation = True
body.CreatePolygonFixture(box=(width, height), density=1, friction=0.3)
head = world.CreateDynamicBody(position=(x0, y0 + 4))
head.fixedRotation = True
head.CreateCircleFixture(radius=1.8, density=1, friction=0.3)
neck = world.CreateDynamicBody(position=(x0, y0 + 2.5))
neck.fixedRotation = True
neck.CreatePolygonFixture(box=(0.5, 1),
density=1,
friction=WHEEL_FRICTION)
# # for comments, " for docstrings
# Connect the body with the joints, but rotation is not allowed.
world.CreateRevoluteJoint(bodyA=head,
bodyB=neck,
anchor=head.worldCenter,
enableMotor=False)
world.CreateRevoluteJoint(bodyA=neck,
bodyB=body,
anchor=body.worldCenter,
enableMotor=False)
# Construct the leg and connect it with the body
right_leg = world.CreateDynamicBody(position=(x0 + 1, y0 - 2))
right_leg.CreatePolygonFixture(box=(0.5, 3),
density=1,
friction=WHEEL_FRICTION)
world.CreateRevoluteJoint(bodyA=body,
bodyB=right_leg,
anchor=body.worldCenter + (1, -2),
lowerAngle=-0.3 * pi,
upperAngle=0.3 * pi,
enableLimit=True,
maxMotorTorque=MAX_TORQUE,
motorSpeed=cos(SPEED),
enableMotor=True)
left_leg = world.CreateDynamicBody(position=(x0 - 1, y0 - 2))
left_leg.CreatePolygonFixture(box=(0.5, 3),
density=1,
friction=WHEEL_FRICTION)
world.CreateRevoluteJoint(bodyA=body,
bodyB=left_leg,
anchor=body.worldCenter + (-1, -2),
lowerAngle=-0.3 * pi,
upperAngle=0.3 * pi,
enableLimit=True,
maxMotorTorque=MAX_TORQUE,
motorSpeed=sin(SPEED),
enableMotor=True)
print(width, height)
self.bodies = [body, head, neck, right_leg, left_leg]
self.tracker = body.worldCenter
# return the body object for tracking position
return body
# border_body3 = world.CreateStaticBody(position=(0.59,0), shapes=border_shape_v)
# border_body4 = world.CreateStaticBody(position=(-0.6,0), shapes=border_shape_v)
# env = [env_body, border_body1, border_body2, border_body3, border_body4]
# env = [env_body]
env = []
# create dynamic bodies
snowflakes = []
for ind in range(len(data)):
row = data[ind]
circle = circleShape(pos=(0, 0), radius=SNOWBALL_RADIUS)
snowflake_body = world.CreateDynamicBody(position=row, userData=ind)
fixture = snowflake_body.CreateFixture(
fixtureDef(shape=circle, density=1, friction=0.3))
snowflakes.append(snowflake_body)
force_magnitude = 1
force_decrement = force_magnitude / (end_frame - start_frame - 1)
# force_decrement = 0
force_vec = vec2(0.0001, 0)
# main game loop
running = True
frame = start_frame
while running:
# Check the event queue
for event in pygame.event.get():
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=(3.2,1.0),
shapes=polygonShape(box=(3.0, .2)),
)
# Create a dynamic body
dynamic_body = world.CreateDynamicBody(position=(3.0,3.0), angle=45)
# And add a box fixture onto it (with a nonzero density, so it will move)
box = dynamic_body.CreatePolygonFixture(box=(.1, .1), density=9, friction=0.7, restitution=0.89)
colors = {
staticBody: (255, 255, 255, 255),
dynamicBody: (127, 127, 127, 255),
}
# --- main game loop ---
running = True
while running:
# Check the event queue
for event in pygame.event.get():
if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE):
i = 0
while callback.hit == False:
world.RayCast(callback, (i - 10, -10), (i - 10, SCREEN_HEIGHT / PPM))
i += 3
if callback.fixture.userData != 'outer':
print('clockwise track detected. Initiating corrections')
temp = outer_track
outer_track = inner_track
inner_track = temp
tracks.append([outer_track, 'white'])
tracks.append([inner_track, 'lightblue'])
#make a box for the mouse and move it around
car = world.CreateDynamicBody(position=(200 / PPM, 200 / PPM),
angle=0,
linearDamping=.3,
angularDamping=6)
car_color = 'blue'
box = car.CreatePolygonFixture(box=(1, .5), density=1, friction=0.002)
pygame.init()
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption('Drifter Evolver')
clock = pygame.time.Clock()
def step():
world.Step(1.0 / 60.0, 10, 10)
for event in pygame.event.get():
if event.type == QUIT:
(0 , 0)])
ground_body = world.CreateStaticBody(
position=(0, 0),
shapes=(polygonShape(box=(100, 5))),
)
quakeFloor = world.CreateKinematicBody(position=(1 , 6))
quakeFloor.CreateFixture(shape=Box2D.b2PolygonShape(vertices=[(0 , 0),
(0 , 1),
(40 , 1),
(40 , 0)])
,friction=4)
dynamic_body = world.CreateDynamicBody(position=(10, 15), angle=math.radians(90))
box = dynamic_body.CreatePolygonFixture(box=blockSize, density=1, friction=0.3)
colors = {
staticBody: (255, 255, 255, 255),
dynamicBody: (127, 127, 127, 255),
kinematicBody: (250 , 128 , 0 , 255)
}
maxForces = {}
font = pygame.font.Font(None , 24)
fontLarge = pygame.font.Font(None , 80)
###
def simulate(cmd, trj, na, color):
# Create dynamic bodies
des_body = world.CreateDynamicBody(position=(db_init[0], db_init[1]),
angle=db_init[2],
linearDamping=0.5 * 1 * 9.8,
angularDamping=0.3 * 1 / 12 * 9.8)
obs1_body = world.CreateDynamicBody(position=(o1_init[0], o1_init[1]),
angle=o1_init[2],
linearDamping=0.5 * 18 * 9.8,
angularDamping=0.3 * 4 / 12 * 9.8)
obs2_body = world.CreateDynamicBody(position=(o2_init[0], o2_init[1]),
angle=o2_init[2],
linearDamping=0.5 * 24 * 9.8,
angularDamping=0.3 * 4 / 12 * 9.8)
# Create Gripper
gripper = world.CreateKinematicBody(position=(18, 5), angle=0)
# Add polygon fixtures for objects
des_fixt = des_body.CreatePolygonFixture(box=(1, 1),
density=1,
friction=0.3,
restitution=0.8)
obs1_box = obs1_body.CreatePolygonFixture(box=(1.5, 3),
density=1,
friction=0.3,
restitution=0.8)
obs2_box = obs2_body.CreatePolygonFixture(box=(3, 2),
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)
# gripperbase = polygonShape(vertices=[(-w/2,0), (-w/2,l), (-ow/2,l),
# (-ow/2,l+lf), (-iw/2,l+lf), (-iw/2,l+lf-lt), (iw/2,l+lf-lt), (iw/2,l+lf), (ow/2,l+lf),
# (ow/2,l), (w/2,l), (w/2,0)])
gripperbase = gripper.CreatePolygonFixture(box=(w / 2, l / 2),
density=1,
friction=0.3)
gripperpalm = gripper.CreatePolygonFixture(box=(ow / 2, lt / 2,
vec2(0,
l / 2 + lt / 2), 0),
density=1,
friction=0.3)
gripperfngL = gripper.CreatePolygonFixture(
box=(wfng / 2, lfng / 2, vec2(-ow / 2 + wfng / 2,
l / 2 + lt + lfng / 2), 0),
density=1,
friction=0.3)
gripperfngR = gripper.CreatePolygonFixture(
box=(wfng / 2, lfng / 2, vec2(ow / 2 - wfng / 2,
l / 2 + lt + lfng / 2), 0),
density=1,
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_body: " + str(obs_body.mass) + " kg , " + str(obs_body.inertia) + " kg*m^2"
# white = (255, 255, 255, 255)
# print des_body.fixtures
colors = [(255, 50, 50, 255), (124, 252, 0, 0), (124, 252, 0, 0),
(50, 50, 255, 255), (50, 50, 255, 255), (255, 255, 255, 255),
(255, 255, 255, 255), (255, 255, 255, 255), (255, 255, 255, 255)]
bodies = [des_body, obs1_body, obs2_body, gripper]
if cmd != "fwd":
for i in range(color):
colors.insert(0, (124, 252, 0, 0))
if color == 1:
bodies.insert(0, na)
else:
for b in na:
bodies.insert(0, b)
print colors
print bodies
LQ = 1
# --- main game loop ---
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
hits = 0
cnt_reward = 0
# Cast a ray from the center of the fingers to the desired object
psi_ray = gripper.GetWorldPoint(gripper.localCenter + [0, l / 2 + lt])
x_ray = psi_ray[0]
y_ray = psi_ray[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()
# Check contact points on the gripper
for ce in gripper.contacts:
# print 'contact: ',ce.contact
if ce.contact.touching:
cntbody = ce.contact.fixtureA.body
if moveList.count(cntbody) == 0:
moveList.append(cntbody)
elif moveList_prev.count(cntbody) != 0:
if avoidList.count(cntbody) == 0:
avoidList.append(cntbody)
# raw_input()
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]
# print "vertices",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
gripper.linearVelocity = (trj[k, 0], trj[k, 1])
gripper.angularVelocity = trj[k, 2]
if hits > 0:
cnt_reward = 0
else:
cnt_reward = 1 * LQ
# print cnt_reward
# 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)
pygame.display.flip()
clock.tick(TARGET_FPS)
db = [des_body.worldCenter[0], des_body.worldCenter[1], des_body.angle]
o1 = [obs1_body.worldCenter[0], obs1_body.worldCenter[1], obs1_body.angle]
o2 = [obs2_body.worldCenter[0], obs2_body.worldCenter[1], obs2_body.angle]
for body in bodies:
world.DestroyBody(body)
return db, o1, o2
def createBlock(pos):
global blockRotation
dynamic_body = world.CreateDynamicBody(position=pos, angle=math.radians(blockRotation))
block = dynamic_body.CreatePolygonFixture(box=blockSize, density=1, friction=0.3)
return
# --- pybox2d world setup ---
# Create the world
world = world(gravity=(0, -10), doSleep=True, CollideConnected=False)
# And a static body to hold the ground shape
ground_body = world.CreateStaticBody(position=(0, 0),
shapes=polygonShape(box=(50, 1)))
ground_body = world.CreateStaticBody(position=(0, 24),
shapes=polygonShape(box=(1, 24)))
ground_body = world.CreateStaticBody(position=(1, 25),
shapes=polygonShape(box=(3, 1)))
# graund_body = world.CreateDynamicBody(position = (0,0),angle = 0)
# grauund = graund_body.CreatePolygonFixture(box = (50,1),density = 1, friction = 0.3)
# Create a couple dynamic bodies
body1 = world.CreateDynamicBody(position=(20, 1), linearVelocity=(0, 0))
# body.linearVelocity(-10)
circle = body1.CreateCircleFixture(radius=1.5, density=9999999, friction=0.3)
body2 = world.CreateDynamicBody(position=(20, 25), linearVelocity=(0, 5))
circle2 = body2.CreateCircleFixture(radius=0.5, density=1, friction=0.3)
# joint = world.CreateJoint(bodyA = body1,bodyB = body2, collideConnected = True, type = b2RevoluteJoint)
body = world.CreateDynamicBody(position=(20, 25),
angle=0,
linearVelocity=(0, 0))
box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
body = world.CreateDynamicBody(position=(60, 20),
angle=0,
TARGET_FPS = 60
PPM = 20
TIMESTEP = 1.0 / TARGET_FPS
SCREEN_WIDTH, SCREEN_HEIGHT = 640, 400
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption("aaaa")
clock = pygame.time.Clock()
world = world(gravity=(0, -10))
ground_body = world.CreateStaticBody(position=(0, 0),
shapes=polygonShape(box=(50, 1)))
for x in range(5, 30, 5):
body = world.CreateDynamicBody(position=(x, 20))
circle = body.CreateCircleFixture(radius=1, density=1, friction=0.3)
for x in range(5, 40, 4):
body = world.CreateDynamicBody(position=(x, 30), angle=x)
box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
colors = {staticBody: (255, 255, 255, 255), dynamicBody: (127, 127, 127, 255)}
def my_draw_polygon(polygon, body, fixture):
vertices = [(body.transform * v) * PPM for v in polygon.vertices]
vertices = [(v[0], SCREEN_HEIGHT - v[1]) for v in vertices]
pygame.draw.polygon(screen, colors[body.type], vertices)
polygonShape.draw = my_draw_polygon
def simulate(cmd, trj):
import pygame
from pygame.locals import (QUIT, KEYDOWN, K_ESCAPE)
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)
import numpy as np
import copy
# --- constants ---
# Box2D deals with meters, but we want to display pixels,
# so define a conversion factor:
PPM = 20.0 # pixels per meter
TARGET_FPS = 60
TIME_STEP = 1.0 / TARGET_FPS
SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480
# --- cost function constants ---
C_fng = 1
C_des = 1
C_obs = 1
C_cnt = 1
# --- 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)
# And 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 * 1 * 9.8,
angularDamping=0.3 * 1 / 12 * 9.8)
obs_body = world.CreateDynamicBody(position=(18, 12),
angle=0,
linearDamping=0.5 * 4 * 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)
obs_box = obs_body.CreatePolygonFixture(box=(2, 2),
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_body: " + str(obs_body.mass) + " kg , " + str(obs_body.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), (255, 255, 255, 255),
(255, 255, 255, 255)]
bodies = [ground_body, des_body, obs_body, fng1, fng2]
# LOAD NAIVE VELOCITY PROFILE: generated in matlab
if cmd == "naive":
v_prof = np.loadtxt('examples/vel_prof1.txt', delimiter=";")
v_x = v_prof[:, 0]
v_y = v_prof[:, 1]
# print v_prof
# v_prof = np.array(v_prof, dtype='f')
else:
v_x = np.gradient(trj[:, 0]) / TARGET_FPS
v_y = np.gradient(trj[:, 1]) / TARGET_FPS
# INITIALIZE THE COST
f_x = 0
pos_des_prev = copy.copy(des_body.worldCenter)
pos_obs_prev = copy.copy(obs_body.worldCenter)
LQ = 1
# print "initial pos: (" + str(pos_des_prev[0]) + ", " + str(pos_des_prev[1]) + ")"
# --- main game loop ---
# while True:
for k in range(0, 181):
# 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
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]
pygame.draw.polygon(screen, colors[i], vertices)
i = i + 1
# print i
vd = vec2(v_x[k], v_y[k])
fng1.linearVelocity = vd
fng2.linearVelocity = vd
# print fng1.linearVelocity
# print fng2.linearVelocity
# Collect data from these bodies
pos_des = des_body.worldCenter
pos_obs = obs_body.worldCenter
# Calculate the difference
d_des = np.linalg.norm(pos_des - pos_des_prev)
d_obs = np.linalg.norm(pos_obs - pos_obs_prev)
# print d_des
KE_des = KE(des_body)
KE_obs = KE(obs_body)
# print KE_des
# print KE_obs
# 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
# Determine the kinetic energy of the fingers
KE_fng1 = KE_fng(fng1)
KE_fng2 = KE_fng(fng2)
# print KE_fng1 + KE_fng2
# Integrate the Cost function
f_x = C_fng * KE_fng1 + C_fng * KE_fng2 + C_des * KE_des + C_obs * KE_obs - C_cnt * cnt_reward + f_x
# print f_x
# Update the previous position
pos_des_prev = copy.copy(pos_des)
pos_obs_prev = copy.copy(pos_obs)
# 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!')
# RETURN THINGS
# x_k = v_prof[:,0]
# y_k = v_prof[:,1]
# return x_k # the velocity trajectory x_k
# return y_k # the velocity trajectory y_k
return (v_x, v_y, f_x) # the velocity profile, the total cost
def create_polygons():
for x in range(5, 40, 4):
"""Создаем прямоугольники"""
body = world.CreateDynamicBody(position=(x, 30), angle=x)
box = body.CreatePolygonFixture(box=(2, 1), density=1, friction=0.3)
def PostSolve(self, contact, impulse):
pass
# construct world
world = world(gravity=(0, 0),
doSleep=True,
contactListener=myContactListener())
# add balloons
balloon1_shape = polygonShape(vertices=balloon1)
balloon1_knot_shape = polygonShape(vertices=balloon1_knot)
balloon2_shape = polygonShape(vertices=balloon2)
balloon2_knot_shape = polygonShape(vertices=balloon2_knot)
balloon1_body = world.CreateDynamicBody(position=(0, 0))
fixture = balloon1_body.CreateFixture(
fixtureDef(shape=balloon1_shape, density=1, friction=0.3))
balloon1_knot_body = world.CreateDynamicBody(position=(0, 0))
fixture = balloon1_knot_body.CreateFixture(
fixtureDef(shape=balloon1_knot_shape, density=1, friction=0.3))
balloon2_body = world.CreateDynamicBody(position=(0, 0))
fixture = balloon2_body.CreateFixture(
fixtureDef(shape=balloon2_shape, density=1, friction=0.3))
balloon2_knot_body = world.CreateDynamicBody(position=(0, 0))
fixture = balloon2_knot_body.CreateFixture(
fixtureDef(shape=balloon2_knot_shape, density=1, friction=0.3))
# weld joints
world.CreateWeldJoint(bodyA=balloon1_body,
