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
