class SimulatedLucy(Lucy): def __init__(self, visible=False): Lucy.__init__(self) self.visible = visible genetic_bioloid = os.getcwd() + self.sysConf.getFile("Lucy vrep model") self.sim = Simulator(genetic_bioloid) self.clientID = self.sim.getClientId() if self.clientID == -1: retry_counter = sysConstants.ERROR_RETRY time.sleep(0.1) self.clientID = self.sim.getClientId() while self.clientID == -1 and retry_counter > 0: time.sleep(0.5) retry_counter = retry_counter - 1 self.clientID = self.sim.getClientId() print "waiting for vrep connection" if self.clientID == -1: raise VrepException("error connecting with Vrep", -1) self.sim.startSim(self.clientID,self.visible) self.jointHandleCachePopulated = False configuration = LoadRobotConfiguration() self.joints = configuration.getJointsName() self.firstCallGetFrame = True self.sim.robotOrientationToGoal() self.updateLucyPosition() self.posesExecutedByStepQty = self.sim.getPosesExecutedByStepQty(self.clientID) def getSimTime(self): if self.stop == False: self.time = time.time() - self.startTime return self.time def getSimDistance(self): return self.distance def getFitness(self, secuenceLength, concatenationGap): error, angle = self.sim.robotOrientationToGoal() distance = self.getSimDistance() error, upD = self.sim.getUpDistance() framesQty = secuenceLength cycleEnded = 0 print "--------------------------------------------------------------------" print "orientation: ", angle print "distance traveled: ", distance print "poses executed/total poses: ", self.poseExecuted, "/", framesQty if self.isLucyUp(): print "isRobotUp?: True" cycleEnded = 1 framesExecuted = 1 endCycleBalance = upD/self.balanceHeight #Distance from the floor when lucy is straight up if endCycleBalance > 1: endCycleBalance = 1 else: print "isRobotUp?: False" if framesQty > 0: framesExecuted = self.poseExecuted / float(framesQty) if framesExecuted == 1: framesExecuted = framesExecuted - framesExecuted/10 else: framesExecuted = 0 endCycleBalance = 0 dtFitness = DTFitness(distance, concatenationGap, framesExecuted, endCycleBalance, angle) fitnessFunction = DistanceConcatenationgapFramesexecutedEndcyclebalanceAngle(dtFitness) #dtFitness = DTFitness(distance=distance, concatenationGap=concatenationGap, framesExecuted=framesExecuted, angle=angle, cycleEnded=cycleEnded) #fitnessFunction = NormdistanceConcatenationgapFramesexecutedNormAngle(dtFitness) #fitnessFunction = ConcatenationgapFramesexecutedNormAngle(dtFitness) fitness = fitnessFunction.getFitness() print "framesExecuted: ", framesExecuted print "FITNESS: ", fitness print "upDistance: ", self.sim.getUpDistance() print "endCycleBalance: ", endCycleBalance print "--------------------------------------------------------------------" return fitness def getPosesExecutedByStepQty(self): return self.posesExecutedByStepQty def executePose(self, pose): error = False #Above's N joints will be received and set on the V-REP side at the same time jointsQty = len(self.RobotImplementedJoints) jointExecutedCounter = 0 error = self.sim.pauseSim(self.clientID) or error for joint in self.RobotImplementedJoints: angle = pose.getValue(joint) angleAX = AXAngle() angleAX.setDegreeValue(angle) #print "setting joint: ", joint, " to value: ", angle if jointExecutedCounter < jointsQty - 1: error = self.sim.setJointPositionNonBlock(self.clientID, joint, angleAX.toVrep()) or error else: ###self.moveHelperArm() error = self.sim.resumePauseSim(self.clientID) or error error = self.sim.setJointPosition(self.clientID, joint, angleAX.toVrep()) or error jointExecutedCounter += 1 self.updateLucyPosition() self.poseExecuted = self.poseExecuted + self.getPosesExecutedByStepQty() #if error: # raise VrepException("error excecuting a pose", error) def executeRawPose(self, pose): error = False #Above's N joints will be received and set on the V-REP side at the same time jointsQty = len(self.RobotImplementedJoints) jointExecutedCounter=0 error = self.sim.pauseSim(self.clientID) or error for joint in self.RobotImplementedJoints: angle = pose.getValue(joint) #print "setting joint: ", joint, " to value: ", angle if jointExecutedCounter < jointsQty - 1: error = self.sim.setJointPositionNonBlock(self.clientID, joint, angle) or error else: error = self.sim.resumePauseSim(self.clientID) or error error = self.sim.setJointPosition(self.clientID, joint, angle) or error jointExecutedCounter += 1 self.updateLucyPosition() print "distance traveled: ", self.distance self.poseExecuted = self.poseExecuted + self.getPosesExecutedByStepQty() def getFrame(self): error = False pose = {} dontSupportedJoints = self.sysConf.getVrepNotImplementedBioloidJoints() if (self.jointHandleCachePopulated == False): self.sim.populateJointHandleCache(self.clientID) self.jointHandleCachePopulated = True error = self.sim.pauseSim(self.clientID) or error for joint in self.joints: if joint not in dontSupportedJoints: #actual model of vrep bioloid don't support this joints errorGetJoint, value = self.sim.getJointPositionNonBlock(self.clientID, joint, self.firstCallGetFrame) error = error or errorGetJoint pose[joint] = 150 - value * 60 self.firstCallGetFrame = False error = self.sim.resumePauseSim(self.clientID) or error #if error: # raise VrepException("error geting a frame", error) return error, pose def angle(self,v): if v.imag >=0: resAngle = angle(v, True) #angle second argument is for operate with degrees instead of radians else: resAngle = 180+angle(-v, True) #angle second argument is for operate with degrees instead of radians return resAngle def updateLucyPosition(self): if self.stop == False: self.time = time.time() - self.startTime error, distToGoal = self.sim.getDistanceToSceneGoal() if not error: distTravelToGoal = 1.0 - distToGoal if distTravelToGoal < 0: self.distance = 0 else: self.distance = distTravelToGoal else: print "*****************************************ERROR CALCULATING DIST TO GOAL IN Lucy:updateLucyPosition" def stopLucy(self): self.stop = True self.updateLucyPosition() self.sim.finishSimulation(self.clientID) def isLucyUp(self): error, up = self.sim.isRobotUp(self.clientID) if error: #raise VrepException("error consulting if lucy is up", error) return False return up def moveHelperArm(self): error = False armDistanceStep = self.distance - self.distanceBeforMoveArmLastCall if armDistanceStep > 0: self.armDistance = self.armDistance + armDistanceStep #error = self.sim.pauseSim(self.clientID) or error error = self.sim.moveHelperArm(armDistanceStep) or error #error = self.sim.resumePauseSim(self.clientID) or error self.distanceBeforMoveArmLastCall = self.distance return error