Python Simulator.getBioloidPlannarPosition方法代码示例

# 需要导入模块: from Simulator import Simulator [as 别名]
# 或者: from Simulator.Simulator import getBioloidPlannarPosition [as 别名]
genetic_bioloid=os.getcwd()+"/models/genetic_bioloid.ttt"

print 'Program started' 
angle = AXAngle()
sim = Simulator()

clientID = sim.connectVREP()

if clientID !=-1:
    print 'Connected to remote API server'
    sim.loadscn(clientID, genetic_bioloid)
    sim.startSim(clientID,True)
    angle.setDegreeValue(150)    
    sim.setJointPosition(clientID,"R_Hip_Pitch",angle.toVrep())
    sim.setJointPosition(clientID,"L_Hip_Pitch",angle.toVrep())    
    pos1x, pos1y=sim.getBioloidPlannarPosition(clientID)
    end=False
    while not end:
        for i in xrange(30):
            angle.setDegreeValue(150-i)
            #print "voy a setear ax12 value en cadera izquierda: ", 150 - i
            sim.setJointPosition(clientID,'L_Hip_Pitch',angle.toVrep())
            angle.setDegreeValue(i*2)
            #print "voy a setear ax12 value en rodilla derecha: ", i
            sim.setJointPosition(clientID,'R_Knee',angle.toVrep())
            
        for i in xrange(30):
            angle.setDegreeValue(150-i)
            sim.setJointPosition(clientID,'R_Hip_Pitch',angle.toVrep()) 
            sim.setJointPosition(clientID,'L_Knee',angle.toVrep()) 
            

# 需要导入模块: from Simulator import Simulator [as 别名]
# 或者: from Simulator.Simulator import getBioloidPlannarPosition [as 别名]
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:
#.........这里部分代码省略.........