class Robot: def __init__(self, world, space, density): self.world = world self.space = space self.density = density self.femurHeight = const.femurHeight self.femurWidth = const.femurWidth self.femurLength = const.femurLength self.tibiaHeight = const.tibiaHeight self.tibiaWidth = const.tibiaWidth self.tibiaLength = const.tibiaLength self.densityFemur = 10 self.densityTibia = 10 self.FMax = 10000 self.totalMass = 0.0 self.bodyExist = True self.showLabels = True self.center = (0,0,0) self.joints = [] self.check = 0 self.checkBack = 0 self.centerRobot = True self.minDeg = const.minDeg self.maxDeg = const.maxDeg self.maxTibiaDeg = const.tibiaDeg self.time = 0.5 # means how many percent of the body do have the density # like human body where only 90% is water self.bodyCoverage = 1.0 self.graph = Graph() self.graphVal = [] def setLegs(self): '''Adds some legs to our robot''' self.femur = [] # body self.tibia = [] # body for i in range(const.numLegs): tmpFemur = self.createBody(self.femurLength, self.femurHeight, self.femurWidth, self.densityFemur) self.femur.append(tmpFemur) for i in range(const.numLegs): tmpTibia = self.createBody(self.tibiaLength, self.tibiaHeight, self.tibiaWidth, self.densityTibia) self.tibia.append(tmpTibia) def createBody(self, lx, ly, lz, density): '''Creates the geom, body and visual box''' # Create body body = vpyode.GDMFrameBody(self.world) element = vpyode.GDMElement() element.DefineBox((density*self.bodyCoverage), lx, ly, lz) body.AddGDMElement('Box', element) self.totalMass += body.getMass().mass return body def dropRobot(self): '''Drops the whole body''' self.body = self.createBody(const.bodyLength, const.bodyHeight, const.bodyWidth, self.density) self.setLegs() self.dropLegs() self.dropBody() self.createJoints() def createJoints(self): '''Create the connections between the legs and body Note: Actually we do net set some of the legs to 90deg. The limitation of the angle stops before we can reach that angle. The reason for using that approach is that the force used for reaching that angle is much higher that way.''' self.joints = [] for i in range(const.numLegs): fX = const.bodyLength/2 fY = const.bodyHeight/2+const.Height fZ = const.bodyWidth/2 tX = const.bodyLength/2 tY = const.bodyHeight/2+const.Height tZ = const.bodyWidth/2+self.femurWidth/2#+const.tibiaWidth/2 if i == 0: self.addHingeJoint(self.femur[i], self.body, (fX, fY, fZ), (0, -1, 0), -0, self.minDeg, self.maxDeg) self.addHingeJoint(self.tibia[i], self.femur[i], (tX, tY, tZ), (1, 0, 0), const.maxAngle, -0.01, self.maxTibiaDeg) elif i == 1: self.addHingeJoint(self.femur[i], self.body, (-fX, fY, fZ), (0, -1, 0), 0, self.maxDeg, self.minDeg) self.addHingeJoint(self.tibia[i], self.femur[i], (-tX, tY, tZ), (1, 0, 0), const.maxAngle, -0.01, self.maxTibiaDeg) elif i == 2: self.addHingeJoint(self.femur[i], self.body, (-fX, fY, -fZ), (0, -1, 0), -const.maxAngle, self.minDeg, self.maxDeg) self.addHingeJoint(self.tibia[i], self.femur[i], (-tX, tY, -tZ), (-1, 0, 0), const.maxAngle, -0.01, self.maxTibiaDeg) elif i == 3: self.addHingeJoint(self.femur[i], self.body, (fX, fY, -fZ), (0, -1, 0), const.maxAngle, self.maxDeg, self.minDeg) self.addHingeJoint(self.tibia[i], self.femur[i], (tX, tY, -tZ), (-1, 0, 0), const.maxAngle, -0.01, self.maxTibiaDeg) def addFixedJoint(self, body1, body2): ''' ''' joint = ode.FixedJoint(self.world) joint.attach(body1, body2) joint.setFixed() return joint def addHingeJoint(self, body1, body2, anchor, axis, degree=0, loStop = 0.2, hiStop=0.2): ''' ''' joint = ode.HingeJoint(self.world) joint.attach(body1, body2) joint.setAnchor(anchor) joint.setAxis(axis) joint.setParam(ode.ParamLoStop, -pi * hiStop) # must be smaller than -pi joint.setParam(ode.ParamHiStop, pi * loStop) # must be smaller than pi joint.setParam(ode.ParamFMax, self.FMax) joint.setParam(ode.ParamVel, self.DegreeToRadian(degree)) joint.addTorque(self.FMax) self.joints.append([joint, self.visualizeHinge(anchor)]) def setAngle(self, val, jNr): '''sets the angle of a specific joint''' val -= 180 self.joints[jNr][0].setParam(ode.ParamVel, self.DegreeToRadian(val)) def visualizeHinge(self, anchor): '''shows a point where the anchor point of the joint is''' #points(pos=[anchor], size=5, color=(1,0,0)) return sphere(pos=anchor, radius=0.3, color=(0,0,1)) def removeJointVisuals(self): for joint in self.joints: joint[1].visible = False del joint[1] def refreshJoints(self): for joint in self.joints: joint[1].pos = joint[0].getAnchor2() def dropLegs(self): '''Drops the legs of the robot''' fX = const.bodyLength/2 fY = const.bodyHeight/2+const.Height fZ = const.bodyWidth/2+self.femurWidth/2 i=0 for leg in self.femur: if i == 0: leg.setPosition((fX, fY, fZ)) elif i == 1: leg.setPosition((-fX, fY, fZ)) elif i == 2: leg.setPosition((-fX, fY, -fZ)) elif i == 3: leg.setPosition((fX, fY, -fZ)) i+=1 i=0 fX = const.bodyLength/2 fY = const.bodyHeight/2-self.femurHeight/2-self.tibiaHeight/2+const.Height fZ = const.bodyWidth/2+self.femurWidth+self.tibiaWidth/2 for leg in self.tibia: if i == 0: leg.setPosition((fX, fY, fZ)) elif i == 1: leg.setPosition((-fX, fY, fZ)) elif i == 2: leg.setPosition((-fX, fY, -fZ)) elif i == 3: leg.setPosition((fX, fY, -fZ)) i+=1 def dropBody(self): '''drops the main body of the robot''' self.body.setPosition( (0, const.bodyHeight/2+const.Height, 0) ) def refreshRobot(self, lBody, lFemur=None, lTibia=None): '''Refreshes the position of the labels and camera''' self.refreshBody(lBody) #self.refreshLegs(lTibia, lFemur) self.refreshJoints() def refreshBody(self, lBody): '''Refreshes the label position and the camera of the center body''' self.body.UpdateDisplay() x,y,z = self.body.getPosition() self.center = (x,y,z) self.refreshGraph((x,y)) if self.showLabels: lBody.pos = (x,y,z) lBody.text = '%.2f, %.2f, %.2f, %.1f kg' % (x,y,z,self.totalMass) def refreshGraph(self, val): if len(self.graphVal) > const.maxSize: self.graphVal = [] else: self.graphVal.append(val) self.graph.plotData(self.graphVal) def refreshLegs(self, lTibia, lFemur): '''Refreshes the position of the labels''' for i in range(len(self.femur)): x,y,z = self.femur[i].getPosition() lFemur.pos = (x,y,z) for i in range(len(self.tibia)): x,y,z = self.tibia[i].getPosition() lTibia.pos = (x,y,z) def scalp (self, vec, scal): '''geometric utility functions - from ODE example''' vec[0] *= scal vec[1] *= scal vec[2] *= scal def length (self, vec): '''geometric utility functions - from ODE example''' return sqrt (vec[0]**2 + vec[1]**2 + vec[2]**2) def DegreeToRadian(self, deg): '''geometric utility functions - from ODE example ''' return float(deg)/180.0 * pi def RadianToDegree(self, rad): '''geometric utility functions - from ODE example ''' return rad/pi * 180 def pushRobot(self): '''Applies a force to the robot - from ODE Example''' l = self.body.getPosition () d = self.length (l) a = max(0, 800000*(1.0-0.2*d*d)) l = [l[1] / 4, l[1], l[2] /4] self.scalp (l, a / self.length (l)) for leg in self.femur: leg.addForce(l) for leg in self.tibia: leg.addForce(l) self.body.addRelForce(l) print 'Applied Force: ', l def setDensity(self, val): '''Sets the density of the robot, considering the percentage it is covered with the material''' self.density = (float(val)*self.bodyCoverage) def setFemurDensity(self, val): '''Sets the density of the femur, considering the percentage it is covered with the material''' self.densityFemur = (float(val)*self.bodyCoverage) def setTibiaDensity(self, val): '''Sets the density of the tibia, considering the percentage it is covered with the material''' self.densityTibia = (float(val)*self.bodyCoverage) def setFemurLength(self, val): self.femurHeight = float(val) def setFemurHeight(self, val): self.femurHeight = float(val) def setFemurWidth(self, val): self.femurWidth = float(val) def setTibiaLength(self, val): self.tibiaLength = float(val) def setTibiaHeight(self, val): self.tibiaHeight = float(val) def setTibiaWidth(self, val): self.tibiaWidth = float(val) def setMinDeg(self, val): self.minDeg = float(val) def setMaxDeg(self, val): self.maxDeg = float(val) def setTibiaMaxDeg(self, val): self.maxTibiaDeg = float(val) def setBodyCoverage(self, val): '''Percentage of the body using the material''' self.bodyCoverage = float(val) print 'Bodyshare = %2.1f percent' % (float(val)*100) def setFMax(self, val): '''The maximum force or torque that the motor will use to achieve the desired velocity. Must be greater than zero. 0 = motor off''' for joint in self.joints: joint[0].setParam(ode.ParamFMax, float(val)) self.FMax = float(val) print 'Torque = %1.2f' % (float(val)) def bodyExists(self): '''Returns the boolean if the robot body exists''' return self.bodyExist def dropAgain(self): '''Removes the old body and readds it to its initial position''' self.bodyExist = False self.check = 0 self.totalMass = 0.0 for b in self.body.GetElementKeys(): self.body.RemoveElement(b) for leg in self.femur: for b in leg.GetElementKeys(): leg.RemoveElement(b) for leg in self.tibia: for b in leg.GetElementKeys(): leg.RemoveElement(b) self.removeJointVisuals() self.dropRobot() print 'total mass is %.1f kg' % (self.totalMass) self.bodyExist = True def moveForward(self): if self.check == 0: t = threading.Timer(self.time, self.moveForward) t.start() self.check += 1 self.setAngle(90, 3) self.setAngle(270, 2) elif self.check == 1: t = threading.Timer(self.time, self.moveForward) t.start() self.check += 1 self.setAngle(270, 3) self.setAngle(90, 0) elif self.check == 2: t = threading.Timer(self.time, self.moveForward) t.start() self.check += 1 self.setAngle(90, 7) self.setAngle(90, 6) elif self.check == 3: t = threading.Timer(self.time, self.moveForward) t.start() self.check += 1 self.setAngle(270, 7) self.setAngle(270, 4) elif self.check == 4: t = threading.Timer(self.time, self.moveForward) t.start() self.check += 1 self.setAngle(90, 5) self.setAngle(90, 4) elif self.check == 5: t = threading.Timer(self.time, self.moveForward) t.start() self.check += 1 self.setAngle(270, 5) self.setAngle(270, 6) elif self.check == 6: t = threading.Timer(self.time, self.moveForward) t.start() self.check += 1 self.setAngle(90, 1) self.setAngle(270, 0) elif self.check == 7: self.check = 0 self.setAngle(270, 1) self.setAngle(90, 2) def moveBackward(self): if self.check == 0: t = threading.Timer(self.time, self.moveBackward) t.start() self.check += 1 self.setAngle(90, 1) self.setAngle(90, 0) elif self.check == 1: t = threading.Timer(self.time, self.moveBackward) t.start() self.check += 1 self.setAngle(270, 1) self.setAngle(270, 2) elif self.check == 2: t = threading.Timer(self.time, self.moveBackward) t.start() self.check += 1 self.setAngle(90, 5) self.setAngle(270, 4) elif self.check == 3: t = threading.Timer(self.time, self.moveBackward) t.start() self.check += 1 self.setAngle(270, 5) self.setAngle(90, 6) elif self.check == 4: t = threading.Timer(self.time, self.moveBackward) t.start() self.check += 1 self.setAngle(90, 7) self.setAngle(270, 6) elif self.check == 5: t = threading.Timer(self.time, self.moveBackward) t.start() self.check += 1 self.setAngle(270, 7) self.setAngle(90, 4) elif self.check == 6: t = threading.Timer(self.time, self.moveBackward) t.start() self.check += 1 self.setAngle(90, 3) self.setAngle(90, 2) elif self.check == 7: self.check = 0 self.setAngle(270, 3) self.setAngle(270, 0) def turnLeft(self): if self.check == 0: t = threading.Timer(self.time, self.turnLeft) t.start() self.check += 1 self.setAngle(90, 3) self.setAngle(270, 2) elif self.check == 1: t = threading.Timer(self.time, self.turnLeft) t.start() self.check += 1 self.setAngle(270, 3) self.setAngle(90, 0) elif self.check == 2: t = threading.Timer(self.time, self.turnLeft) t.start() self.check += 1 self.setAngle(90, 5) self.setAngle(270, 4) elif self.check == 3: t = threading.Timer(self.time, self.turnLeft) t.start() self.check += 1 self.setAngle(270, 5) self.setAngle(90, 6) elif self.check == 4: t = threading.Timer(self.time, self.turnLeft) t.start() self.check += 1 self.setAngle(90, 7) self.setAngle(270, 6) elif self.check == 5: t = threading.Timer(self.time, self.turnLeft) t.start() self.check += 1 self.setAngle(270, 7) self.setAngle(90, 4) elif self.check == 6: t = threading.Timer(self.time, self.turnLeft) t.start() self.check += 1 self.setAngle(90, 1) self.setAngle(270, 0) elif self.check == 7: self.check = 0 self.setAngle(270, 1) self.setAngle(90, 2) def turnRight(self): if self.check == 0: t = threading.Timer(self.time, self.turnRight) t.start() self.check += 1 self.setAngle(90, 1) self.setAngle(90, 0) elif self.check == 1: t = threading.Timer(self.time, self.turnRight) t.start() self.check += 1 self.setAngle(270, 1) self.setAngle(270, 2) elif self.check == 2: t = threading.Timer(self.time, self.turnRight) t.start() self.check += 1 self.setAngle(90, 7) self.setAngle(90, 6) elif self.check == 3: t = threading.Timer(self.time, self.turnRight) t.start() self.check += 1 self.setAngle(270, 7) self.setAngle(270, 4) elif self.check == 4: t = threading.Timer(self.time, self.turnRight) t.start() self.check += 1 self.setAngle(90, 5) self.setAngle(90, 4) elif self.check == 5: t = threading.Timer(self.time, self.turnRight) t.start() self.check += 1 self.setAngle(270, 5) self.setAngle(270, 6) elif self.check == 6: t = threading.Timer(self.time, self.turnRight) t.start() self.check += 1 self.setAngle(90, 3) self.setAngle(90, 2) elif self.check == 7: self.check = 0 self.setAngle(270, 3) self.setAngle(270, 0)
class DataPanel(object): def __init__(self, appScreen, petriDish, left, top, width, height): self.dataPanelSurface = appScreen.subsurface(pygame.Rect((left, top), (width, height))) self.left = left self.top = top self.width = width self.height = height self.petriDish = petriDish self.herbPlot = Graph(left, 100, width, height-100) self.herbPlot.setColor((255,160,0)) self.predPlot = Graph(left, 100, width, height-100) self.predPlot.setColor((255,40,0)) self.omniPlot = Graph(left, 100, width, height-100) self.omniPlot.setColor((0,0,255)) self.grassPlot = Graph(left, 100, width, height-100) self.grassPlot.setColor((0,255,0)) self.time = 0 def updatePlots(self, timePassed): self.time += timePassed # updates all plots self.herbPlot.updateData(self.petriDish.getHerbivoreCount(), timePassed) self.herbPlot.plotData(self.dataPanelSurface) self.predPlot.updateData(self.petriDish.getPredatorCount(), timePassed) self.predPlot.plotData(self.dataPanelSurface) self.omniPlot.updateData(self.petriDish.getOmnivoreCount(), timePassed) self.omniPlot.plotData(self.dataPanelSurface) self.grassPlot.updateData(self.petriDish.getGrassCount() * 200, timePassed) self.grassPlot.plotData(self.dataPanelSurface) def blitAllData(self): # draws all the data variables to the data panel DataVariable('Herbivores', self.petriDish.getHerbivoreCount(), self.dataPanelSurface, 20, 10, 100, 50).draw() DataVariable('Predators', self.petriDish.getPredatorCount(), self.dataPanelSurface, 20, 35, 100, 50).draw() DataVariable('Omnivores', self.petriDish.getOmnivoreCount(), self.dataPanelSurface, 20, 60, 100, 50).draw() DataVariable('Grass', self.petriDish.getGrassCount(), self.dataPanelSurface, 20, 85, 100, 50).draw() DataVariable('hAvgSpeed', self.petriDish.getAvgHSpeed(), self.dataPanelSurface, 20, 110, 100, 50).draw() DataVariable('hAvgFleeSpeed', self.petriDish.getAvgHerbFleeSpeed(), self.dataPanelSurface, 20, 135, 100, 50).draw() DataVariable('hAvgVision', self.petriDish.getAvgHVision(), self.dataPanelSurface, 20, 160, 100, 50).draw() DataVariable('pAvgSpeed', self.petriDish.getAvgPSpeed(), self.dataPanelSurface, 20, 185, 100, 50).draw() DataVariable('pAvgChaseSpeed', self.petriDish.getAvgPredChaseSpeed(), self.dataPanelSurface, 20, 210, 100, 50).draw() DataVariable('pAvgVision', self.petriDish.getAvgPredVision(), self.dataPanelSurface, 20, 235, 100, 50).draw() DataVariable('oAvgSpeed', self.petriDish.getAvgOSpeed(), self.dataPanelSurface, 20, 260, 100, 50).draw() DataVariable('oAvgFleeSpeed', self.petriDish.getAvgOmniFleeSpeed(), self.dataPanelSurface, 20, 285, 100, 50).draw() DataVariable('oAvgVision', self.petriDish.getAvgOmniVision(), self.dataPanelSurface, 20, 310, 100, 50).draw()