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 __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)
class Lucy(object): def __init__(self): self.sysConf = LoadSystemConfiguration() self.robotConfiguration = LoadRobotConfiguration() self.joints = self.robotConfiguration.getJointsName() self.time = 0 self.startTime = time.time() self.distance = 0 self.stop = False self.poseExecuted = 0 def getFitness(self, endFrameExecuted=False): pass def executePose(self, pose): pass def getFrame(self): pass def stopLucy(self): pass def isLucyUp(self): pass
class Lucy(object): def __init__(self): self.sysConf = LoadSystemConfiguration() self.robotConfiguration = LoadRobotConfiguration() self.joints = self.robotConfiguration.getJointsName() self.time = 0 self.startTime = time.time() self.distance = 0 self.stop = False self.poseExecuted = 0 dontSupportedJoints = self.sysConf.getVrepNotImplementedBioloidJoints() self.RobotImplementedJoints = [] for joint in self.joints: if joint not in dontSupportedJoints: self.RobotImplementedJoints.append(joint) def getFitness(self, secuenceLength, concatenationGap): pass def executePose(self, pose): pass def getFrame(self): pass def stopLucy(self): pass def isLucyUp(self): pass def getPosesExecutedByStepQty(self): pass
def __init__(self, simulatorModel=None): self.robotOrientationFirstTime = True self.getDistanceToGoalFirstTime = True self.getUpDistanceFirstTime = True self.getObjectPositionFirstTime = True self.sysConf = LoadSystemConfiguration() #this data structure is like a cache for the joint handles self.jointHandleMapping = {} robotConf = LoadRobotConfiguration() self.model = simulatorModel self.LucyJoints = robotConf.getJointsName() for joint in self.LucyJoints: self.jointHandleMapping[joint]=0 self.clientId = self.connectVREP() RETRY_ATTEMPTS = 50 if simulatorModel: for i in range(RETRY_ATTEMPTS): #TODO try to reutilize the same scene for the sake of performance error = self.loadscn(self.clientId, simulatorModel) if not error: break print "retrying connection to vrep" if error: raise VrepException("error loading Vrep robot model", -1) if int(self.sysConf.getProperty("synchronous mode?"))==1: self.synchronous = True vrep.simxSynchronousTrigger(self.clientId) else: self.synchronous = False self.speedmodifier = int(self.sysConf.getProperty("speedmodifier")) #setting the simulation time step self.simulationTimeStepDT = float(self.sysConf.getProperty("simulation time step")) '''#testing printing in vrep error, consoleHandler = vrep.simxAuxiliaryConsoleOpen(self.clientId, "lucy debugging", 8, 22, None, None, None, None, vrep.simx_opmode_oneshot_wait) print "console handler: ", consoleHandler vrep.simxAuxiliaryConsoleShow(self.clientId, consoleHandler, 1, vrep.simx_opmode_oneshot_wait) error = vrep.simxAuxiliaryConsolePrint(self.clientId, consoleHandler, "Hello World", vrep.simx_opmode_oneshot_wait)''' error, self.upDistanceHandle = vrep.simxGetDistanceHandle(self.clientId, "upDistance#", vrep.simx_opmode_blocking) error, self.distLFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceLFootGoal#", vrep.simx_opmode_blocking) error, self.distRFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceRFootGoal#", vrep.simx_opmode_blocking) error, self.bioloidHandle = vrep.simxGetObjectHandle(self.clientId,"Bioloid", vrep.simx_opmode_oneshot_wait) error, self.cuboidHandle = vrep.simxGetObjectHandle(self.clientId,"Cuboid", vrep.simx_opmode_oneshot_wait) error, self.armHandler = vrep.simxGetObjectHandle(self.clientId, "Pivot", vrep.simx_opmode_oneshot_wait) self.populateJointHandleCache(self.clientId) self.isRobotUpFirstCall = True self.getDistanceToSceneGoal() #to fix the first invocation self.getUpDistance() self.isRobotUp(self.clientId) self.armPositionXAxis = -9.0000e-02
def __init__(self, simulatorModel=None): self.getObjectPositionFirstTime = True #this data structure is like a cache for the joint handles self.jointHandleMapping = {} robotConf = LoadRobotConfiguration() self.model = simulatorModel self.LucyJoints = robotConf.getJointsName() for joint in self.LucyJoints: self.jointHandleMapping[joint]=0 self.clientId = self.connectVREP() if simulatorModel: self.loadscn(self.clientId, simulatorModel)
def normaliseConcatenationGap(self, concatenationGap): prop = DTIndividualPropertyVanillaEvolutive() robotConf = LoadRobotConfiguration() robotJoints = robotConf.getJointsName() totalJointsQty = len(robotJoints) avoidJointsQty = 0 maxJointDiff = 300 for joint in robotJoints: if prop.avoidJoint(joint): avoidJointsQty += 1 minDiff = (totalJointsQty - avoidJointsQty) * maxJointDiff maxDiff = 0 normalizedGap = (concatenationGap - minDiff) / (maxDiff - minDiff) return abs(normalizedGap)
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
from Communication import CommSerial import Actuator comm_tty = CommSerial() #TODO read a configuration file to use the correct parameters for CommSimulator comm_tty.connect() actuator_tty = Actuator.Actuator(comm_tty) root = ET.Element("root") lucy = ET.SubElement(root, "Lucy") config = LoadRobotConfiguration() 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) print joint_id pos = actuator_tty.get_position(joint_id) #actuator_tty.move_actuator(frameIt,500,700) #print pos #xmlJointAngle = xmlJoint.set("angle" , "3") xmlJointAngle = xmlJoint.set("angle" , str(pos)) ri = raw_input('presione \'c\' para capturar otra tecla para terminar \n') frameIt = frameIt + 1 tree = ET.ElementTree(root) tree.write("poses.xml")