예제 #1
0
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
예제 #2
0
파일: Lucy.py 프로젝트: aguirrea/lucy
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