def __init__(self, img, pos, world, scale=1):
img = pygame.transform.rotozoom(img, 0, scale)
self.img = img
self.rect = img.get_rect(center=(pos[0] * PPM, 600 - pos[1] * PPM))
anchora = ((self.rect.topleft[0] + 10) / PPM,
(600 - self.rect.topleft[1] - 10) / PPM)
anchorb = ((self.rect.topright[0] - 10) / PPM,
(600 - self.rect.topright[1] - 10) / PPM)
self.anchora = world.CreateStaticBody(position=(anchora), angle=0)
self.anchorb = world.CreateStaticBody(position=(anchorb), angle=0)
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, all_sprites):
"""Инициализация объекта"""
super().__init__(all_sprites)
ground_body = world.CreateStaticBody(position=(0, 0),
shapes=polygonShape(box=(30, 5)))
self.image = load_image("ground.jpg")
self.rect = pygame.Rect(0, 700, 600, 100)
self.mask = pygame.mask.from_surface(self.image)
def PG(ph, a, d):
theta = ph[2]
phi = math.pi / 2 - theta
neg_area = world.CreateStaticBody(position=(ph[0] + 0.5 * d * cos(theta),
ph[1] + 0.5 * d * sin(theta)),
angle=theta)
na_fixt = neg_area.CreatePolygonFixture(box=(d, a), density=1, friction=1)
shape = na_fixt.shape
vertices = [(neg_area.transform * v) * PPM for v in shape.vertices]
return vertices
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,
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, vertices, inner_corners, color, position, world):
# use box2d edge
self.uuid = uuid1()
self.vertices = vertices
self.color = color
self.position = position
self.world = world
self.edge_fixtures = []
self.edge_bodies = []
for index, corner in enumerate(inner_corners):
next_corner = inner_corners[(index + 1) % len(inner_corners)]
fixture = edgeShape(vertices=[corner, next_corner * 2 - corner])
self.edge_fixtures.append(fixture)
body = world.CreateStaticBody(shapes=fixture, position=corner)
body.userData = self
self.edge_bodies.append(body)
def __init__(self,
color,
vertices,
world_map=None,
mass=1.0,
density=0.050,
friction=0.3,
position=(0.0, 0.0),
angle=0.0,
momentum_vector=np.array([0, 0], dtype=float),
angular_velocity=0.0,
dynamic=True):
self.uuid = uuid1()
self.color = color
self.vertices = [np.array(vertex, dtype=float) for vertex in vertices]
self.edges = []
for index, vertex in enumerate(self.vertices):
self.edges.append([
vertex,
self.vertices[(index + 1) % len(self.vertices)] - vertex
])
self.momentum_vector = momentum_vector # x,y, direction
self.world_map = world_map
self.world = world_map.world
self.body = None
self.fixture = None
self.mass = mass
self.friction = friction
self.position = position
if world_map is not None:
if dynamic is True:
self.body = self.world.CreateDynamicBody(position=position,
angle=angle)
self.fixture = self.body.CreatePolygonFixture(
vertices=vertices, density=density, friction=friction)
else:
self.body = world.CreateStaticBody(position=position,
angle=angle)
self.body.userData = self
self.body.linearVelocity += momentum_vector / mass
self.body.angularVelocity += angular_velocity
world_map.add_physical(self)
def build(self, world):
# is it an error to call terrain.build() before terrain.generate()?
# raise an exception, to make this easier to detect.
# Or, terrain.build() could call terrain.generate() if terrain.generate()
# has not been called; this makes its API easier to use, and the code
# less fragile
if not self.generated:
return None
start = 0
self.bodies = []
for i in range(self.n_segments):
body = world.CreateStaticBody(
position=self.seg_positions[i],
angle=self.seg_angles[i],
shapes=polygonShape(box=(self.seg_lengths[i] / 2., .5)))
start += self.seg_lengths[i]
self.bodies.append(body)
return self.length
def move(pi, pf, T):
# Calculate trajectory
T = int(T)
trj_x = TARGET_FPS * (pf[0] - pi[0]) / T * np.matrix(np.ones((T, 1)))
trj_y = TARGET_FPS * (pf[1] - pi[1]) / T * np.matrix(np.ones((T, 1)))
trj_th = TARGET_FPS * (pf[2] - pi[2]) / T * np.matrix(np.ones((T, 1)))
psi = np.hstack((trj_x, trj_y, trj_th))
# Calculate negative area
th = atan2(pf[1] - pi[1], pf[0] - pi[0])
d_tot = dist(pf[0:2], pi[0:2])
xc = pi[0] + d_tot * cos(th)
yc = pi[1] + d_tot * sin(th)
neg_area = world.CreateStaticBody(position=(xc, yc), angle=th)
neg_area.active = False
na_fixt = neg_area.CreatePolygonFixture(box=(d_tot, a),
density=1,
friction=1)
# color = (124,252,0,0)
color = 1
return neg_area, color, psi
def __init__(self,
world,
start_point,
end_point,
width=0.5,
color=(100, 100, 100, 255)):
self.color = color
length = math.sqrt((end_point[0] - start_point[0])**2 +
(end_point[1] - start_point[1])**2)
angle = math.atan2(end_point[1] - start_point[1],
end_point[0] - start_point[0])
self.body = world.CreateStaticBody(
position=((start_point[0] + end_point[0]) / 2,
(start_point[1] + end_point[1]) / 2),
angle=angle)
# self.body = world.CreateDynamicBody(position=((start_point[0]+end_point[0])/2, (start_point[1]+end_point[1])/2), angle=angle)
self.fixture = self.body.CreatePolygonFixture(box=(length / 2,
width / 2),
density=1,
friction=0.3)
self.box = self.fixture.shape
def PG(ph, a, d, theta0):
# Calculate Vertices of Negative Are
theta = theta0
print theta
neg_area = world.CreateStaticBody(position=(ph[0] + 0.5 * d * cos(theta),
ph[1] + 0.5 * d * sin(theta)),
angle=theta)
neg_area.active = False
na_fixt = neg_area.CreatePolygonFixture(box=(d, a), density=1, friction=1)
# shape = na_fixt.shape
# vertices = [(neg_area.transform * v) * PPM for v in shape.vertices]
# color = [(124,252,0,0)]
color = 1
# Calculate Push motion
v = d / (N - Nt)
trj_x = v * cos(theta) * np.matrix(np.ones((N - Nt, 1)))
trj_x = TARGET_FPS * np.vstack((np.zeros((Nt, 1)), trj_x))
trj_y = v * sin(theta) * np.matrix(np.ones((N - Nt, 1)))
trj_y = TARGET_FPS * np.vstack((np.zeros((Nt, 1)), trj_y))
trj_th = float(theta0 - init_pos[2]) / (Nt) * np.matrix(np.ones((Nt, 1)))
trj_th = TARGET_FPS * np.vstack((trj_th, np.zeros((N - Nt, 1))))
psi_naive = np.hstack((trj_x, trj_y, trj_th))
return neg_area, color, psi_naive
screen = pg.display.set_mode(size)
pg.display.update()
run = True # переменна, с помощью ее можно выходить из цикла
"""А вот тут будет волшебство)"""
clock = pg.time.Clock()
RED = (255, 0, 0)
BLACK = (0, 0, 0)
world = world(gravity=(0, -10)) # создание поля
ground_body = world.CreateStaticBody(
position=(0, 10),
shapes=polygonShape(box=(20, 0.5)),
)
ground_body2 = world.CreateStaticBody(
position=(37, 10),
shapes=polygonShape(box=(10, 0.5)),
)
ground_body3 = world.CreateStaticBody(position=(0, 0),
shapes=polygonShape(box=(50, 1)))
body = world.CreateStaticBody(position=(20, 15))
circle = body.CreateCircleFixture(radius=1, density=1, friction=0.3)
def create_circle():
for i in range(5, 40, 5):
start_frame = int(sys.argv[1])
end_frame = int(sys.argv[2])
# construct world
world = world(gravity=(0, 0), doSleep=True)
# 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)
ground_body = world.CreateStaticBody(position=(0, -0.4), shapes=border_shape_h)
border_body3 = world.CreateStaticBody(position=(0.49, 0),
shapes=border_shape_v)
border_body4 = world.CreateStaticBody(position=(-0.5, 0),
shapes=border_shape_v)
env = [border_body1, ground_body, border_body3, border_body4]
# load strokes
chains = []
static_chains = []
dynamic_chains = []
TARGET_FPS = 60
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)),
)
agent = AgentBody(world)
agents = [agent]
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):
# The user closed the window or pressed escape
running = False
screen.fill((0, 0, 0, 0))
# Draw the world
# for body in world.bodies:
TARGET_FPS = 60
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=(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
pygame.init()
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption('GraviTask')
clock = pygame.time.Clock()
world = world(gravity=(0, -10), doSleep=True)
ground_shape = Box2D.b2PolygonShape(vertices = [(0 , 10),
(2 , 1),
(98 , 1),
(100 , 10),
(100 , 0),
(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 = {
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
def test(func, godisp, seed):
startrun = 0
from Box2D.b2 import (world, polygonShape, circleShape, staticBody,
dynamicBody)
if godisp:
PPM = 20.0 # pixels per meter
TARGET_FPS = 60
TIME_STEP = 1.0 / TARGET_FPS
else:
TIME_STEP = 1.0 / 60
SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480
# --- pygame setup ---
if godisp:
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, -20), doSleep=True)
# And a static body to hold the ground shape
ground_body = world.CreateStaticBody(
position=(0, 0),
shapes=polygonShape(box=(50, 1)),
)
top = world.CreateStaticBody(position=(0, 24),
shapes=polygonShape(box=(50, 1)))
side_body = world.CreateStaticBody(
position=(0, 0),
shapes=polygonShape(box=(1, 50)),
)
other_side = world.CreateStaticBody(
position=(32, 0),
shapes=polygonShape(box=(1, 50)),
)
# Create a couple dynamic bodies
body = world.CreateDynamicBody(position=(seed[2], seed[3]))
ball = body.CreateCircleFixture(radius=0.5,
density=.2,
friction=0,
restitution=1)
body = world.CreateDynamicBody(position=(15.5, 0), angle=0)
slider = body.CreatePolygonFixture(box=(3, .4), 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.
if godisp:
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
if godisp:
def my_draw_circle(circle, body, fixture):
position = body.transform * circle.pos * PPM
position = (position[0], SCREEN_HEIGHT - position[1])
pygame.draw.circle(screen, colors[body.type],
[int(x) for x in position],
int(circle.radius * PPM))
# Note: Python 3.x will enforce that pygame get the integers it requests,
# and it will not convert from float.
circleShape.draw = my_draw_circle
# --- main game loop ---
world.bodies[4].linearVelocity = (seed[0], seed[1])
running = True
while running:
startrun += 1
movement = 0
movement = func([
world.bodies[5].position[0], world.bodies[4].linearVelocity,
world.bodies[4].position
]) * 28
# Check the event queue
if godisp:
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_LEFT:
movement -= 1
if event.key == pygame.K_RIGHT:
movement += 1
if event.type == QUIT or (event.type == KEYDOWN
and event.key == K_ESCAPE):
running = False
pos = world.bodies[5].position
# print(pos)
world.bodies[5].position = Box2D.b2Vec2(float(movement), pos[1])
if godisp:
screen.fill((0, 0, 0, 0))
if world.bodies[4].position[1] <= 1.8:
running = False
if godisp:
# Draw the world
for body in world.bodies:
for fixture in body.fixtures:
fixture.shape.draw(body, fixture)
# Make Box2D simulate the physics of our world for one step.
world.Step(TIME_STEP, 10, 10)
# Flip the screen and try to keep at the target FPS
if godisp:
pygame.display.flip()
clock.tick(TARGET_FPS)
if startrun > 5000:
return startrun
if godisp:
pygame.quit()
return startrun
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
