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