def __init__(self, n=2, radius=0.2, frontIR=12, debug=False):
global bDebug
bDebug = debug
print "-------------------------------------------------"
self.yini = -1.2
self.radius = radius
world.gravity = Box2D.b2Vec2(0, -1.01)
self.epucks = [
Epuck(position=[-1 + 2 * i, self.yini],
frontIR=frontIR,
bHorizontal=True) for i in range(n)
]
for e in self.epucks:
e.userData["score"] = 0
e.userData["reward"] = 0
self.walls_dx = 7.25
addWalls((0, 0), dx=self.walls_dx, dh=2, bHoriz=False)
createBox((0, -2), w=7.65, h=0.2, bDynamic=False, name="floor")
self.callback = RayCastCallback()
self.objs = []
self.box = None
# self.setOcclusion()
self.phase_names = ["Training", "Easy", "Intermmediate", "Difficult"]
def addWalls(pos, dx=3, dh=0, h=2.8, th=0, bHoriz=True, bVert=True, damping=0):
""" Also defined locally in ExpSetupDualCartPole!!! """
x, y = pos
wl = 0.2
yh = (5 + 1) / 2.0
if (bHoriz):
createBox((x, y - 1 - dh + th),
w=h + dh + wl + th,
h=wl,
bDynamic=False,
damping=damping,
name="wall_top")
createBox((x, y + 5 + dh + th),
w=h + dh + wl + th,
h=wl,
bDynamic=False,
damping=damping,
name="wall_bottom")
if (bVert):
createBox((x - dx - wl, y + yh - 1 + dh / 2 + th),
w=wl,
h=h + dh,
bDynamic=False,
damping=damping,
friction=0,
name="wall_left")
createBox((x + dx + wl, y + yh - 1 + dh / 2 + th),
w=wl,
h=h + dh,
bDynamic=False,
damping=damping,
friction=0,
name="wall_right")
def addWalls(pos, dx=3, dh=0, h=2.8, th=0, bHoriz=True, bVert=True, damping=0):
x, y = pos
wl = 0.2
yh = (5 + 1) / 2.0
if (bHoriz):
createBox((x, y - 1 - dh + th),
w=h + dh + wl + th,
h=wl,
bDynamic=False,
damping=damping,
name="wall_top")
createBox((x, y + 5 + dh + th),
w=h + dh + wl + th,
h=wl,
bDynamic=False,
damping=damping,
name="wall_bottom")
if (bVert):
createBox((x - dx - wl, y + yh - 1 + dh / 2 + th),
w=wl,
h=h + dh,
bDynamic=False,
damping=damping,
name="wall_left")
createBox((x + dx + wl, y + yh - 1 + dh / 2 + th),
w=wl,
h=h + dh,
bDynamic=False,
damping=damping,
name="wall_right")
def addWalls(pos, dx=3, dh=0, h=2.8, th=0, bHoriz=True, bVert=True):
x, y = pos
wl = 0.2
yh = (5 + 1) / 2.0
if (bVert):
# createBox((x, y - 1 - dh + th), w=h + dh + wl + th, h=wl, bDynamic=False, name="wall_top")
createBox((x, y - 0.95 - dh + th),
w=h + dh + wl + th + 0.8,
h=wl,
bDynamic=False,
name="wall_bottom") # horizontal abajo
# createBox((x, y + 5 + dh + th), w=h + dh + wl + th, h=wl, bDynamic=False, name="wall_bottom")
createBox((x, y + 6.05 + dh + th),
w=h + dh + wl + th + 0.8,
h=wl,
bDynamic=False,
name="xwall_top") # horizontal arriba
if (bHoriz):
# createBox((x - dx - wl, y + yh - 1 + dh / 2 + th), w=wl, h=h + dh, bDynamic=False, name="wall_left")
createBox((x - dx - wl - 0.1, y + yh - 0.85 + dh / 2 + th),
w=wl,
h=h + 0.3 + dh,
bDynamic=False,
name="wall_left") # vertical izq
# createBox((x + dx + wl, y + yh - 1 + dh / 2 + th), w=wl, h=h + dh, bDynamic=False, name="wall_right")
createBox((x + dx + wl + 0.1, y + yh - 0.85 + dh / 2 + th),
w=wl,
h=h + 0.3 + dh,
bDynamic=False,
name="wall_right") # vertical der
def __init__(self, position=(0, 0), name="simple", d=1, bHand=0, collisionGroup=None):
"""Init using IR class."""
global bDebug
self.name = name
self.ini_pos = position
self.salientMode = "all"
self.circle = createCircle(position, r=d * 0.6, bDynamic=True, density=1, name="wheel")
self.box = createBox((position[0], position[1] + d * 1.9), d * 0.2, d * 2, bDynamic=True, density=1)
self.joint = myCreateRevoluteJoint(self.circle, self.box, position, iswheel=True)
self.bHand = bHand
self.IR = IR(1)
self.box.userData["name"] = name
self.box.userData["nIR"] = 1
if(name == "cartLeft"):
self.IR.IRAngles = [0]
else:
self.IR.IRAngles = [np.pi]
self.box.userData["IRAngles"] = self.IR.IRAngles
self.box.userData["IRValues"] = self.IR.IRValues
if(bHand > 0):
body = self.box
h = d * 0.15
w = d * 0.4
pos = (2 * w - d * 0.2, 0)
if(bHand == 2):
pos = (-2 * w + d * 0.2, 0)
fixtureDef = createBoxFixture(pos, width=w, height=h, collisionGroup = collisionGroup)
body.CreateFixture(fixtureDef)
self.motor_speed = 0
self.angle = 0
def __init__(self,
position=(0, 0),
angle=np.pi / 2,
r=0.48,
bHorizontal=False,
frontIR=6,
nother=0,
nrewsensors=0,
RGB=[255, 0, 0]):
"""Init of userData map with relevant values."""
self.ini_pos = position
#self.body = createCircle(position, r=r, bDynamic=True, restitution=0, name="epuck")
self.body = createBox(position,
w=r,
h=r,
wdiv=1,
hdiv=1,
bDynamic=True,
restitution=0,
name="epuck")
self.body.angle = angle
self.r = r
# self.body = createBox(position, w=0.2,h=0.2,bDynamic=True)
self.motors = [0, 0]
self.bHorizontal = bHorizontal
self.bForceMotors = True
self.frontIR = frontIR
self.IR = IR(frontIR)
self.RGB = RGB
self.body.userData["nIR"] = frontIR
self.body.userData["IRAngles"] = self.IR.IRAngles
self.body.userData["IRValues"] = self.IR.IRValues
self.body.userData["RGB"] = self.RGB
self.body.userData["radius"] = self.r
self.GradSensors = []
self.body.userData["nOtherSensors"] = nother
self.nother = nother
if (nother > 0):
otherGrad = GradSensor(nother, name="other")
self.GradSensors.append(otherGrad)
self.body.userData["OtherAngles"] = otherGrad.GradAngles
self.body.userData["OtherValues"] = otherGrad.GradValues
self.nrewardsensors = nrewsensors
self.body.userData["nRewardSensors"] = nrewsensors
if (nrewsensors > 0):
rewGrad = GradSensor(nrewsensors, name="reward")
self.GradSensors.append(rewGrad)
self.body.userData["RewardAngles"] = rewGrad.GradAngles
self.body.userData["RewardValues"] = rewGrad.GradValues
self.userData = self.body.userData
def setOcclusion(self, y=3, h=1):
"""Create an occulsion box that has no collisions."""
self.box = createBox([0, y],
w=self.walls_dx,
h=h,
bDynamic=False,
bCollideNoOne=True,
name="occlusion")
self.box.userData["visible"] = 1.0
self.box.userData["height"] = h
self.box.userData["y"] = y
def iniThreeObjects(self, pos_obj, obj_type):
self.objs.append(createCircle(pos_obj, r=0.45))
self.objs.append(createTri(pos_obj, r=0.45))
self.objs.append(createBox(pos_obj, wdiv=1, hdiv=1, w=0.35, h=0.35))
self.target_objs = []
self.target_objs.append(createCircle(pos_obj, r=0.45))
self.target_objs.append(createTri(pos_obj, r=0.45))
self.target_objs.append(
createBox(pos_obj, wdiv=1, hdiv=1, w=0.35, h=0.35))
for t in self.target_objs:
t.active = False
t.position = [2, 4]
x = -2
for o in self.objs:
o.position, x = (x, 4), x + 1
if (obj_type == "circle"):
self.obj, self.target_obj = self.objs[0], self.target_objs[0]
elif (obj_type == "box"):
self.obj, self.target_obj = self.objs[2], self.target_objs[2]
def __init__(self, position=(0,0), name="simple", bTwoArms=True, collisionGroup=None, bOppositeArms=False):
"""Init body and arms of the robot."""
global nao
nao = self
self.ini_pos = position
x, y = position[0], position[1]
self.salient = []
self.bTwoArms = bTwoArms
self.body_pos = (x, y)
w = 0.4
if(bOppositeArms):
w = 0.5
if(not bOppositeArms and bTwoArms):
createBox((x - w / 2.0, y), w=w / 2.0, h=w / 1.8, bDynamic=False, collisionGroup=-1)
createBox((x + w / 2.0, y), w=w / 2.0, h=w / 1.8, bDynamic=False, collisionGroup=-1)
bShrink = False
length = 1
self.nparts = 2
if(name == "human"):
self.nparts = 3
length = 1.0 / 1.5
bShrink = True
self.arms = []
if(not bOppositeArms):
self.arms.append(Arm(bLateralize=1, hdiv=1, nparts=self.nparts, position=(x - w, y), length=length, name=name, bShrink=bShrink, collisionGroup=collisionGroup))
if(bTwoArms):
self.arms.append(Arm(bLateralize=2, hdiv=1, nparts=self.nparts, position=(x+w,y), length=length, name=name, bShrink=bShrink, collisionGroup=collisionGroup))
else:
arm1 = Arm(position=(x, y), bLateralize=0, hdiv=1, nparts=self.nparts, length = length, name=name, bShrink=bShrink, collisionGroup=collisionGroup)
arm2 = Arm(position=(x, y + 3), signDir=-1, bLateralize=0, hdiv=1, nparts=self.nparts, length=length, name=name, bShrink=bShrink, collisionGroup=collisionGroup)
self.arms.append(arm1)
self.arms.append(arm2)
def addWalls(self, pos):
"""Limits of the world."""
x, y = pos
wl = 0.2
h = (5 + 1) / 2.0
l = 6
createBox((x, y - 1), w=l + 2 * wl, h=wl, bDynamic=False)
# createBox((x,y+5), w = 3, h = wl, bDynamic=False)
createBox((x - l - wl, y + h - 1), w=wl, h=2.8, bDynamic=False)
createBox((x + l + wl, y + h - 1), w=wl, h=2.8, bDynamic=False)
def iniConstrainedObject(self, pos_obj):
obj = createCircle(pos_obj, r=0.3, density=0.01, name="ball")
self.objs.append(obj)
obj.position = [0, 1.5]
self.obj = self.objs[0]
obj.userData["name"] = "toy"
bar = createBox(obj.position,
w=1,
h=0.001,
bDynamic=False,
bCollideNoOne=True)
bar.userData["name"] = "bar"
self.joint = myCreateLinearJoint(bar,
obj,
force=0,
lowerTranslation=-0.82,
upperTranslation=0.82)
def addWalls(self, pos):
"""Limits of the world. Also defined globaly!!! """
x, y = pos
wl = 0.2
h = (5 + 1) / 2.0
l = 4.5
createBox((x, y - 1), w=l + 2 * wl, h=wl, bDynamic=False)
# createBox((x,y+5), w = 3, h = wl, bDynamic=False)
createBox((x - l - wl, y + h - 1),
w=wl,
h=1.5,
bDynamic=False,
damping=0,
friction=0)
createBox((x + l + wl, y + h - 1),
w=wl,
h=1.5,
bDynamic=False,
damping=0,
friction=0)
def __init__(self,
pos_obj=(0, 1.3),
pos_nao=(0, 0),
obj_type="circle",
salientMode="center",
name="bimanual",
debug=False,
bTwoArms=True,
bSelfCollisions=True):
global bDebug
bDebug = debug
print "-------------------------------------------------------------"
print "Created Exp Bimanual Setup: ", name, "Debug: ", bDebug, "Object"
self.name = name.lower()
self.dm_lim = 1
self.v_lim = 0.3
self.salient = []
self.haptic = []
self.obj_type = obj_type
if (bSelfCollisions): collisionGroup = None
else: collisionGroup = -1
self.name_robot = "simple"
bOppositeArms = False
self.objs = []
if (self.name == "bimanual"):
self.name_robot = "human"
addWalls(pos_nao)
self.iniThreeObjects(pos_obj, obj_type)
elif (self.name == "twooppositearms"):
self.name_robot = "human"
bOppositeArms = True
self.obj_type = "circle"
self.iniConstrainedObject(pos_obj)
w = 0.3
self.boxA = createBox([-0.8, 1.5],
w=w,
h=w,
bDynamic=False,
bCollideNoOne=True,
name="boxA")
self.boxB = createBox([0.8, 1.5],
w=w,
h=w,
bDynamic=False,
bCollideNoOne=True,
name="boxB")
self.nao = NaoRobot(pos_nao,
name=self.name_robot,
bTwoArms=bTwoArms,
bOppositeArms=bOppositeArms,
collisionGroup=collisionGroup)
self.arms = self.nao.arms
self.ini_obj_pos = pos_obj
self.changeSalientMode(salientMode)
if (bDebug):
print "Exp Setup created", self.salientMode, "salient points: ", self.salient
def __init__(self,
xshift=0,
salientMode="center",
name="simple",
debug=False,
objBetween=4,
objWidth=0.1,
objForce=100,
bSelfCollisions=True):
global world, bDebug
bDebug = debug
print "-------------------------------------------------"
print "Created Exp Dual Cart Pole Setup ", name, "Debug: ", bDebug
world.gravity = Box2D.b2Vec2(0, -250)
self.name = name
self.salient = []
self.haptic = [0.1] * 10
self.addWalls(pos=[0, 0])
self.xshift = xshift
self.objBetween = objBetween
xpos = 1.5
self.carts = [
CartPole(name="cartLeft",
position=(-xpos + xshift, -0.3),
bHand=1,
d=0.8,
collisionGroup=1),
CartPole(name="cartRight",
position=(xpos + xshift, -0.3),
bHand=2,
d=0.8,
collisionGroup=2)
]
if (objWidth > 0):
bodyleft, bodyright = self.carts[0].box, self.carts[1].box
if (objBetween == 1):
self.link = [
createBox((xshift, 2),
xpos * 0.8,
objWidth,
bDynamic=True,
restitution=0.8)
]
elif (objBetween >= 2 and objBetween <= 3):
y = 1.53
if (objBetween == 3):
objLong = xpos * 0.8 / 3.5
else:
objLong = xpos * 0.8 / 2.0
bodyA = createBox((xshift - objLong, y),
objLong,
objWidth,
bDynamic=True,
restitution=0.8)
bodyB = createBox((xshift + objLong, y),
objLong,
objWidth - 0.03,
bDynamic=True,
restitution=0.8)
self.joint = myCreateLinearJoint(bodyA,
bodyB,
force=objForce,
lowerTranslation=-0.9,
upperTranslation=0)
self.link = [bodyA, bodyB]
if (objBetween == 3):
dy = 0.3
bodyA.position = (xshift - objLong, y - dy)
bodyB.position = (xshift + objLong, y - dy)
self.jointleft = myCreateRevoluteJoint(
bodyleft,
bodyA,
(xshift - 1.95 * objLong, y + objWidth / 2.0 - dy),
lowerAngle=-2 * np.pi,
upperAngle=2 * np.pi)
self.jointright = myCreateRevoluteJoint(
bodyright,
bodyB,
(xshift + 1.95 * objLong, y + objWidth / 2.0 - dy),
lowerAngle=-2 * np.pi,
upperAngle=2 * np.pi)
elif (objBetween == 4):
pini = (bodyleft.position[0] + 0.8, bodyleft.position[1])
rbodies, rlinks = createRope(pini, 10, r=0.1, density=0.1)
self.link = rbodies
myCreateDistanceJoint(bodyleft, rbodies[0], dx=0.8)
myCreateDistanceJoint(bodyright, rbodies[-1], dx=-0.8)
if (bSelfCollisions):
collisionGroup = None
else:
collisionGroup = -1
if (bDebug):
print "Exp Setup created", "salient points: ", self.salient
