Python ALProxy.walkInit方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import walkInit [as 别名]
#
# Nao porject test.py
# Made by platel_k
#

## Import
from naoqi import ALProxy

## Code
#nao = "127.0.0.1"
nao = "192.168.0.42"
tts = ALProxy("ALTextToSpeech", nao, 9559)
motion = ALProxy("ALMotion", nao, 9559)
motion.walkInit()
motion.post.walkTo(0.5, 0, 0)
tts.say("Bonjour")

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import walkInit [as 别名]
class BallTracker:
        
    def __init__(self, glade_file_path=c.GLADE_FILE_PATH):
	self.glade_file_path = glade_file_path
            
	# Gtk Builder Init
	self.builder = Gtk.Builder()
	self.builder.add_from_file(self.glade_file_path)
	self.builder.connect_signals(self)

	# Add UI Components
	self.window = self.builder.get_object("ballWindow")

	# Show UI
	self.window.show_all()

	self.myBroker = ALBroker("myBroker","0.0.0.0",0,"10.0.2.3",9559)
        self.motion   = ALProxy("ALMotion")
        self.tracker  = ALProxy("ALRedBallTracker")
        self.vision   = ALProxy("ALVideoDevice")
        self.tts      = ALProxy("ALTextToSpeech")
        self.currentCamera = 0
        self.setTopCamera(self)
        self.tracker.setWholeBodyOn(False)
        self.tracker.startTracker()
        self.ballPosition = []
        self.targetPosition = []

    ### Destroy GUI
    def destroy(self, widget):
	print "destroyed"
	Gtk.main_quit()


    def __del__(self, widget):
	self.tracker.stopTracker(self)
	pass

    # If Nao has ball returns True
    def hasBall(self, widget):
        self.checkForBall(self)
        time.sleep(0.5)
        if self.checkForBall(self):
	    print "I see the Ball"
            return True
        else:
	    print "Sorry I cant find the ball"
            return False

    # Nao scans around for the redball
    def searchBall(self, widget):
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        for turnAngle in [0,math.pi/1.8,math.pi/1.8,math.pi/1.8]:
            if turnAngle > 0:
                self.motion.walkTo(0,0,turnAngle)
            if self.hasBall(self):
                self.turnToBall(self)
                return
            for headPitchAngle in [((math.pi*29)/180),((math.pi*12)/180)]:
                self.motion.angleInterpolation("HeadPitch", headPitchAngle,0.3,True)
                for headYawAngle in [-((math.pi*40)/180),-((math.pi*20)/180),0,((math.pi*20)/180),((math.pi*40)/180)]:
                    self.motion.angleInterpolation("HeadYaw",headYawAngle,0.3,True)
                    time.sleep(0.3)
                    if self.hasBall(self):
                        self.turnToBall(self)
                        return


    # Nao walks close to ball
    def walkToBall(self, widget):
        ballPosition = self.tracker.getPosition()
        headYawTreshold = ((math.pi*10)/180)
        x = ballPosition[0]/2 + 0.05
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.turnToBall(self)
        self.motion.post.walkTo(x,0,0)
        while True:
            headYawAngle = self.motion.getAngles("HeadYaw", False)
            if headYawAngle[0] >= headYawTreshold or headYawAngle[0] <= -headYawTreshold:
                self.motion.stopWalk()
                self.turnToBall(self)
                self.walkToBall(self)
                break
            #dist = self.getDistance()
            # if dist < 0.1:
            #     self.motion.stopWalk()
            #     self.turnToBall()
            #     self.safePosition()
            #     time.sleep(1)
            #     self.tts.say("You have 5 seconds to move the ball")
            #     time.sleep(5)
            #     # naoWalkToPosition()
            #     self.walkToPosition()
            #     self.setTopCamera()
            #     break
                return


#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import walkInit [as 别名]

#.........这里部分代码省略.........
        
    def manipulationInit(self, height, orientation=goal_orientation, rightSafe=right_safe, leftSafe=left_safe):
        ''' 
        manipulationInit method is used to set safe positions for both hands through rightSafe and leftSafe parameters, 
        which are organized as list with 6 entries [pos_x, pos_y, pos_z, rot_x, rot_y, rot_z]
        parameter height is used to set the clearance of the hand with respect to the playing field when playing, 
        which is done by increasing the local coordinates of goal points
        parameter orientation is a list containing 3 entries and denotes the orientation that robots hand will assume at the goal point
        '''
        
        ## added height for manipulation
        self.height = height
        
        ## goal point orientation
        self.orient = orientation
        
        ## safe positions for hands
        self.rightSafe = rightSafe
        self.leftSafe = leftSafe
        
    def stanceInit(self):
        '''
        Used to make robot stand and position itself correctly
        '''
        ## Shutting down awareness
        self.awareness.stopAwareness()
        
        ## Set stiffnesses, enableing robot to stand up.
        self.motion.setStiffnesses("Body", 1.0)
        self.motion.setStiffnesses("LArm", 0.0)
        self.motion.setStiffnesses("RArm", 0.0)
        self.posture.goToPosture("StandInit", 0.5)
        ## Go to walk init pose
        self.motion.walkInit()
        self.torso_default = self.motion.getPosition("Torso", 2, True)
        
        ## Release stiffnesses of the arms
        self.motion.setStiffnesses("RArm", 0.0)
        self.motion.setStiffnesses("LArm", 0.0)
        
        ## Release head stiffness
        self.motion.setStiffnesses("Head", 0.0)
        
        ## initialize manipulation
        ## TODO: remove hard coding of the goal position height clearance
        self.manipulationInit(0.025)
        
        print("[INFO ] Robot pose initialized")
        
    def getImage(self):
        '''
        Gets the image from NAO's camera by using getImageRemote method of ALVideoDevice module
        Converts alimage data into OpenCV format for further processing
        '''
        alimg = self.videoProxy.getImageRemote(self.video)
        cv2.cv.SetData(self.imgheader, alimg[6])
        self.img = np.asarray(self.imgheader[:,:])
        return self.img

    def checkStates(self):
        '''
        Check if all states are the same
        '''
        for i in range(len(self.states)-1):
            for j in range(len(self.states[i])):
                if not self.states[i][j] == self.states[i+1][j]:

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import walkInit [as 别名]
            walk_proxy.initPosition()
        elif nao_action == 4:
            walk_proxy.stopWalking()
        elif nao_action == 5:
            walk_proxy.walk()
        elif nao_action == 6:
            walk_proxy.setStiffness(0.5)
        elif nao_action == 7:
            walk_proxy.setStiffness(1.0)
            walk_proxy.initPosition()
        elif nao_action == 8:
            motion_proxy.stiffnessInterpolation("Body", 1.0, 0.1)
            motion_proxy.setWalkArmsEnabled(False, False)
            # enable motion whe lifted in the air
            motion_proxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", False]])
            motion_proxy.walkInit()

            # (X length, Y length, theta, frequency)
            motion_proxy.walkTo(0.8, 0.0, 0.0);
        elif nao_action == 9:
            # reset stiffness and angles using motion proxy,
            # otherwise it doesn't work well later
            motion_proxy.stiffnessInterpolation("Body", 0.0, 1.0)
            numAngles = len(motion_proxy.getJointNames("Body"))
            angles = [0.0] * numAngles
            motion_proxy.angleInterpolationWithSpeed ("Body", angles, 0.3)


    except Exception,e:
        print "Execution of the action was failed."
        exit(1)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import walkInit [as 别名]
class SimpleMotions:
    def __init__(self):
        self.motionProxy = ALProxy("ALMotion", Config.ROBOT_IP, Config.PORT)
        self.postureProxy = ALProxy("ALRobotPosture", Config.ROBOT_IP, Config.PORT)
        robotConfig = self.motionProxy.getRobotConfig()
        # print the robot configuration information
        for i in range(len(robotConfig[0])):
            print robotConfig[0][i], ": ", robotConfig[1][i]

    # turn on stiffness of body
    def stiffnessOn(self, motionProxy):
        allJoints = "Body"
        pStiffnessLists = 1.0
        pTimeLists = 1.0
        motionProxy.stiffnessInterpolation(allJoints, pStiffnessLists, pTimeLists)

    # turn off stiffness
    def stiffnessOff(self, motionProxy):
        allJoints = "Body"
        pStiffnessLists = 0.0
        pTimeLists = 1.0
        motionProxy.stiffnessInterpolation(allJoints, pStiffnessLists, pTimeLists)

    # make robot stand up
    def stand(self):  # def stand(self, name, speed):
        self.stiffnessOn(motionProxy=self.motionProxy)
        self.postureProxy.goToPosture("Stand", 0.4)

    # make the robot stand up fast
    def fastStand(self):  # def stand(self, name, speed):
        self.stiffnessOn(motionProxy=self.motionProxy)
        self.postureProxy.goToPosture("Stand", 1)

    # make the robot sit
    def sit(self):
        self.stiffnessOn(motionProxy=self.motionProxy)
        self.postureProxy.goToPosture("SitRelax", 1.0)

    '''
    make the robot move in a direction
    x: positive move forward, negative move backwards [-1.0 to 1.0]
    y: positive left, negative right [-1.0 to 1.0]
    theta: positive for counterclockwise, negative for clockwise [-1.0 to 1.0]
    speed: determines the frequency of the steps, so the velocity [0.0 to 1.0]
    '''
    def move(self, x, y, theta, speed):
        x = float(x)
        y = float(y)
        theta = float(theta)
        speed = float(speed)
        self.motionProxy.setWalkTargetVelocity(x, y, theta, speed)
        logObj.logWrite(time.time().__str__() + "_3_{0}_{1}_{2}_{3}".format(x,y,theta,speed))

    '''
    as an alternative and more controllable method of moving,
    this moves the robot a desired amount of cm in units of 4 cm. This method makes the robot move L R Fw Bw
    for rotation see the rotateTheta() method
    Config.DIRECTIONS = ["L", "R", "Fw", "Bw"]
    sends as output [time,action,dForwards,dSideways,dtheta,speed]

    input given in integer cm
    '''
    # TODO figure out how many m one footstep is, for all cases so L R Bw Fw
    def moveXYCm(self, x, y):
        self.stand()
        # convert from input string to integer
        x = int(x)
        y = int(y)
        action = 1
        theta = 0
        #self.standStraight()
        self.motionProxy.walkInit()

        # pos is L, neg is R
        if x == 0:
            amountStepsY, stepSizeY = self.getSteps(y)
            #stepSizeY from cm to m
            stepSizeY = float(stepSizeY)  # / 100 apparently not necessary
            amountStepsX = 0
            stepSizeX = 0
            if y > 0:
                positivity = True
                direction = Config.DIRECTIONS[0]
                stepSize = stepSizeY
                for i in xrange(0, amountStepsY):
                    if i % 2 == 0:
                        self.setStep(Config.LLEG, stepSizeX, stepSizeY, theta)
                        lastMovedLeg = Config.LLEG
                    else:
                        self.setStep(Config.RLEG, stepSizeX, stepSizeY, theta)
                        lastMovedLeg = Config.RLEG
            else:
                positivity = False
                direction = Config.DIRECTIONS[1]
                stepSize = -stepSizeY
                for i in xrange(0, amountStepsY):
                    if i % 2 == 0:
                        self.setStep(Config.RLEG, -stepSizeX, -stepSizeY, theta)
                        lastMovedLeg = Config.RLEG
                    else:
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import walkInit [as 别名]
class NaoWalk():
    # INCOMPLETE AT: walkUpToBall(), include MCC-CALL and delete walkToPosition()

    # Usage description:
    # When the NaoWalk Phase started, call
    #       hasBall()
    #
    # to determine if NAO has the Redball in vision.
    # If not, call
    #       retrieveBall()
    #
    # to find and turn to the ball
    # If NAO has a ball in vision, call
    #       walkUpToBall()
    #
    # NAO will walk up closely to the Redball and call the MCC to
    # request the NXT to move to his next position
    # After the NXT left his initial Position, call
    #       walkToPosition()
    #
    # and NAO makes his few last steps.
    #
    # Repeat this progress until NAO has reached
    # the target area.

    def __init__(self):
        self.myBroker = ALBroker("myBroker","0.0.0.0",0,"194.95.174.146",9559)
        self.motion = ALProxy("ALMotion")
        self.tracker = ALProxy("ALRedBallTracker")
        self.vision = ALProxy("ALVideoDevice")
        self.tts = ALProxy("ALTextToSpeech")
        self.currentCam = 0
        self.__setTopCamera()
        self.tracker.setWholeBodyOn(False)
        self.tracker.startTracker()
        self.ballPosition = []
        self.targetPosition = []
        # ALModule.__init__(self,name)

    def __del__(self):
        self.tracker.stopTracker()
        pass

    # determines whether NAO sees a redball or not
    # returns true is he sees one, false if not
    def hasBall(self):
        self.__checkForBall()
        time.sleep(0.5)
        if self.__checkForBall():
            return True
        else :
            return False

    # NAO scans his environment for the redball
    # after calling, NAO either has a redball in vision or there is none in
    # his range of sight.
    # Maybe include MCC-CALL after failure
    def retrieveBall(self):
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        for turnAngle in [0,math.pi/1.8,math.pi/1.8,math.pi/1.8]:
            if turnAngle > 0:
                self.motion.walkTo(0,0,turnAngle)
            if self.hasBall():
                self.__turnToBall()
                return
            for headPitchAngle in [((math.pi*29)/180),((math.pi*12)/180)]:
                self.motion.angleInterpolation("HeadPitch", headPitchAngle,0.3,True)
                for headYawAngle in [-((math.pi*40)/180),-((math.pi*20)/180),0,((math.pi*20)/180),((math.pi*40)/180)]:
                    self.motion.angleInterpolation("HeadYaw",headYawAngle,0.3,True)
                    time.sleep(0.3)
                    if self.hasBall():
                        self.__turnToBall()
                        return

    # lets the nao walk close up to the ball
    # Redball in vision is mandatory to call this method!
    # !! NEED TO INCLUDE MCC-CALL TO MOVE NXT TO NEXT POSITION IN LINE 85 !!
    def walkUpToBall(self):
        ballPosi = self.tracker.getPosition()
        headYawTreshold = ((math.pi*10)/180)
        x = ballPosi[0]/2 + 0.05
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.__turnToBall()
        self.motion.post.walkTo(x,0,0)
        while True:
            headYawAngle = self.motion.getAngles("HeadYaw", False)
            if headYawAngle[0] >= headYawTreshold or headYawAngle[0] <= -headYawTreshold:
                self.motion.stopWalk()
                self.__turnToBall()
                self.walkUpToBall()
                break
            dist = self.__getDistance()
            if dist < 0.65:
                self.motion.stopWalk()
                self.__turnToBall()
                self.__safePosition()
#                    # !!
#                    # meldung ans mcc dass nxt weiterlaufen soll
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import walkInit [as 别名]
class Nao():

    # Usage
    # Nao().searchBall()
    # Nao().hasBall()
    # Nao().walkToBall()
    # Nao().protectionOff()  should call before moves
    
    def __init__(self):
        self.myBroker = ALBroker("myBroker","0.0.0.0",0,"10.51.5.167",9559)
        self.motion   = ALProxy("ALMotion")
        self.tracker  = ALProxy("ALRedBallTracker")
        self.vision   = ALProxy("ALVideoDevice")
        self.tts      = ALProxy("ALTextToSpeech")
        self.currentCamera = 0
        self.setTopCamera()
        self.tracker.setWholeBodyOn(False)
        self.tracker.startTracker()
        self.ballPosition = []
        self.targetPosition = []


    def __del__(self):
        self.tracker.stopTracker()
        pass


    # If Nao has ball returns True
    def hasBall(self):
        self.checkForBall()
        time.sleep(0.5)
        if self.checkForBall():
            return True
        else:
            return False


    # Nao scans around for the redball
    def searchBall(self):
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        for turnAngle in [0,math.pi/1.8,math.pi/1.8,math.pi/1.8]:
            if turnAngle > 0:
                self.motion.walkTo(0,0,turnAngle)
            if self.hasBall():
                self.turnToBall()
                return
            for headPitchAngle in [((math.pi*29)/180),((math.pi*12)/180)]:
                self.motion.angleInterpolation("HeadPitch", headPitchAngle,0.3,True)
                for headYawAngle in [-((math.pi*40)/180),-((math.pi*20)/180),0,((math.pi*20)/180),((math.pi*40)/180)]:
                    self.motion.angleInterpolation("HeadYaw",headYawAngle,0.3,True)
                    time.sleep(0.3)
                    if self.hasBall():
                        self.turnToBall()
                        return


    # Nao walks close to ball
    def walkToBall(self):
        ballPosition = self.tracker.getPosition()
        headYawTreshold = ((math.pi*10)/180)
        x = ballPosition[0]/2 + 0.05
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.turnToBall()
        self.motion.post.walkTo(x,0,0)
        while True:
            headYawAngle = self.motion.getAngles("HeadYaw", False)
            if headYawAngle[0] >= headYawTreshold or headYawAngle[0] <= -headYawTreshold:
                self.motion.stopWalk()
                self.turnToBall()
                self.walkToBall()
                break
            #dist = self.getDistance()
            # if dist < 0.1:
            #     self.motion.stopWalk()
            #     self.turnToBall()
            #     self.safePosition()
            #     time.sleep(1)
            #     self.tts.say("You have 5 seconds to move the ball")
            #     time.sleep(5)
            #     # naoWalkToPosition()
            #     self.walkToPosition()
            #     self.setTopCamera()
            #     break
                return


    # nao turns to ball 
    def turnToBall(self):
        if not self.hasBall():
            return False
        self.ballPosition = self.tracker.getPosition()
        b = self.ballPosition[1]/self.ballPosition[0]
        z = math.atan(b)
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.motion.walkTo(0,0,z)


#.........这里部分代码省略.........