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
