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, 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 connectVREP(self, ipAddr=LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep IP"), port=int(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep port"))): self.getObjectPositionFirstTime = True #vrep.simxFinish(-1) # just in case, close all opened connections #TODO this could be a problem when the connection pool is implemented time1 = time() clientID = vrep.simxStart(ipAddr,port,True,True,5000,5) time2 = time() print "LOGGING time for connecting VREP in senconds: ", time2 - time1 return clientID
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 isRobotUp(self, clientID): error1 = False if self.isRobotUpFirstCall: error1, LSP_Handle=vrep.simxGetObjectHandle(clientID,"Bioloid", vrep.simx_opmode_oneshot_wait) if error1 == 0: self.bioloidHandle = LSP_Handle self.isRobotUpFirstCall = False error2, bioloid_position = self.getObjectPositionWrapper(clientID, self.bioloidHandle) #print "bioloid position", bioloid_position[2] return error1 or error2, bioloid_position[2]>float(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"FALL_THRESHOLD_DOWN")) and bioloid_position[2] < float(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"FALL_THRESHOLD_UP"))
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 __init__(self): self.sysConf = LoadSystemConfiguration() self.robotConfiguration = LoadRobotConfiguration() self.joints = self.robotConfiguration.getJointsName() self.time = 0 self.startTime = time.time() self.distance = 0 self.distanceBeforMoveArmLastCall = 0 self.armDistance = 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) self.balanceHeight = float(self.sysConf.getProperty("BALANCE_HEIGHT"))
def isRobotUp(self, clientID): error = False error, LSP_Handle=vrep.simxGetObjectHandle(clientID,"Bioloid", vrep.simx_opmode_oneshot_wait) or error error, bioloid_position = self.getObjectPositionWrapper(clientID, LSP_Handle) or error return error, bioloid_position[2]>float(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"FALL_THRESHOLD"))
def connectVREP(self, ipAddr=LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep IP"), port=int(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep port"))): self.getObjectPositionFirstTime = True vrep.simxFinish(-1) # just in case, close all opened connections return vrep.simxStart(ipAddr,port,True,True,5000,5)
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
def connectVREP(self, ipAddr=LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep IP"), port=int(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep port"))): self.getObjectPositionFirstTime = True #vrep.simxFinish(-1) # just in case, close all opened connections #TODO this could be a problem when the connection pool is implemented return vrep.simxStart(ipAddr,port,True,True,5000,5)
class Simulator: 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() if simulatorModel: self.loadscn(self.clientId, simulatorModel) 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) self.populateJointHandleCache(self.clientId) self.isRobotUpFirstCall = True self.getDistanceToSceneGoal() #to fix the first invocation self.getUpDistance() self.isRobotUp(self.clientId) def getClientId(self): return self.clientId def setClientId(self, idClient): self.clientId = idClient def getObjectPositionWrapper(self, clientID, LSP_Handle): if self.getObjectPositionFirstTime: error, ret = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_streaming) self.getObjectPositionFirstTime = False else: error, ret = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_buffer) return error, ret def connectVREP(self, ipAddr=LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep IP"), port=int(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep port"))): self.getObjectPositionFirstTime = True #vrep.simxFinish(-1) # just in case, close all opened connections #TODO this could be a problem when the connection pool is implemented return vrep.simxStart(ipAddr,port,True,True,5000,5) def loadscn(self, clientID, model): error=vrep.simxLoadScene(clientID, model, 2, vrep.simx_opmode_oneshot_wait) return error def printJointPositions(self, clientID): error, handlers, intData, floatData, stringData=vrep.simxGetObjectGroupData(clientID,vrep.sim_appobj_object_type,0,vrep.simx_opmode_oneshot_wait) itemHandle=0 print stringData for name in stringData: error, position = vrep.simxGetJointPosition(clientID,handlers[itemHandle], vrep.simx_opmode_streaming) itemHandle=itemHandle+1 print name + ":" + str(position) def isRobotUp(self, clientID): error1 = False if self.isRobotUpFirstCall: error1, LSP_Handle=vrep.simxGetObjectHandle(clientID,"Bioloid", vrep.simx_opmode_oneshot_wait) if error1 == 0: self.bioloidHandle = LSP_Handle self.isRobotUpFirstCall = False error2, bioloid_position = self.getObjectPositionWrapper(clientID, self.bioloidHandle) #print "bioloid position", bioloid_position[2] return error1 or error2, bioloid_position[2]>float(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"FALL_THRESHOLD_DOWN")) and bioloid_position[2] < float(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"FALL_THRESHOLD_UP")) #error, upDistance = self.getUpDistance() #return error, upDistance > float(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"FALL_THRESHOLD_DOWN")) and upDistance < float(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"FALL_THRESHOLD_UP"))''' def startSim(self, clientID, screen=True): #I need the simulator stopped in order to be started retSimStop = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) if retSimStop != simx_return_ok : print "simulation couldnt be stopped!" else: print "simulation stopped!" #setting the physics engine retSetPhyEngine = vrep.simxSetIntegerParameter(clientID, vrep.sim_intparam_dynamic_engine, bulletEngine, vrep.simx_opmode_oneshot_wait) if retSetPhyEngine != simx_return_ok: print "unable to set the physical engine" else: print "physical engine correctly setted" if int(self.sysConf.getProperty("physics enable?"))==1: vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,True,vrep.simx_opmode_oneshot) else: vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,False,vrep.simx_opmode_oneshot) #settig simulation speed if self.speedmodifier > 0: vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_speedmodifier, self.speedmodifier, vrep.simx_opmode_oneshot_wait) #settig simulation step retSetTimeStep = vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, self.simulationTimeStepDT, vrep.simx_opmode_oneshot_wait) if retSetTimeStep != simx_return_ok : print "problems setting time step" else: print "time step setted!" #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_realtime_simulation,1,vrep.simx_opmode_oneshot_wait) #sync mode configuration if self.synchronous: vrep.simxSynchronous(clientID,True) #light mode configuration if not screen: vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_visible_layers,2,vrep.simx_opmode_oneshot_wait) #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait) #start simulation error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot) return error def pauseSim(self, clientID): return vrep.simxPauseCommunication(clientID,True) def resumePauseSim(self, clientID): ret=vrep.simxPauseCommunication(clientID,False) if self.speedmodifier > 0: vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_speedmodifier, self.speedmodifier, vrep.simx_opmode_oneshot_wait) return ret #when the simulator is paused the call to simxGetObjectHandle returns error def populateJointHandleCache(self, clientID): for joint in self.LucyJoints: error, handle = vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait) self.jointHandleMapping[joint]=handle pass def setJointPosition(self, clientID, joint, angle): error = False handle = self.jointHandleMapping[joint] if handle == 0: errorGetObjetHandle, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait) error = errorGetObjetHandle or error self.jointHandleMapping[joint]=handle #error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot_wait) error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot) #using oneshot instead of oneshot_wait make it a nonblocking call but is the only way to avoid 5 seconds wait in the execution of the individual if self.synchronous: vrep.simxSynchronousTrigger(clientID) return error def setJointPositionNonBlock(self, clientID, joint, angle): handle = self.jointHandleMapping[joint] error = False if (handle == 0): error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot) self.jointHandleMapping[joint]=handle return vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_streaming) def getJointPositionNonBlock(self, clientID, joint, firstTime): error = False value = 0 handle = self.jointHandleMapping[joint] if handle == 0: error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot) self.jointHandleMapping[joint]=handle if not error: if firstTime: error, value = vrep.simxGetJointPosition(clientID,handle,vrep.simx_opmode_streaming) else: error, value = vrep.simxGetJointPosition(clientID,handle,vrep.simx_opmode_buffer) return error, value def finishSimulation(self, clientID): self.getObjectPositionFirstTime = True errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait) error=errorStop or errorClose errorFinish=vrep.simxFinish(clientID) error=error or errorFinish if self.synchronous: vrep.simxSynchronous(clientID,False) return error def getBioloidPlannarPosition(self, clientID): errorHandler, LSP_Handle=vrep.simxGetObjectHandle(clientID,"Bioloid", vrep.simx_opmode_oneshot_wait) errorObjectPosition = True if not errorHandler: errorObjectPosition, bioloid_position = self.getObjectPositionWrapper(clientID, LSP_Handle) #print "error handler: ", errorHandler, " error object position: ", errorObjectPosition if not errorHandler or errorObjectPosition: return False, bioloid_position[0], bioloid_position[1] return True, None, None def getPosesExecutedByStepQty(self, clientID): return int(float(vrep.simxGetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, vrep.simx_opmode_oneshot_wait)[1] ) / float(0.050)) def getDistanceToSceneGoal(self): if self.getDistanceToGoalFirstTime: errorL, distanceL = vrep.simxReadDistance(self.clientId, self.distLFootToGoalHandle, vrep.simx_opmode_streaming) errorR, distanceR = vrep.simxReadDistance(self.clientId, self.distRFootToGoalHandle, vrep.simx_opmode_streaming) self.getDistanceToGoalFirstTime = False else: errorL, distanceL = vrep.simxReadDistance(self.clientId, self.distLFootToGoalHandle, vrep.simx_opmode_buffer) errorR, distanceR = vrep.simxReadDistance(self.clientId, self.distRFootToGoalHandle, vrep.simx_opmode_buffer) return errorL or errorR, (distanceL + distanceR)*0.5 def getUpDistance(self): if self.getUpDistanceFirstTime: error, distance = vrep.simxReadDistance(self.clientId, self.upDistanceHandle, vrep.simx_opmode_streaming) self.getUpDistanceFirstTime = False else: error, distance = vrep.simxReadDistance(self.clientId, self.upDistanceHandle, vrep.simx_opmode_buffer) return error, distance def robotOrientationToGoal(self): if self.robotOrientationFirstTime: error, angle = vrep.simxGetObjectOrientation(self.clientId, self.bioloidHandle, self.cuboidHandle, vrep.simx_opmode_streaming) self.robotOrientationFirstTime = False else: error, angle = vrep.simxGetObjectOrientation(self.clientId, self.bioloidHandle, self.cuboidHandle, vrep.simx_opmode_buffer) return error, angle[1]
class Simulator: 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 getClientId(self): return self.clientId def setClientId(self, idClient): self.clientId = idClient def getObjectPositionWrapper(self, clientID, LSP_Handle): if self.getObjectPositionFirstTime: error, ret = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_streaming) self.getObjectPositionFirstTime = False else: error, ret = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_buffer) return error, ret def connectVREP(self, ipAddr=LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep IP"), port=int(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep port"))): self.getObjectPositionFirstTime = True #vrep.simxFinish(-1) # just in case, close all opened connections #TODO this could be a problem when the connection pool is implemented time1 = time() clientID = vrep.simxStart(ipAddr,port,True,True,5000,5) time2 = time() print "LOGGING time for connecting VREP in senconds: ", time2 - time1 return clientID def loadscn(self, clientID, model): time1 = time() error = vrep.simxLoadScene(clientID, model, 2, vrep.simx_opmode_oneshot_wait) time2 = time() print "LOGGING time for loading VREP scene in seconds: ", time2 - time1 return error def printJointPositions(self, clientID): error, handlers, intData, floatData, stringData=vrep.simxGetObjectGroupData(clientID,vrep.sim_appobj_object_type,0,vrep.simx_opmode_oneshot_wait) itemHandle=0 print stringData for name in stringData: error, position = vrep.simxGetJointPosition(clientID,handlers[itemHandle], vrep.simx_opmode_streaming) itemHandle=itemHandle+1 print name + ":" + str(position) def isRobotUp(self, clientID): GET_DISTANCE_BUGGY_VALUE = 100000 error, upDistance = self.getUpDistance() upDistance = float(upDistance) if upDistance > GET_DISTANCE_BUGGY_VALUE: #filter for garbage return on the firsts invocations of vrep.simxReadDistance return error, True else: return error, upDistance > float(self.sysConf.getProperty("FALL_THRESHOLD_DOWN")) and upDistance < float(self.sysConf.getProperty("FALL_THRESHOLD_UP")) def startSim(self, clientID, screen=True): #I need the simulator stopped in order to be started retSimStop = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) if retSimStop != simx_return_ok : print "simulation couldnt be stopped!" else: print "simulation stopped!" #setting the physics engine retSetPhyEngine = vrep.simxSetIntegerParameter(clientID, vrep.sim_intparam_dynamic_engine, bulletEngine, vrep.simx_opmode_oneshot_wait) if retSetPhyEngine != simx_return_ok: print "unable to set the physical engine" else: print "physical engine correctly setted" if int(self.sysConf.getProperty("physics enable?"))==1: vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,True,vrep.simx_opmode_oneshot) else: vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,False,vrep.simx_opmode_oneshot) #settig simulation speed if self.speedmodifier > 0: vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_speedmodifier, self.speedmodifier, vrep.simx_opmode_oneshot_wait) #settig simulation step retSetTimeStep = vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, self.simulationTimeStepDT, vrep.simx_opmode_oneshot_wait) if retSetTimeStep != simx_return_ok : print "problems setting time step" else: print "time step setted!" #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_realtime_simulation,1,vrep.simx_opmode_oneshot_wait) #sync mode configuration if self.synchronous: vrep.simxSynchronous(clientID,True) #light mode configuration if not screen: vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_visible_layers,2,vrep.simx_opmode_oneshot_wait) #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait) #start simulation error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot) if int(self.sysConf.getProperty("blank screen?"))==1: vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait) return error def pauseSim(self, clientID): return vrep.simxPauseCommunication(clientID,True) def resumePauseSim(self, clientID): ret=vrep.simxPauseCommunication(clientID,False) if self.speedmodifier > 0: vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_speedmodifier, self.speedmodifier, vrep.simx_opmode_oneshot_wait) return ret #when the simulator is paused the call to simxGetObjectHandle returns error def populateJointHandleCache(self, clientID): for joint in self.LucyJoints: error, handle = vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait) self.jointHandleMapping[joint]=handle pass def setJointPosition(self, clientID, joint, angle): error = False handle = self.jointHandleMapping[joint] if handle == 0: errorGetObjetHandle, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait) error = errorGetObjetHandle or error self.jointHandleMapping[joint]=handle #error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot_wait) error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot) #using oneshot instead of oneshot_wait make it a nonblocking call but is the only way to avoid 5 seconds wait in the execution of the individual if self.synchronous: vrep.simxSynchronousTrigger(clientID) return error def setJointPositionNonBlock(self, clientID, joint, angle): handle = self.jointHandleMapping[joint] error = False if (handle == 0): error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot) self.jointHandleMapping[joint]=handle return vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_streaming) def getJointPositionNonBlock(self, clientID, joint, firstTime): error = False value = 0 handle = self.jointHandleMapping[joint] if handle == 0: error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot) self.jointHandleMapping[joint]=handle if not error: if firstTime: error, value = vrep.simxGetJointPosition(clientID,handle,vrep.simx_opmode_streaming) else: error, value = vrep.simxGetJointPosition(clientID,handle,vrep.simx_opmode_buffer) return error, value def finishSimulation(self, clientID): self.getObjectPositionFirstTime = True errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait) error=errorStop or errorClose errorFinish=vrep.simxFinish(clientID) error=error or errorFinish if self.synchronous: vrep.simxSynchronous(clientID,False) return error def getBioloidPlannarPosition(self, clientID): errorHandler, LSP_Handle=vrep.simxGetObjectHandle(clientID,"Bioloid", vrep.simx_opmode_oneshot_wait) errorObjectPosition = True if not errorHandler: errorObjectPosition, bioloid_position = self.getObjectPositionWrapper(clientID, LSP_Handle) #print "error handler: ", errorHandler, " error object position: ", errorObjectPosition if not errorHandler or errorObjectPosition: return False, bioloid_position[0], bioloid_position[1] return True, None, None def getPosesExecutedByStepQty(self, clientID): return int(float(vrep.simxGetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, vrep.simx_opmode_oneshot_wait)[1] ) / float(0.050)) def getDistanceToSceneGoal(self): if self.getDistanceToGoalFirstTime: errorL, distanceL = vrep.simxReadDistance(self.clientId, self.distLFootToGoalHandle, vrep.simx_opmode_streaming) errorR, distanceR = vrep.simxReadDistance(self.clientId, self.distRFootToGoalHandle, vrep.simx_opmode_streaming) self.getDistanceToGoalFirstTime = False else: errorL, distanceL = vrep.simxReadDistance(self.clientId, self.distLFootToGoalHandle, vrep.simx_opmode_buffer) errorR, distanceR = vrep.simxReadDistance(self.clientId, self.distRFootToGoalHandle, vrep.simx_opmode_buffer) return errorL or errorR, max(distanceL, distanceR) def getUpDistance(self): if self.getUpDistanceFirstTime: error, distance = vrep.simxReadDistance(self.clientId, self.upDistanceHandle, vrep.simx_opmode_streaming) retrys = 0 while error and retrys < 1000: error, distance = vrep.simxReadDistance(self.clientId, self.upDistanceHandle, vrep.simx_opmode_streaming) retrys += 1 self.getUpDistanceFirstTime = False else: error, distance = vrep.simxReadDistance(self.clientId, self.upDistanceHandle, vrep.simx_opmode_buffer) return error, distance def robotOrientationToGoal(self): if self.robotOrientationFirstTime: error, angle = vrep.simxGetObjectOrientation(self.clientId, self.bioloidHandle, self.cuboidHandle, vrep.simx_opmode_streaming) self.robotOrientationFirstTime = False else: error, angle = vrep.simxGetObjectOrientation(self.clientId, self.bioloidHandle, self.cuboidHandle, vrep.simx_opmode_buffer) return error, angle[1] def moveHelperArm(self, distTraveled): SET_OBJECT_POSITION_ERROR_THRESHOLD = 2 MOVE_HELPER_ARM_ERROR = 1 newArmPositionX = float(distTraveled * 1.2) newArmPosition = (newArmPositionX, 0, 0) #vrep.simxSetBooleanParameter(self.clientId,sim_boolparam_dynamics_handling_enabled,False,vrep.simx_opmode_oneshot) #moves relative to the robot itself error = vrep.simxSetObjectPosition(self.clientId, self.armHandler, self.armHandler, newArmPosition, vrep.simx_opmode_oneshot) #vrep.simxSetBooleanParameter(self.clientId,sim_boolparam_dynamics_handling_enabled,True,vrep.simx_opmode_oneshot) if error < SET_OBJECT_POSITION_ERROR_THRESHOLD: self.armPositionXAxis = newArmPosition else: return MOVE_HELPER_ARM_ERROR