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,
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