def __init__(self, poseValues={}): self.value = poseValues self.modelReposeValue=DTModelVrepReda() configuration = LoadRobotConfiguration() for joint in configuration.getJointsName(): if joint not in self.value.keys(): self.value[joint]=self.modelReposeValue.getReposeValue(joint)
def __init__(self, geneticMaterial): DTIndividualGeneticMaterial.__init__(self) lp = LoadPoses(geneticMaterial) robotConfig = LoadRobotConfiguration() poseSize = lp.getFrameQty() self.geneticMatrix = [[lp.getPose(i).getValue(j) for j in robotConfig.getJointsName()] for i in xrange(poseSize)]
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)
def getFramePose(self, frameNumber): config = LoadRobotConfiguration() frame = self.framelist[frameNumber] for jointName in config.getJointsName(): joint = frame.getElementsByTagName(jointName)[0] angle = joint.getAttribute("angle") self.jointAngleMapping[jointName] = float(angle) return self.jointAngleMapping
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"
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 getPose(self, frameNumber): sysconf = LoadSystemConfiguration() robotConf = LoadRobotConfiguration() dontSupportedJoints = sysconf.getVrepNotImplementedBioloidJoints() robotImplementedJoints = [] robotJoints = robotConf.getJointsName() for joint in robotJoints: if joint not in dontSupportedJoints: robotImplementedJoints.append(joint) frame = self.framelist[frameNumber] for jointName in robotImplementedJoints: joint = frame.getElementsByTagName(jointName)[0] angle = joint.getAttribute("angle") self.jointAngleMapping[jointName] = float(angle) return Pose(self.jointAngleMapping)
def __init__(self): conf = LoadSystemConfiguration() self.geneticMatrix = [] self.cyclesQty = int(conf.getProperty("Concatenate walk cycles?")) self.robotConfig = LoadRobotConfiguration() self.jointNameIDMapping = {} jointIDCounter = 0 for j in self.robotConfig.getJointsName(): self.jointNameIDMapping[jointIDCounter] = j jointIDCounter += 1
class DTModelRepose(object): def __init__(self): self.repose={} self.robotConfiguration = LoadRobotConfiguration() self.joints = self.robotConfiguration.getJointsName() def getReposeValue(self, joint): if joint in self.repose.keys(): res = self.repose[joint] else: res = 0 return res
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')
class DTIndividualProperty(object): def __init__(self): self.poseFix={} self.avoidJoints=[] self.robotConfiguration = LoadRobotConfiguration() self.joints = self.robotConfiguration.getJointsName() def avoidJoint(self, joint): return joint in self.avoidJoints def getPoseFix(self, joint): if joint in self.poseFix.keys(): res = self.poseFix[joint] else: res = 0 return res def setPoseFix(self, poses): for joint in joints: self.poseFix[joint] = poses[joint]
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
import xml.etree.cElementTree as ET from simulator.Actuator import Actuator from simulator.Communication import CommSerial from simulator.LoadRobotConfiguration import LoadRobotConfiguration from simulator.Lucy import SimulatedLucy from simulator.Pose import Pose comm_tty = CommSerial() #TODO read a configuration file to use the correct parameters for CommSimulator comm_tty.connect() 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)
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
class DTIndividualGeneticMaterial(object): def __init__(self): conf = LoadSystemConfiguration() self.geneticMatrix = [] self.cyclesQty = int(conf.getProperty("Concatenate walk cycles?")) self.robotConfig = LoadRobotConfiguration() self.jointNameIDMapping = {} jointIDCounter = 0 for j in self.robotConfig.getJointsName(): self.jointNameIDMapping[jointIDCounter] = j jointIDCounter += 1 def getGeneticMatrix(self): return self.geneticMatrix def equalSentinelFrame(self, frame): sentinelFramePresent = True for jointValue in frame: sentinelFramePresent = sentinelFramePresent and jointValue == sysConstants.JOINT_SENTINEL return sentinelFramePresent def getLength(self): iter = 0 genomaRawLength = len(self.geneticMatrix) while iter < genomaRawLength and not self.equalSentinelFrame(self.geneticMatrix[iter]): iter = iter + 1 length = iter return length def concatenate(self, individualGeneticMaterial): beforeConcatenationLength = len(self.geneticMatrix) dtgf = DTGenomeFunctions() self.geneticMatrix += individualGeneticMaterial.getGeneticMatrix() concatenationGap = dtgf.euclideanDiff(self.geneticMatrix[beforeConcatenationLength-1], self.geneticMatrix[beforeConcatenationLength]) #print "--------------------------------concatenationGap: ", concatenationGap #if concatenationGap < GAP_THRESHOLD : #self.calculateGapByLinearInterpolation(REFERENCE_WINDOW_RADIUS, INTERPOLATION_WINDOW, SPLINE_SMOOTHING_FACTOR, beforeConcatenationLength, 1, True) self.calculateGapByCubicInterpolation(REFERENCE_WINDOW_RADIUS, INTERPOLATION_WINDOW, SPLINE_SMOOTHING_FACTOR, beforeConcatenationLength, 1) def repeat(self, times): self.geneticMatrix *= times #calculates the gap generated when composing cyclic movements def getConcatenationGap(self, startingCyclePose=0): dtgf = DTGenomeFunctions() if self.getLength() > 0: #return dtgf.rawDiff(self.geneticMatrix[0], self.geneticMatrix[self.getLength() - 1]) return dtgf.euclideanDiff(self.geneticMatrix[startingCyclePose-1], self.geneticMatrix[self.getLength() - 1]) else: return 0 # the '|' represents the cycleSize - 1 position # |...........[referenceWindowsRadius](interpolationWindow)|[referenceWindowsRadius]........| #calculates the set of the last points of the cycle (of size interpolationWindow) using as refence for the line #the set of referenceWindowsRadius size points before the interpolationWindow and the referenceWindowsRadius size #set of the first points of the cycle. It does this for all the cyckeRepetition gaps in the concatenation of the #cycle def calculateGapByLinearInterpolation(self, referenceWindowRadius, interpolationWindow, splineSmoothingFactor, cycleSize, cycleRepetition, graphicalRepresentation=False): x = np.ndarray(referenceWindowRadius * 2) y = np.ndarray(referenceWindowRadius * 2) poseQty = len(self.geneticMatrix) poseLength = len(self.geneticMatrix[0]) #print "poseQty: ", poseQty, "poseLength: ", poseLength, "lp.getFrameQty(): ", lp.getFrameQty() for joint in range(poseLength): interpolationDataIter = referenceWindowRadius - 1 for k in xrange(referenceWindowRadius): referenceFrame = cycleSize + k x[interpolationDataIter] = referenceFrame y[interpolationDataIter] = self.geneticMatrix[referenceFrame][joint] interpolationDataIter += 1 interpolationDataIter = referenceWindowRadius - 2 for k in xrange(referenceWindowRadius): referenceFrame = cycleSize - 1 - (interpolationWindow + k) x[interpolationDataIter] = referenceFrame y[interpolationDataIter] = self.geneticMatrix[referenceFrame][joint] interpolationDataIter -= 1 x = np.sort(x) spl = interp1d(x, y) #spl.set_smoothing_factor(splineSmoothingFactor/10.0) if graphicalRepresentation: px = linspace(x[0], x[len(x)-1], len(x)) py = spl(px) plt.plot(x, y, '.-') plt.plot(px, py) xinter = np.ndarray(interpolationWindow) yinter = np.ndarray(interpolationWindow) for k in xrange(interpolationWindow): smoothFrameIter = cycleSize - 1 - 1 - k xinter[k] = smoothFrameIter yinter[k] = self.geneticMatrix[smoothFrameIter][joint] plt.plot(xinter, yinter, '.-') #original data for k in xrange(interpolationWindow): smoothFrameIter = cycleSize - 1 - 1 - k xinter[k] = smoothFrameIter yinter[k] = spl(smoothFrameIter) plt.plot(xinter, yinter, '*-') #interpolated data plt.title(self.jointNameIDMapping[joint]) plt.show() print "gap between first and last: ", self.getConcatenationGap() for i in xrange(cycleRepetition): for k in range(cycleSize - 1 - interpolationWindow, cycleSize): newValue = spl(k) self.geneticMatrix[cycleSize * i + k][joint] = newValue # the '|' represents the cycleSize - 1 position # |...........[referenceWindowsRadius](interpolationWindow)|[referenceWindowsRadius]........| #calculates the set of the last points of the cycle (of size interpolationWindow) using as refence for the spline #the set of referenceWindowsRadius size points before the interpolationWindow and the referenceWindowsRadius size #set of the first points of the cycle. It does this for all the cycleRepetition gaps in the concatenation of the #cycle def calculateGapByCubicInterpolation(self, referenceWindowRadius, interpolationWindow, splineSmoothingFactor, cycleSize, cycleRepetition, graphicalRepresentation=False): x = np.ndarray(referenceWindowRadius * 2) y = np.ndarray(referenceWindowRadius * 2) poseQty = len(self.geneticMatrix) poseLength = len(self.geneticMatrix[0]) #print "poseQty: ", poseQty, "poseLength: ", poseLength, "lp.getFrameQty(): ", lp.getFrameQty() for joint in range(poseLength): interpolationDataIter = referenceWindowRadius - 1 for k in xrange(referenceWindowRadius + 1): referenceFrame = cycleSize + k x[interpolationDataIter] = referenceFrame y[interpolationDataIter] = self.geneticMatrix[referenceFrame][joint] interpolationDataIter += 1 interpolationDataIter = referenceWindowRadius - 2 for k in xrange(referenceWindowRadius + 1): referenceFrame= cycleSize -1 - (interpolationWindow + k) x[interpolationDataIter] = referenceFrame y[interpolationDataIter] = self.geneticMatrix[referenceFrame][joint] interpolationDataIter -= 1 x = np.sort(x) if abs(self.geneticMatrix[cycleSize - interpolationWindow][joint] - self.geneticMatrix[cycleSize][joint]) < 3: spl = interp1d(x, y) else: spl = UnivariateSpline(x, y) spl.set_smoothing_factor(splineSmoothingFactor/10.0) if graphicalRepresentation: px = linspace(x[0], x[len(x)-1], len(x)) py = spl(px) plt.plot(x, y, '.-') plt.plot(px, py) xinter = np.ndarray(interpolationWindow) yinter = np.ndarray(interpolationWindow) for k in xrange(interpolationWindow): smoothFrameIter = cycleSize - 1 - k xinter[k] = smoothFrameIter yinter[k] = self.geneticMatrix[smoothFrameIter][joint] plt.plot(xinter, yinter, '.-') #original data for k in xrange(interpolationWindow): smoothFrameIter = cycleSize - 1 - k xinter[k] = smoothFrameIter yinter[k] = spl(smoothFrameIter) plt.plot(xinter, yinter, '*-') #interpolated data plt.title(self.jointNameIDMapping[joint]) plt.show() print "gap between first and last: ", self.getConcatenationGap() for i in xrange(cycleRepetition): for k in range(cycleSize - 1 - interpolationWindow, cycleSize): newValue = spl(k) self.geneticMatrix[cycleSize * i + k][joint] = newValue
def G2DListMutatorRealGaussianSpline(genome, **args): """ A gaussian mutator for G2DList of Real with UnivariateSpline for smoothing Accepts the *rangemin* and *rangemax* genome parameters, both optional. Also accepts the parameter *gauss_mu* and the *gauss_sigma* which respectively represents the mean and the std. dev. of the random distribution. """ dtgenome = DTGenomeFunctions() pmut = args["pmut"] if pmut <= 0.0: return 0 width = dtgenome.getIndividualFrameLength(genome) height = dtgenome.getIndividualLength(genome) elements = height * width mutations = pmut * elements mu = genome.getParam("gauss_mu") sigma = genome.getParam("gauss_sigma") prop = DTIndividualPropertyVanillaEvolutive() robotConfig = LoadRobotConfiguration() robotJoints = robotConfig.getJointsName() validIDs = [] jointIndex = 0 for joint in robotJoints: if not prop.avoidJoint(joint): validIDs.append(jointIndex) jointIndex += 1 if mu is None: mu = CDefG2DListMutRealMU if sigma is None: sigma = CDefG2DListMutRealSIGMA if mutations < 1.0: mutations = 0 for i in xrange(genome.getHeight()): for j in xrange(genome.getWidth()): if genome[i][j] != sysConstants.JOINT_SENTINEL and j in validIDs and randomFlipCoin(pmut): offset = rand_gauss(mu, sigma) print "OFFSET: ", offset, "joint: ", j, "frame: ", i final_value = genome[i][j] + offset final_value = min(final_value, genome.getParam("rangemax", CDefRangeMax)) final_value = max(final_value, genome.getParam("rangemin", CDefRangeMin)) genome.setItem(i, j, final_value) dtgenome.smooth(genome, j, i, offset) ##pca.poseInterpolationWithPCA(chromosomeToLucyGeneticMatrix(genome), j) mutations += 1 else: for it in xrange(int(round(mutations))): foundFrameDifferentThanSentinel = False validIDFound = False while not foundFrameDifferentThanSentinel and not validIDFound: which_x = rand_randint(0, genome.getWidth() - 1) # joint to mutate if which_x in validIDs: #only mutate not avoided ids validIDFound = True which_y = rand_randint(0, genome.getHeight() - 1) # pose to mutate if genome[which_y][which_x] != sysConstants.JOINT_SENTINEL: foundFrameDifferentThanSentinel = True offset = rand_gauss(mu, sigma) final_value = genome[which_y][which_x] + offset #print "OFFSET: ", offset, "joint: ", which_x, "frame: ", which_y # to be sure that the value is less than the rangemax and more than the rangemin value final_value = min(final_value, genome.getParam("rangemax", CDefRangeMax)) final_value = max(final_value, genome.getParam("rangemin", CDefRangeMin)) # valueBeforeMutation = genome[which_y][which_x] genome.setItem(which_y, which_x, final_value) dtgenome.smooth(genome, which_x, which_y) ##pca.poseInterpolationWithPCA(chromosomeToLucyGeneticMatrix(genome), which_x) return int(mutations)
def __init__(self): self.robotConfig = LoadRobotConfiguration()
class DTGenomeFunctions(object): def __init__(self): self.robotConfig = LoadRobotConfiguration() # returns the distance between two frame poses, if one of the poses is a sentinel pose the distance is INFINITE_DISTANCE # uses square power to make visible a change in one of the joints def euclideanDiff(self, frame1, frame2): diff = 0 prop = DTIndividualPropertyVanillaEvolutive() robotJoints = self.robotConfig.getJointsName() jointIndex = 0 for joint in robotJoints: if not prop.diffAvoidJoint(joint): if frame1[jointIndex] == sysConstants.JOINT_SENTINEL or frame2[jointIndex] == sysConstants.JOINT_SENTINEL: diff = INFINITE_DISTANCE break else: jointDiff = (frame1[jointIndex] - frame2[jointIndex]) ** 2 diff += abs(jointDiff) #print "joint: ", joint, "diff: ", jointDiff, "frame1[jointIndex]", frame1[jointIndex], "frame2[jointIndex]", frame2[jointIndex] jointIndex = jointIndex + 1 return diff # returns the distance between two frame poses, if one of the poses is a sentinel pose the distance is INFINITE_DISTANCE def rawDiff(self, frame1, frame2): diff = 0 prop = DTIndividualPropertyVanillaEvolutive() robotJoints = self.robotConfig.getJointsName() jointIndex = 0 for joint in robotJoints: if not prop.avoidJoint(joint): jointDiff = abs(frame1[jointIndex] - frame2[jointIndex]) diff += jointDiff jointIndex = jointIndex + 1 return diff # compares every joint of a frame with the JOINT_SENTINEL value, and return True if every joint has a value equal to it def equalSentinelFrame(self, frame): sentinelFramePresent = True for jointValue in frame: sentinelFramePresent = sentinelFramePresent and jointValue == sysConstants.JOINT_SENTINEL return sentinelFramePresent # returns the length of a genome, as the number of poses it contains until the JOINT_SENTINEL value is present in all the joints of a pose def getIndividualLength(self, genome): iter = 0 genomaHeight = genome.getHeight() while iter < genomaHeight and not self.equalSentinelFrame(genome[iter]): iter = iter + 1 length = iter return length def getIndividualFrameLength(self, genome): return genome.getWidth() def smooth(self, genome, which_x, which_y): interpolationPointsQty = SMOOTHING_WINDOW which_y_InterpolationNeighborhood = interpolationPointsQty / 2 minimunInterpolationNeighborhoodSize = interpolationPointsQty / 4 if which_y - interpolationPointsQty / 2 < 0: interpolationPointsQty -= abs(which_y - which_y_InterpolationNeighborhood) * 2 which_y_InterpolationNeighborhood = interpolationPointsQty / 2 elif which_y + interpolationPointsQty / 2 > genome.getHeight() - 1: interpolationPointsQty -= (which_y + which_y_InterpolationNeighborhood - (genome.getHeight() - 1)) * 2 which_y_InterpolationNeighborhood = interpolationPointsQty / 2 if which_y_InterpolationNeighborhood >= minimunInterpolationNeighborhoodSize: x = np.ndarray(interpolationPointsQty) y = np.ndarray(interpolationPointsQty) for k in xrange(interpolationPointsQty): poseToSmooth = which_y - which_y_InterpolationNeighborhood + k x[k] = poseToSmooth y[k] = genome[poseToSmooth][which_x] spl = UnivariateSpline(x, y) spl.set_smoothing_factor(SPLINE_SMOOTHING_FACTOR_SPLINE/10) for k in xrange(interpolationPointsQty): if y[k] != sysConstants.JOINT_SENTINEL: newValue = spl(int(x[k])) genome.setItem(int(x[k]), which_x, newValue) #/____A____/____B_____ /which_y/____C_____/_____D____/ B + C are the interpolationWindow, A + B + C + D are the # interpolationPointsQty. using A and D as fixed points, interpolate B and C def interpolate(self, genome, which_x, which_y, wich_y_is_fixed_data=0): interpolationPointsQty = SMOOTHING_WINDOW which_y_InterpolationNeighborhood = interpolationPointsQty / 2 minimunInterpolationNeighborhoodSize = interpolationPointsQty / 4 array_size = 0 if which_y - which_y_InterpolationNeighborhood < 0: interpolationPointsQty -= abs(which_y - which_y_InterpolationNeighborhood) * 2 which_y_InterpolationNeighborhood = interpolationPointsQty / 2 elif which_y + interpolationPointsQty / 2 > genome.getHeight() - 1: interpolationPointsQty -= (which_y + which_y_InterpolationNeighborhood - (genome.getHeight() - 1)) * 2 which_y_InterpolationNeighborhood = interpolationPointsQty / 2 interpolationWindowRadius = interpolationPointsQty / 4 if which_y_InterpolationNeighborhood >= minimunInterpolationNeighborhoodSize: array_size = interpolationPointsQty - interpolationWindowRadius * 2 if wich_y_is_fixed_data: array_size += 1 x = np.ndarray(array_size) y = np.ndarray(array_size) splineIndexCounter = 0 for k in xrange(interpolationPointsQty + 1): poseToSmooth = which_y - which_y_InterpolationNeighborhood + k if poseToSmooth <= which_y - interpolationWindowRadius or poseToSmooth > which_y + interpolationWindowRadius: x[splineIndexCounter] = poseToSmooth y[splineIndexCounter] = genome[poseToSmooth][which_x] splineIndexCounter += 1 if wich_y_is_fixed_data: x[splineIndexCounter] = which_y y[splineIndexCounter] = genome[which_y][which_x] splineIndexCounter += 1 if genome[which_y - interpolationWindowRadius][which_x] == genome[which_y + interpolationWindowRadius][wich_x]: spl = interp1d(x, y) else: x_order = np.argsort(x) spl = UnivariateSpline(x_order, y) spl.set_smoothing_factor(SPLINE_SMOOTHING_FACTOR_INTERPOLATION/10) for k in xrange(interpolationPointsQty): iter = which_y - which_y_InterpolationNeighborhood + k if genome[iter][which_x] != sysConstants.JOINT_SENTINEL: if iter > which_y - interpolationWindowRadius and iter <= which_y + interpolationWindowRadius: if wich_y_is_fixed_data: #if fixed data do not change the which_y point if iter != which_y: newValue = spl(iter) genome.setItem(iter, which_x, newValue) else: newValue = spl(iter) genome.setItem(iter, which_x, newValue)
AR_C = 0 AR_A = 0 AR_Phi = 0 T = 0.5 root = ET.Element("root") lucyPersistence = ET.SubElement(root, "Lucy") try: lucy = SimLucy(True) pose = {} poses = {} poseNumber = 0 configuration = LoadRobotConfiguration() simTimeMark = lucy.getSimTime() counter = 0 while lucy.isLucyUp() and counter <= 400: print "executin pose number: ", counter frame = ET.SubElement(lucyPersistence, "frame") frame.set("number" , str(poseNumber)) simTime = lucy.getSimTime() #Calculate Joint Angles #Shoulder Pitch pose["L_Shoulder_Pitch"] = SP_C+SP_A*math.sin(2*math.pi*simTime/T+SP_Phi) pose["R_Shoulder_Pitch"] = SP_C+SP_A*math.sin(2*math.pi*simTime/T+SP_Phi+math.pi) #Hip Roll pose["L_Hip_Roll"] = HR_C+HR_A*math.sin(2*math.pi*simTime/T+HR_Phi) pose["R_Hip_Roll"] = HR_C+HR_A*math.sin(2*math.pi*simTime/T+HR_Phi+math.pi)
class DTGenomeFunctions(object): def __init__(self): self.robotConfig = LoadRobotConfiguration() # returns the distance between two frame poses, if one of the poses is a sentinel pose the distance is INFINITE_DISTANCE # uses square power to make visible a change in one of the joints def euclideanDiff(self, frame1, frame2): diff = 0 prop = DTIndividualPropertyVanillaEvolutive() robotJoints = self.robotConfig.getJointsName() jointIndex = 0 for joint in robotJoints: if not prop.avoidJoint(joint): if frame1[jointIndex] == sysConstants.JOINT_SENTINEL or frame2[jointIndex] == sysConstants.JOINT_SENTINEL: diff = INFINITE_DISTANCE break else: jointDiff = (frame1[jointIndex] - frame2[jointIndex]) ** 2 diff += jointDiff jointIndex = jointIndex + 1 return diff # returns the distance between two frame poses, if one of the poses is a sentinel pose the distance is INFINITE_DISTANCE def rawDiff(self, frame1, frame2): diff = 0 prop = DTIndividualPropertyVanillaEvolutive() robotJoints = self.robotConfig.getJointsName() jointIndex = 0 for joint in robotJoints: if not prop.avoidJoint(joint): jointDiff = frame1[jointIndex] - frame2[jointIndex] diff += jointDiff jointIndex = jointIndex + 1 return diff # compares every joint of a frame with the JOINT_SENTINEL value, and return True if every joint has a value equal to it def equalSentinelFrame(self, frame): sentinelFramePresent = True for jointValue in frame: sentinelFramePresent = sentinelFramePresent and jointValue == sysConstants.JOINT_SENTINEL return sentinelFramePresent # returns the length of a genome, as the number of poses it contains until the JOINT_SENTINEL value is present in all the joints of a pose def getIndividualLength(self, genome): iter = 0 genomaHeight = genome.getHeight() while iter < genomaHeight and not self.equalSentinelFrame(genome[iter]): iter = iter + 1 length = iter return length def getIndividualFrameLength(self, genome): return genome.getWidth() def smooth(self, genome, which_x, which_y): interpolationPointsQty = SMOOTHING_WINDOW which_y_InterpolationNeighborhood = interpolationPointsQty / 2 minimunInterpolationNeighborhoodSize = 4 if which_y - interpolationPointsQty / 2 < 0: interpolationPointsQty -= abs(which_y - which_y_InterpolationNeighborhood) * 2 which_y_InterpolationNeighborhood = interpolationPointsQty / 2 elif which_y + interpolationPointsQty / 2 > genome.getHeight() - 1: interpolationPointsQty -= (which_y + which_y_InterpolationNeighborhood - (genome.getHeight() - 1)) * 2 which_y_InterpolationNeighborhood = interpolationPointsQty / 2 if which_y_InterpolationNeighborhood > minimunInterpolationNeighborhoodSize: x = np.ndarray(interpolationPointsQty) y = np.ndarray(interpolationPointsQty) for k in xrange(interpolationPointsQty): poseToSmooth = which_y - which_y_InterpolationNeighborhood + k x[k] = poseToSmooth y[k] = genome[poseToSmooth][which_x] spl = UnivariateSpline(x, y, s=SPLINE_SMOOTHING_FACTOR) for k in xrange(interpolationPointsQty): # print "before ", genome[int(x[k])][which_x], "now ", spl(int(x[k])), "k ", k, "diff ", genome[int(x[k])][which_x]-spl(int(x[k])), "offset ", offset # if x[k] == which_x: # print "no mutation" # elif y[k]!= sysConstants.JOINT_SENTINEL: if y[k] != sysConstants.JOINT_SENTINEL: newValue = spl(int(x[k])) genome.setItem(int(x[k]), which_x, newValue) ##print "mutating value: ", y[k], "to value: ", newValue, "difference: ", abs(y[k]-newValue) ##else: ##print "oh no!, we have a problem in mutation operator .. :S" def interpolate(self, genome, which_x, which_y): interpolationPointsQty = SMOOTHING_WINDOW which_y_InterpolationNeighborhood = interpolationPointsQty / 2 minimunInterpolationNeighborhoodSize = 4 if which_y - interpolationPointsQty / 2 < 0: interpolationPointsQty -= abs(which_y - which_y_InterpolationNeighborhood) * 2 which_y_InterpolationNeighborhood = interpolationPointsQty / 2 elif which_y + interpolationPointsQty / 2 > genome.getHeight() - 1: interpolationPointsQty -= (which_y + which_y_InterpolationNeighborhood - (genome.getHeight() - 1)) * 2 which_y_InterpolationNeighborhood = interpolationPointsQty / 2 interpolationWindow = interpolationPointsQty / 5 if which_y_InterpolationNeighborhood > minimunInterpolationNeighborhoodSize: x = np.ndarray(interpolationPointsQty - interpolationWindow * 2) y = np.ndarray(interpolationPointsQty - interpolationWindow * 2) splineIndexCounter = 0 for k in xrange(interpolationPointsQty): poseToSmooth = which_y - which_y_InterpolationNeighborhood + k if poseToSmooth <= which_y - interpolationWindow or poseToSmooth > which_y + interpolationWindow: x[splineIndexCounter] = poseToSmooth y[splineIndexCounter] = genome[poseToSmooth][which_x] splineIndexCounter += 1 spl = UnivariateSpline(x, y, s=SPLINE_SMOOTHING_FACTOR) for k in xrange(interpolationPointsQty): iter = which_y - which_y_InterpolationNeighborhood + k if genome[iter][which_x] != sysConstants.JOINT_SENTINEL: newValue = spl(iter) genome.setItem(iter, which_x, newValue)
def __init__(self, robot): self.lucy = robot self.framesCapturedQty = 0 self.poses = {} self.sniffing = False self.configuration = LoadRobotConfiguration()
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)
# You should have received a copy of the GNU General Public License # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA from simulator.SimLucy import SimLucy from Pose import Pose from simulator.AXAngle import AXAngle from simulator.LoadRobotConfiguration import LoadRobotConfiguration import xml.etree.cElementTree as ET posesVect = {} angle = AXAngle() BalieroLucyMapping = {} newPose = Pose() robotConfig = LoadRobotConfiguration() lucy = SimLucy(True) root = ET.Element("root") lucyPersistence = ET.SubElement(root, "Lucy") configuration = LoadRobotConfiguration() BalieroLucyMapping['L_Shoulder_Pitch']=2 BalieroLucyMapping['R_Shoulder_Pitch']=1 BalieroLucyMapping['L_Shoulder_Yaw']=4 BalieroLucyMapping['R_Shoulder_Yaw']=3 BalieroLucyMapping['L_Elbow_Yaw']=6 BalieroLucyMapping['R_Elbow_Yaw']=5 BalieroLucyMapping['R_Hip_Yaw']=7
def __init__(self): self.repose={} self.robotConfiguration = LoadRobotConfiguration() self.joints = self.robotConfiguration.getJointsName()
def __init__(self): self.poseFix={} self.avoidJoints=[] self.robotConfiguration = LoadRobotConfiguration() self.joints = self.robotConfiguration.getJointsName()
def __init__(self, poseValues={}): self.value = poseValues configuration = LoadRobotConfiguration() for joint in configuration.getJointsName(): if joint not in self.value.keys(): self.value[joint]=REPOSE_JOINT_VALUE