def step():
world.Step(1.0 / 60.0, 10, 10)
for event in pygame.event.get():
if event.type == QUIT:
pygame.quit()
sys.exit()
quit()
#change car color
car_color = 'blue'
for contact in car.contacts:
if car.contacts[0].contact.touching:
car_color = 'red'
# draw background, walls, car
screen.fill(THECOLORS['gray'])
for track in tracks:
for fixture in track[0].fixtures:
shape = fixture.shape
pygame.draw.polygon(screen, THECOLORS[track[1]],
tfm(shape.vertices))
car.position = (pygame.mouse.get_pos()[0] / PPM,
pygame.mouse.get_pos()[1] / PPM)
shape = car.fixtures[0].shape
vertices = [(car.transform * v) * PPM for v in shape.vertices]
pygame.draw.polygon(screen, THECOLORS[car_color], vertices)
pygame.display.flip()
clock.tick(60.0)
def start_balls():
global run
fon()
while run:
fon()
time_delta = clock.tick(SPEED) / 1000.0
for body in world.bodies:
for fixture in body.fixtures:
fixture.shape.draw(body, fixture)
world.Step(TIME_STEP, 10, 10)
for event in pg.event.get():
if event.type == pg.QUIT:
run = False
if event.type == pg.MOUSEBUTTONUP:
if event.button == 1:
create_circle()
elif event.button == 3:
create_polygons()
if event.type == pg.KEYDOWN:
if event.key == pg.K_DOWN:
pass
if event.key == pg.K_UP:
pass
pg.display.flip() # Обновление кадра
# output_body(balloon1_knot_body, start_frame, frame, 1)
# output_chain(chain1, frame, 2)
# output_body(balloon2_body, start_frame, frame, 3)
# output_body(balloon2_knot_body, start_frame, frame, 4)
# output_chain(chain2, frame, 5)
for ind in range(len(chains)):
chain = chains[ind]
output_chain(chain, frame, ind)
# 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)
world.ClearForces()
print("time stepped")
# Flip the screen and try to keep at the target FPS
pygame.display.flip()
clock.tick(TARGET_FPS)
if frame == end_frame:
break
if period_counter == PERIOD:
PERIOD = math.ceil(random.random() * 5 + 20)
period_counter = 0
rand = [random.random(), random.random()]
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
elif event.key == pygame.K_s:
controlState |= TDC_DOWN
elif event.type == pygame.KEYUP:
if event.key == pygame.K_a:
controlState &= ~TDC_LEFT
elif event.key == pygame.K_d:
controlState &= ~TDC_RIGHT
elif event.key == pygame.K_w:
controlState &= ~TDC_UP
breaking = False
elif event.key == pygame.K_s:
controlState &= ~TDC_DOWN
breaking = False
Step(car, controlState)
world.Step(diff, 10, 10)
position = car.m_body.worldCenter
offset = [-position.x * PPM, position.y * PPM]
offset[0] += size[0] / 2
offset[1] += size[1] / 2 - 13
topLeft = [0, 0]
topLeft[0] = int(position.x / TILE_SIZE - 15)
topLeft[1] = int(-position.y / TILE_SIZE - 15)
for x in xrange(topLeft[0], topLeft[0] + 30):
for y in xrange(topLeft[1], topLeft[1] + 27):
if (y < 0 or x < 0 or len(grid) <= y or len(grid[y]) <= x):
continue
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
east_doorways=[doorways[4]], name='r 5')
building = Building(rooms=[room1, room2, room3, room4, room5],
doorways=doorways)
num_agents = 30
# agents = [AgentBody(world, x,y) for x,y in [(random.randint(1, 19), random.randint(1, 19)) for i in range(num_agents)]]
agents = [agent.Agent(world, building, type='leader', spawn_room=room4) for i in range(num_agents)]
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
building.draw(screen, render_settings)
for agent in agents:
agent.draw(screen, render_settings)
agent.go_to_exit()
# Make Box2D simulate the physics of our world for one step.
world.Step(render_settings['TIME_STEP'], 10, 10)
# Flip the screen and try to keep at the target FPS
pygame.display.flip()
clock.tick(render_settings['TARGET_FPS'])
pygame.quit()
print('Done!')
# 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 ('body.transform: ', body.transform)
print ('shape.vertices: ', shape.vertices)
print ('vertices in pixels: ', 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[body.type], vertices)
#running=False
# 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, 1000, 1000)
# Flip the screen and try to keep at the target FPS
pygame.display.flip()
clock.tick(TARGET_FPS)
pygame.quit()
print('Done!')
posb = slingshot.rect.centerx, slingshot.rect.y
posb = posb[0] / PPM, (600 - posb[1]) / PPM
length = (((posb[0] - posa[0])**2 + (posb[1] - posa[1])**2)**(1 /
2))
if length <= 1.0:
in_sling.body.transform = (posa, in_sling.body.angle)
else:
reduct = 1.0 / length
move = (posb[0] - (posb[0] - posa[0]) * reduct,
posb[1] - (posb[1] - posa[1]) * reduct)
in_sling.body.transform = (move, in_sling.body.angle)
else:
in_sling.body.awake = False
world.Step(TIME_STEP, 10, 10) #always do before drawing!!
if art:
screen.fill((147, 211, 246))
screen.blit(background_art, (0, 0))
slingshot.draw(screen)
draw_sling(SLING_COLOR, slingshot)
for each in things:
if each != slingshot:
each.draw(screen)
elif not art:
screen.fill((0, 0, 0))
for each in things:
each.draw_shape(screen)
draw_sling(WHITE, slingshot)
pygame.display.flip()
# open or resize window (This function is valid only on PC,Ignored in smartphone apps)
core.window(True, 480, 640)
cam = core.camera("maincam")
cam.orthographicProjection = True
cam.position = vmath.vec3(0.0, 0.0, 100.0)
showcase = core.showcase()
boxes = []
world = world(gravity=(0, -10), doSleep=True)
boxes.append(DynamicBox(world, showcase, (0, -100), (150, 20), 0, True))
boxes.append(DynamicBox(world, showcase, (10, 100), (10, 5), 15))
while True:
core.update()
touch = core.singleTouch()
if touch is not None and touch['is_pressed']:
boxes.append(
DynamicBox(world, showcase, (touch['cur_x'], touch['cur_y']),
(10, 5), 15))
world.Step(core.getElapsedTime(), 10, 10)
for box in boxes:
box.update()
core.update()
cam.shoot(showcase)
core.swap()
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 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
