class RobotSniffer: def __init__(self, robot): self.lucy = robot self.framesCapturedQty = 0 self.poses = {} self.sniffing = False self.configuration = LoadRobotConfiguration() def startSniffing(self): self.sniffing = True frame = self.lucy.getFrame() self.poses[self.framesCapturedQty] = Pose(frame) # TODO take into account that angles are represented in simlulator encoding self.framesCapturedQty = self.framesCapturedQty + 1 if self.sniffing: threading.Timer(2, self.startSniffing).start() def stopSniffing(self): self.sniffing = False def generateFile(self,file): root = ET.Element("root") lucy = ET.SubElement(root, "Lucy") for frameIt in range(self.framesCapturedQty): frame = ET.SubElement(lucy, "frame") frame.set("number" , str(frameIt)) for joint in self.configuration.getJointsName(): xmlJoint = ET.SubElement(frame, joint) joint_id = self.configuration.loadJointId(joint) pos = self.poses[frameIt].getValue(joint) xmlJointAngle = xmlJoint.set("angle" , str(pos)) tree = ET.ElementTree(root) tree.write(file)
class PersistPhysicalTask: def __init__(self): comm_tty = CommSerial() comm_tty.connect() self.actuator_tty = Actuator(comm_tty) self.config = LoadRobotConfiguration() self.lucySimulatedRobot = SimulatedLucy(True) self.threadRunning = False self.root = ET.Element("root") self.lucy = ET.SubElement(self.root, "Lucy") self.pose = Pose() self.frameIt = 0 signal.signal(signal.SIGINT, self.signalHandler) self.thread = threading.Thread(target=self.captureTimmer, name='Daemon') #signal.pause() def startCapturingMotion(self): self.threadRunning = True self.thread.setDaemon(True) self.thread.start() pass def stopCapturingMotion(self): self.threadRunning = False tree = ET.ElementTree(self.root) tree.write("poses.xml") self.thread.do_run = False self.thread.join() def signalHandler(self): print ('You pressed Ctrl+C!') self.stopCapturingMotion() sys.exit(0) def captureTimmer(self): while(self.threadRunning): frame = ET.SubElement(self.lucy, "frame") frame.set("number", str(self.frameIt)) for joint in self.config.getJointsName(): xmlJoint = ET.SubElement(frame, joint) joint_id = self.config.loadJointId(joint) pos = self.actuator_tty.get_position(joint_id).toDegrees() #print joint, ":", joint_id, ": ", pos if joint == "R_Knee" or joint == "R_Ankle_Pitch" or joint == "L_Hip_Pitch" or joint == "L_Ankle_Roll" or joint == "R_Ankle_Roll" or joint == "L_Hip_Roll" or joint == "R_Hip_Roll" or joint == "R_Shoulder_Pitch": self.pose.setValue(joint, 300 - pos) else: self.pose.setValue(joint, pos) xmlJointAngle = xmlJoint.set("angle", str(pos)) self.lucySimulatedRobot.executePose(self.pose) self.frameIt += 1 time.sleep(0.2) print "i'm alive"
posesVect[61] = [254,807,279,744,462,561,358,666,467,527,287,686,203,757,649,377,480,458] posesVect[62] = [239,792,279,744,462,561,358,666,473,519,283,685,222,761,627,377,482,459] posesVect[63] = [223,776,279,744,462,561,358,666,482,510,286,684,245,766,607,376,486,485] posesVect[64] = [209,762,279,744,462,561,358,666,490,504,291,683,262,771,595,369,491,500] posesVect[65] = [195,748,279,744,462,561,358,666,496,505,293,680,262,775,596,363,496,505] posesVect[66] = [184,737,279,744,462,561,358,666,501,510,293,676,254,777,605,357,501,510] posesVect[67] = [175,728,279,744,462,561,358,666,507,516,297,669,248,775,614,352,507,516] print "program started" for i in range(len(posesVect)): frame = ET.SubElement(lucyPersistence, "frame") frame.set("number" , str(i)) poseVectItem = posesVect[i] for key in robotConfig.getJointsName(): xmlJoint = ET.SubElement(frame, key) joint_id = configuration.loadJointId(key) angle.setValue(poseVectItem[BalieroLucyMapping[key]-1]) xmlJointAngle = xmlJoint.set("angle" , str(angle.toDegrees())) newPose.setValue(key, angle.toVrep()) #hay que pasarlo a vrep value lucy.executePose(newPose) lucy.stopLucy() tree = ET.ElementTree(root) tree.write("baliero.xml")
actuator_tty = Actuator(comm_tty) root = ET.Element("root") lucy = ET.SubElement(root, "Lucy") config = LoadRobotConfiguration() pose = Pose() lucyRobot = SimulatedLucy(True) frameIt = 0 ri = raw_input('presione \'c\' para capturar otra tecla para terminar \n') while ri == 'c': frame = ET.SubElement(lucy, "frame") frame.set("number", str(frameIt)) for joint in config.getJointsName(): xmlJoint = ET.SubElement(frame, joint) joint_id = config.loadJointId(joint) pos = actuator_tty.get_position(joint_id).toDegrees() print joint, ":", joint_id, ": ", pos if joint == "R_Knee" or joint == "R_Ankle_Pitch" or joint == "L_Hip_Pitch" or joint == "L_Ankle_Roll" or joint == "R_Ankle_Roll" or joint == "L_Hip_Roll" or joint == "R_Hip_Roll" or joint == "R_Shoulder_Pitch": pose.setValue(joint, 300-pos) else: pose.setValue(joint, pos) xmlJointAngle = xmlJoint.set("angle", str(pos)) lucyRobot.executePose(pose) ri = raw_input('presione \'c\' para capturar otra tecla para terminar \n') frameIt += 1 tree = ET.ElementTree(root) tree.write("poses.xml")
class Individual: def __init__(self, idividualProperty, individualGeneticMaterial): self.property = idividualProperty self.fitness = 0 self.robotConfig = LoadRobotConfiguration() self.configuration = LoadSystemConfiguration() self.genomeMatrix = individualGeneticMaterial.getGeneticMatrix() self.poseSize = len(self.genomeMatrix) self.genomeMatrixJointNameIDMapping = {} self.sysConf = LoadSystemConfiguration() i = 0 for jointName in self.robotConfig.getJointsName(): self.genomeMatrixJointNameIDMapping[jointName] = i i = i + 1 dontSupportedJoints = self.configuration.getVrepNotImplementedBioloidJoints() self.robotImplementedJoints = [] robotJoints = self.robotConfig.getJointsName() for joint in robotJoints: if joint not in dontSupportedJoints: self.robotImplementedJoints.append(joint) # is important to use only supported joints to avoid errors obtaining the handler of a joint that doesn't exists for i in xrange(self.poseSize): for joint in self.robotImplementedJoints: # print "i: ", i, "j: ", joint value = self.genomeMatrix[i][self.genomeMatrixJointNameIDMapping[joint]] + self.property.getPoseFix( joint ) # TODO this can be a problem for the physical robot self.genomeMatrix[i][self.genomeMatrixJointNameIDMapping[joint]] = value def stopLucy(self): self.lucy.stopLucy() def execute(self): # create the corresponding instance of Lucy, depending if it's real or simulated. if int(self.sysConf.getProperty("Lucy simulated?")) == 1: self.lucy = SimulatedLucy(int(self.configuration.getProperty("Lucy render enable"))) else: self.lucy = PhysicalLucy() poseExecute = {} i = 0 while self.lucy.isLucyUp() and i < self.poseSize: for joint in self.robotConfig.getJointsName(): if not (self.property.avoidJoint(joint)): value = self.genomeMatrix[i][self.genomeMatrixJointNameIDMapping[joint]] poseExecute[joint] = value i = i + 1 self.lucy.executePose(Pose(poseExecute)) self.lucy.stopLucy() # this function also updates time and distance if i < self.poseSize: self.fitness = self.lucy.getFitness() else: endFrameExecuted = True self.fitness = self.lucy.getFitness(endFrameExecuted) print "fitness: ", self.fitness return self.fitness def getPoseQty(self): return self.lp.getFrameQty() def getPose(self, poseNumber): return self.lp.getFramePose(poseNumber) def getMostSimilarPose(self, pose): diff = MAX_INT moreSimilarPose = self.getPose(1) for i in xrange(self.getPoseQty()): myPose = getPose(i) newDiff = pose.diff(myPose) if newDiff < diff: diff = newDiff moreSimilarPose = myPose return moreSimilarPose def getGenomeMatrix(self): return self.genomeMatrix def setGenomeMatrix(self, geneMatrix): self.genomeMatrix = geneMatrix self.poseSize = len(geneMatrix) def persist(self, file): root = ET.Element("root") lucy = ET.SubElement(root, "Lucy") for frameIt in xrange(self.poseSize): frame = ET.SubElement(lucy, "frame") frame.set("number", str(frameIt)) for joint in self.robotImplementedJoints: xmlJoint = ET.SubElement(frame, joint) joint_id = self.robotConfig.loadJointId(joint) pos = self.genomeMatrix[frameIt][self.genomeMatrixJointNameIDMapping[joint]] xmlJointAngle = xmlJoint.set("angle", str(pos)) tree = ET.ElementTree(root) tree.write(file)
class Individual: def __init__(self, idividualProperty, individualGeneticMaterial, modelReposeValue=DTModelVrepReda()): self.property = idividualProperty self.modelReposeValue = modelReposeValue self.fitness = 0 self.robotConfig = LoadRobotConfiguration() self.configuration = LoadSystemConfiguration() self.genomeMatrix = individualGeneticMaterial.getGeneticMatrix() self.length = individualGeneticMaterial.getLength() self.genomeMatrixJointNameIDMapping = {} self.sysConf = LoadSystemConfiguration() self.individualGeneticMaterial = individualGeneticMaterial self.moveArmFirstTime = True self.precycleLength = 0 self.cycleLength = 0 #Not used yet i=0 for jointName in self.robotConfig.getJointsName(): self.genomeMatrixJointNameIDMapping[jointName]=i i=i+1 dontSupportedJoints = self.configuration.getVrepNotImplementedBioloidJoints() self.robotImplementedJoints = [] robotJoints = self.robotConfig.getJointsName() for joint in robotJoints: if joint not in dontSupportedJoints: self.robotImplementedJoints.append(joint) #is important to use only supported joints to avoid errors obtaining the handler of a joint that doesn't exists for i in xrange(self.length): for joint in self.robotImplementedJoints: #print "i: ", i, "j: ", joint if self.property.avoidJoint(joint): value = modelReposeValue.getReposeValue(joint) else: value = self.genomeMatrix[i][self.genomeMatrixJointNameIDMapping[joint]] + self.property.getPoseFix(joint) #TODO this can be a problem for the physical robot self.genomeMatrix[i][self.genomeMatrixJointNameIDMapping[joint]]=value def stopLucy(self): self.lucy.stopLucy() def execute(self): #create the corresponding instance of Lucy, depending if it's real or simulated. if int(self.sysConf.getProperty("Lucy simulated?"))==1: self.lucy = SimulatedLucy(int(self.configuration.getProperty("Lucy render enable"))) else: self.lucy = PhysicalLucy() poseExecute={} i=0 while (self.lucy.isLucyUp() and i < self.length): for joint in self.robotConfig.getJointsName(): if not(self.property.avoidJoint(joint)): value = self.genomeMatrix[i][self.genomeMatrixJointNameIDMapping[joint]] poseExecute[joint] = value i = i + self.lucy.getPosesExecutedByStepQty() self.lucy.executePose(Pose(poseExecute)) '''if self.precycleLength > 0 and i == self.precycleLength - 1: self.lucy.moveHelperArm() if self.precycleLength > 0 and i > self.precycleLength - 1: if (i - self.precycleLength) > 0 and (i - self.precycleLength) % self.cycleLength == 0: self.lucy.moveHelperArm()''' startingCyclePose = self.getPrecycleLength() executionConcatenationGap = self.individualGeneticMaterial.getConcatenationGap(startingCyclePose) self.fitness = self.lucy.getFitness(self.length, executionConcatenationGap) self.lucy.stopLucy() #this function also updates time and distance return self.fitness def getGenomeMatrix(self): return self.genomeMatrix def setGenomeMatrix(self, geneMatrix): self.genomeMatrix = geneMatrix dtGenome = DTGenomeFunctions() self.length = dtGenome.individualLength(geneMatrix) def persist(self,file): root = ET.Element("root") lucy = ET.SubElement(root, "Lucy") for frameIt in xrange(self.length): frame = ET.SubElement(lucy, "frame") frame.set("number" , str(frameIt)) for joint in self.robotImplementedJoints: #TODO because the notImplementedJoints doesn't participate in the simulation and fitness evaluation, they are not stored xmlJoint = ET.SubElement(frame, joint) joint_id = self.robotConfig.loadJointId(joint) pos = self.genomeMatrix[frameIt][self.genomeMatrixJointNameIDMapping[joint]] xmlJointAngle = xmlJoint.set("angle" , str(pos)) tree = ET.ElementTree(root) tree.write(file) def getJointMatrixIDFromName(self, jointName): return self.genomeMatrixJointNameIDMapping[jointName] def setLength(self, length): self.length = length def setPrecycleLength(self, precycleLength): self.precycleLength = precycleLength def setCycleLength(self, cycleLength): self.cycleLength = cycleLength def getLength(self): return self.length def getPrecycleLength(self): return self.precycleLength def getCycleLength(self): return self.cycleLength