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