def build_rays(self, rays):
ret = []
for i in rays:
input = b2.rayCastInput(p1=self.body.transform * i,
p2=self.body.worldCenter,
maxFraction=1)
output = b2.rayCastOutput()
self.body.fixtures[0].RayCast(output, input, 0)
ret.append((self.body.localCenter + i * (1. - output.fraction), i))
return ret
def get_boundary_intersection(self, p1, p2):
input = b2.rayCastInput(p1=p1, p2=p2, maxFraction=1)
output = b2.rayCastOutput()
closest_fraction = None
closest_point = None
for i in self.boundary.fixtures:
if i.RayCast(output, input, 0):
hit_point = input.p1 + output.fraction * (input.p2 - input.p1)
if closest_point is None or closest_fraction > output.fraction:
closest_point = hit_point
closest_fraction = output.fraction
return closest_point, closest_fraction
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 simulate(cmd, trj):
import Box2D # The main library
# Box2D.b2 maps Box2D.b2Vec2 to vec2 (and so on)
from Box2D.b2 import (world, polygonShape, circleShape, staticBody,
dynamicBody, vec2)
# --- constants ---
# Box2D deals with meters, but we want to display pixels,
# so define a conversion factor:
PPM = 20.0 # pixels per meter
TIME_STEP = 1.0 / TARGET_FPS
SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480
# --- pygame setup ---
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
pygame.display.set_caption('Simple pygame example')
clock = pygame.time.Clock()
# --- pybox2d world setup ---
# Create the world
world = world(gravity=(0, 0), doSleep=True)
# Add a static body to hold the ground shape
ground_body = world.CreateStaticBody(
position=(0, 1),
shapes=polygonShape(box=(50, 1)),
)
# Create dynamic bodies
des_body = world.CreateDynamicBody(position=(15, 12),
angle=0,
linearDamping=0.5 * 4 * 9.8,
angularDamping=0.3 * 1 / 12 * 9.8)
obs1_body = world.CreateDynamicBody(position=(17, 9),
angle=30,
linearDamping=0.5 * 18 * 9.8,
angularDamping=0.3 * 4 / 12 * 9.8)
obs2_body = world.CreateDynamicBody(position=(11, 11),
angle=0,
linearDamping=0.5 * 24 * 9.8,
angularDamping=0.3 * 4 / 12 * 9.8)
# Create fingers as kinematic bodies (infinite masses and directly controls velocity)
width = 2.5
fng1 = world.CreateKinematicBody(position=(16, 5), angle=0)
fng2 = world.CreateKinematicBody(position=(16 + width, 5), angle=0)
# And add box fixtures onto it (with a nonzero density, so it will move)
des_box = des_body.CreatePolygonFixture(box=(1, 1),
density=1,
friction=0.3,
restitution=0.8)
obs1_box = obs1_body.CreatePolygonFixture(box=(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)
printflag = True
# Model fingers as small circular cross sections
# circle = circleShape(radius=0.1)
fng1_cir = fng1.CreatePolygonFixture(box=(0.1, 0.1),
density=5,
friction=0.3)
fng2_cir = fng2.CreatePolygonFixture(box=(0.1, 0.1),
density=5,
friction=0.3)
# Mass and Moment of Inertia data
# print "des_body: " + str(des_body.mass) + " kg , " + str(des_body.inertia) + " kg*m^2"
# print "obs_body1: " + str(obs_body1.mass) + " kg , " + str(obs_body1.inertia) + " kg*m^2"
# print fng1.linearVelocity
colors = [(255, 255, 255, 255), (255, 50, 50, 255), (124, 252, 0),
(124, 252, 0), (50, 50, 255, 255), (50, 50, 255, 255),
(255, 255, 255, 255), (255, 255, 255, 255)]
bodies = [ground_body, des_body, obs1_body, obs2_body, fng1, fng2]
# LOAD DESIRED VELOCITY PROFILE
if cmd == 'naive':
v_prof = np.loadtxt('examples/vel_prof1.txt', delimiter=';')
v_x = v_prof[:, 0]
v_y = v_prof[:, 1]
psi_prof = np.loadtxt('examples/pos_prof1.txt', delimiter=';')
xfng = np.reshape(np.matrix(psi_prof[:, 0]), (N, 1))
yfng = np.reshape(np.matrix(psi_prof[:, 1]), (N, 1))
# print xfng
# print v_y/TARGET_FPS
else:
v_prof = np.gradient(trj, axis=0) * TARGET_FPS
v_x = v_prof[:, 0]
v_y = v_prof[:, 1]
xfng = trj[:, 0]
yfng = trj[:, 1]
# print 'something else', v_x
# GATHER ACTUAL FNG POSITIONS
# xfng = np.zeros((N, 1))
# yfng = np.zeros((N, 1))
# INTIALIZE THE COST FUNCTION
fx = 0
fy = 0
LQ = 1
xdes = np.zeros((N, 1))
ydes = np.zeros((N, 1))
# --- 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
hits = 0
cnt_reward = 0
# Cast a ray from the center of the fingers to the desired object
x_ray = fng1.worldCenter[0] + width / 2
y_ray = fng1.worldCenter[1]
x_db = des_body.worldCenter[0]
y_db = des_body.worldCenter[1]
input = rayCastInput(p1=(x_ray, y_ray), p2=(x_db, y_db), maxFraction=1)
output = rayCastOutput()
for body in bodies: # or: world.bodies
# The body gives us the position and angle of its shapes
for fixture in body.fixtures:
# The fixture holds information like density and friction,
# and also the shape.
shape = fixture.shape
# Naively assume that this is a polygon shape. (not good normally!)
# We take the body's transform and multiply it with each
# vertex, and then convert from meters to pixels with the scale
# factor.
vertices = [(body.transform * v) * PPM for v in shape.vertices]
# But wait! It's upside-down! Pygame and Box2D orient their
# axes in different ways. Box2D is just like how you learned
# in high school, with positive x and y directions going
# right and up. Pygame, on the other hand, increases in the
# right and downward directions. This means we must flip
# the y components.
vertices = [(v[0], SCREEN_HEIGHT - v[1]) for v in vertices]
if body != des_body:
hit = shape.RayCast(output, input, body.transform, 0)
if hit:
hits = 1 + hits
pygame.draw.polygon(screen, colors[i], vertices)
i = i + 1
# print i
vd = vec2((float)(v_x[k]), (float)(v_y[k]))
fng1.linearVelocity = vd
fng2.linearVelocity = vd
# print fng1.linearVelocity
# print fng2.linearVelocity
if hits > 0:
cnt_reward = 0
else:
cnt_reward = 1 * LQ
print cnt_reward
# Collect data from these bodies
KEx_des, KEy_des = KE(des_body)
KEx_obs1, KEy_obs1 = KE(obs1_body)
KEx_obs2, KEy_obs2 = KE(obs2_body)
KEx_obs = KEx_obs1 + KEx_obs2
KEy_obs = KEy_obs1 + KEy_obs2
psi_des = des_body.GetWorldPoint(des_body.localCenter +
[-width / 2, 0])
xdes[k] = psi_des[0]
ydes[k] = psi_des[1]
# xdes[k] = 13.75 # (CONSTANT)
# ydes[k] = 12 # (CONSTANT)12
# Collect data from the fingers
KEx_fng1, KEy_fng1 = KE_fng(fng1, mfng)
KEx_fng2, KEy_fng2 = KE_fng(fng2, mfng)
# xfng[k] = fng1.worldCenter[0]
# yfng[k] = fng1.worldCenter[1]
# # Check contacts
# cnt_reward = 0
# for c in des_body.contacts:
# # if printflag:
# # print c.contact.touching
# # printflag = False
# if c.contact.touching :
# # print "sensor triggered"
# cnt_reward = 0
# else:
# # print "contacts points are free"
# cnt_reward = 1*LQ
# Integrate the Cost function
# print cnt_reward
fx = C1 * KEx_fng1 + C2 * mfng * np.abs(
xfng[k] -
xdes[k])**2 + C3 * KEx_des + C4 * KEx_obs - C5 * cnt_reward + fx
fy = C1 * KEy_fng1 + C2 * mfng * np.abs(
yfng[k] -
ydes[k])**2 + C3 * KEy_des + C4 * KEy_obs - C5 * cnt_reward + fy
# print "KEx: " + str(KEx_fng1) + ", KEy: " + str(KEy_fng1)
# Make Box2D simulate the physics of our world for one step.
# Instruct the world to perform a single step of simulation. It is
# generally best to keep the time step and iterations fixed.
# See the manual (Section "Simulating the World") for further discussion
# on these parameters and their implications.
world.Step(TIME_STEP, 10, 10)
# Flip the screen and try to keep at the target FPS
if cmd == "naive" or cmd == "show":
pygame.display.flip()
clock.tick(TARGET_FPS)
else:
pass
pygame.quit()
print('Simulation Done!')
# print 'xdes', xdes
# print 'xfng', xfng
return xfng, yfng, xdes, ydes, fx, fy
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
cnt_reward = 0
# Cast a ray from the center of the fingers to the desired object
x_ray = fng1.worldCenter[0] + width / 2
y_ray = fng1.worldCenter[1]
x_db = des_body.worldCenter[0]
y_db = des_body.worldCenter[1]
input = rayCastInput(p1=(x_ray, y_ray), p2=(x_db, y_db), maxFraction=1)
output = rayCastOutput()
hits = 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]