Python ALProxy.closeHand方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import closeHand [as 别名]
def main(ip, port):
    motion = ALProxy("ALMotion", ip, port)

    motion.wakeUp()
    print motion.getSummary()
    time.sleep(2)

    motion.openHand('LHand')
    time.sleep(2)

    motion.closeHand('LHand')
    time.sleep(2)

    motion.rest()
    print motion.getSummary()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import closeHand [as 别名]
class ReactToTouch(ALModule):
	""" A simple module able to react
		to touch events.
	"""
	def __init__(self, name):
		ALModule.__init__(self, name)
		# No need for IP and port here because
		# we have our Python broker connected to NAOqi broker

		# Create a proxy to ALMotion for later use
		self.motionProxy = ALProxy("ALMotion")

		# Subscribe to TouchChanged event:
		global memory
		memory = ALProxy("ALMemory")
		memory.subscribeToEvent("TouchChanged","ReactToTouch","onTouched")
		
		self.boolean = True
		

	def loop(self):
		while self.boolean: 
			pass


	def onTouched(self, strVarName, value):
		""" This is called each time a touch
		is detected.	
		QUESTION HERE IS WHY DOES IT DETEC A TOUCH ON RIGHT HAND EVEN IF RIGHT HAND IS NOT BEING TOUCHED...
		WHICH IS WHY I HAD TO USE SPECIFICALLY "IF BACK OF RIGHT HAND IS TOUCHED...DO SMTHG"
		"""
		
		""" we could also do this without even using this whole class:
		for p in value:
			if (p[0] == "RHand/Touch/Back" AND p[1] == True):   do smthg"""

		print("touched activated")
		
		for p in value:
				print ("p[0] is "+p[0])
				if (p[0]=="RHand/Touch/Back"):
						# Unsubscribe to the event when talking,
						# to avoid repetitions
						memory.unsubscribeToEvent("TouchChanged","ReactToTouch")
						#close hand
						self.motionProxy.closeHand("RHand")
						self.boolean = False

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

#.........这里部分代码省略.........
            angles = [x * self.TO_RAD for x in angles]

        # Perform te movement
        self.__Motion.changeAngles(names, angles, max_speed)

    def set_angles(self, names, angles, max_speed, disable_stiffness=False, radians=False):
        """
        This method will set the angles for the joints in the list of names.
        To make sure the joints actually move, stiffness is set on these joints.
        CAUTION: This method sometimes results in very sudden movements of the NAO.
                 If you experience this and want to avoid it, call set_stiffness
                 on these joints before calling set_angles, as the sudden motion
                 results from the stifness being increased in a very short
                 amount of time. set_stiffness allows to do this in a more
                 subtle fashion.
        """
        if not radians:
            angles = [x * self.TO_RAD for x in angles]

        # Perform te movement
        self.__Motion.setAngles(names, angles, max_speed)

    def get_angles(self, names, radians=False):
        useSensors  = False     # Cannot use sensors in simulation :(
        angles = self.__Motion.getAngles(names, useSensors)
        if not radians:
            angles = [x / self.TO_RAD for x in angles]
        return angles

    def open_hand(self, hand):
        return self.__Motion.openHand(hand)

    def close_hand(self, hand):
        return self.__Motion.closeHand(hand)

    def get_proxy(self, which = "motion"):
        if which == "motion":
            return self.__Motion
        elif which == "tts":
            return self.__TTS
        elif which == "video":
            return self.__Video
        elif which == "frame":
            return self.__FM
        elif which == "memory":
            return self.__Memory
        elif which == "vision":
            return self.__VisionTB
        elif which == "balltracker":
            return self.__BallTracker
        elif which == "navigation":
            return self.__Navigation
        elif which == "leds":
            return self.__Leds

    def emergency(self):
        """Disable the NAO stiffness so it stops moving"""
        self.logger.warn("Emergency button pressed")
        self.set_stifness(['Body'], [0], [0.25])
        self.say("My emergency button has been pressed. I am now in emergency mode")
    
    def emergencyLeds(self, mode):
        
        if mode:
            # Switch the new group on
            self.__Leds.on("Emergency")

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

#.........这里部分代码省略.........
        if self.mode == 'x':
            self.board[i][j] = 1
        else:
            self.board[i][j] = 0

        return result, nameEffector, nameHand
    
    def playMove(self, nextMove):
        '''
        Moves nameHand with nameEffector to the goalPos using proxy to ALMotion
        TODO: rewrite to utilize PositionInterpolation method from ALMotion
        '''
        
        ## unpack next move
        goalPos = nextMove[0]
        nameEffector = nextMove[1]
        nameHand = nextMove[2]
        print("[INFO ] Put object in %s and touch arm tactile sensor" % nameHand)
        ## open hand
        self.motion.openHand(nameHand)
        ## wait for the object to be placed in the hand
        while True:
            if nameEffector == 'RArm':
                val1 = self.memory.getData("HandRightLeftTouched")
                val2 = self.memory.getData("HandRightBackTouched")
                val3 = self.memory.getData("HandRightRightTouched")
            else:
                val1 = self.memory.getData("HandLeftLeftTouched")
                val2 = self.memory.getData("HandLeftBackTouched")
                val3 = self.memory.getData("HandLeftRightTouched")   
            if val1 or val2 or val3:
                break
        ## close hand
        self.motion.closeHand(nameHand)
        
        ## enable control of the arm
        print("[INFO ]Enableing whole body motion")
        self.motion.wbEnableEffectorControl(nameEffector, True)
        
        ## extract current position and elevate the hand
        currPos = self.motion.getPosition(nameEffector,2, True)
        currPos[0] += 0.00
        currPos[2] += 0.06
        self.motion.closeHand(nameHand)
        self.motion.setStiffnesses(nameEffector, 1.0)
        
        # lift the hand
        print("[INFO ]Lifting the hand")
        self.motion.positionInterpolations(nameEffector, 2, currPos, 7, 3)
        ## extract goal position and move arm towards it
        goalPosition = [goalPos[0,0], goalPos[1,0], goalPos[2,0]+self.height+0.08, 0.0, 0.0, 0.0]
        midPoint = [(goalPosition[0]+currPos[0])/2, (goalPosition[1]+currPos[1])/2, goalPosition[2], currPos[3], currPos[4], currPos[5]]
        print("[INFO ]Moving to midpoint")
        self.motion.positionInterpolations(nameEffector, 2, midPoint, 7, 3)

        print("[INFO ]Going to goal position")
        self.motion.positionInterpolations(nameEffector, 2, goalPosition, 7, 3)
        goalPosition[3]=0
        goalPosition[4]=0

        self.motion.positionInterpolations(nameEffector, 2, goalPosition, 7, 3)
        goalPosition[2]-=0.08
        self.motion.positionInterpolations(nameEffector, 2, goalPosition, 63, 3)
        ## open hand to release the object
        time.sleep(0.5)
        self.motion.openHand(nameHand)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import closeHand [as 别名]
class DeviceHumanoidMotion(HumanoidMotion):
    
    # Initialization of NAO proxies
    def __init__(self, parameters):

        self.nao_ip = parameters["nao_ip"]
        self.nao_port = int(parameters["nao_port"])

        self.posture = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
        self.motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)

        self.allowed_joints = ['HeadYaw', 'LShoulderPitch', 'LHipYawPitch1', \
                'RHipYawPitch1', 'RShoulderPitch', 'HeadPitch', 'LShoulderRoll', \
                'LHipRoll', 'RHipRoll', 'RShoulderRoll', \
                'LElbowYaw', 'LHipPitch', 'RHipPitch', 'RElbowYaw', \
                'LElbowRoll', 'LKneePitch', 'RKneePitch', 'RElbowRoll', \
                'LWristYaw2', 'LAnklePitch', 'RAnklePitch', 'RWristYaw2', \
                'LHand2', 'RAnkleRoll', 'LAnkleRoll', 'RHand2']

    # Assistive function to return errors
    def ret_exc(self, text):
        print text
        return {'error': text}

    # Sets a humanoid robot's joints. 'joints' are the names of the joints.
    # The nonmeclature used is the one of the NAO robot:
    # http://doc.aldebaran.com/2-1/family/robots/bodyparts.html#nao-chains
    # Angles are in rads.
    # Speed must be between [0,1], where 1 is the maximum nominal speed
    def setJointAngles(self, joints, angles, speed):
 
        if type(joints) != list or type(angles) != list:
            return self.ret_exc('humanoid_motion.setJointAngles: Joints or angles \
                    not lists')
        if len(joints) == 0 or len(angles) == 0:
            return self.ret_exc('humanoid_motion.setJointAngles: Empty joints \
                    or angles')

        for j in joints:
            if type(j) != str:
                return self.ret_exc('humanoid_motion.setJointAngles: A joint is \
                        not string')
            if j not in self.allowed_joints:
                return self.ret_exc('hunanoid_motion.setJointAngles: Not a valid \
                        joint: ' + str(j))
        if speed <= 0 or speed > 1:
            return self.ret_exc('humanoid_motion.setJointAngles: Speed out of bounds')

        for a in angles:
            if type(a) not in [float, int]:
                return self.ret_exc('humanoid_motion.setJointAngles: Angle not a number')

        try:
            self.motion.setAngles(joints, angles, speed)
        except Exception as e:
            return self.ret_exc("humanoid_motion.setJointAngles: Unrecognized exception: "\
                    + e.message)

        return {'error': None}

    # Returns the joint angles of a humanoid. Again 'joints' are the joints'
    # names, which can be found here:
    # http://doc.aldebaran.com/2-1/family/robots/bodyparts.html#nao-chains
    def getJointAngles(self, joints):
        for i in joints:
            if i not in self.allowed_joints:
                return self.ret_exc('hunanoid_motion.getJointAngles: Not a valid \
                        joint: ' + str(j))

        try:
            data = self.motion.getAngles(joints, False) 
        except Exception as e:
            return self.ret_exc("humanoid_motion.setJointAngles: Unrecognized exception: "\
                    + e.message)

        return {'angles': data, 'error': None}

    # Opens a hand of a humanoid robot. The input must be 'Right' or 'Left'
    def openHand(self, hand_name):
        if hand_name not in ['Right', 'Left']:
            return self.ret_exc('humanoid_motion.openHand: Unsupported hand name: ' +\
                    str(hand_name))
        try:
            if hand_name == "Right":
                self.motion.openHand('RHand')
            elif hand_name == "Left":
                self.motion.openHand('LHand')
        except Exception as e:
            return self.ret_exc("humanoid_motion.openHand: Unrecognized exception: "\
                    + e.message)

        return {'error': None}

    # Closes a NAO's hand. The input must be 'Right' or 'Left'
    def closeHand(self, hand_name):
        if hand_name not in ['Right', 'Left']:
            return self.ret_exc('humanoid_motion.closeHand: Unsupported hand name: ' +\
                    str(hand_name))

        if hand_name == "Right":
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import closeHand [as 别名]
#!/usr/bin/python
# -*- coding: utf-8 -*-
from naoqi import ALProxy
from naoconfig import *

proxyMo = ALProxy('ALMotion',robot_IP,robot_port)

proxyMo.post.openHand('LHand')
proxyMo.openHand('RHand')

proxyMo.post.closeHand('LHand')
proxyMo.closeHand('RHand')

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import closeHand [as 别名]
class NaoMove():
	# Class used to make all movements to Nao
	def __init__(self, parent):
		#Constructor
		
		self.motionProxy = None
		self.postureProxy = None
		self.tts = None
		self.memoryProxy = None
		self.behaviourProxy = None
		
		self.motorOn = False
		self.parent = parent
		
		self.memValue = ["FaceDetected"]
		
		self.headNames = ["HeadYaw", "HeadPitch"]
		self.leftArmNames = ["LShoulderRoll","LShoulderPitch","LElbowRoll","LElbowYaw"]
		self.rightArmNames = ["RShoulderRoll","RShoulderPitch","RElbowRoll","RElbowYaw"]
		
	def connect(self, robotIP, port):
		try:
			self.motionProxy = ALProxy("ALMotion", robotIP , port)
			self.tts = ALProxy("ALTextToSpeech", robotIP, port)
			self.postureProxy = ALProxy("ALRobotPosture", robotIP, port)
			self.memoryProxy = ALProxy("ALMemory", robotIP, port)
			self.behaviourProxy = ALProxy("ALBehaviorManager", robotIP, port)
		except:
			return 1
			
		# If connection is successful, we initialize few variables for Arms	
		self.anglesRight = self.motionProxy.getAngles(self.rightArmNames, True)
		self.anglesLeft = self.motionProxy.getAngles(self.leftArmNames, True)
		return 0
		
		
	def walk(self,x,y,theta,frequency):
		# Test if the stiffness of Nao is set to 1.0
		self.testMotorOn()
		
		# Launch the walk
		self.motionProxy.setWalkTargetVelocity(x, y, theta, frequency)
		
	def stopWalk(self, frequency):
		self.motionProxy.setWalkTargetVelocity(0.0, 0.0, 0.0, frequency)
		self.motionProxy.waitUntilMoveIsFinished()
		
	def stop(self):
		self.motionProxy.killMove()
		
	def standInit(self):
		self.postureProxy.goToPosture("StandInit", constante.FREQUENCY)
		
	def crouch(self):
		self.postureProxy.goToPosture("Crouch", constante.FREQUENCY)
		
	def runHello(self):
		self.testMotorOn()
		
		self.behaviourProxy.runBehavior("helloBoy")
		
	def openHand(self, hand):
		if hand == constante.RIGHT_HAND:
			self.motionProxy.openHand('RHand')
		else:
			self.motionProxy.openHand('LHand')
	
	def closeHand(self, hand):
		if hand == constante.RIGHT_HAND:
			self.motionProxy.closeHand('RHand')
		else:
			self.motionProxy.closeHand('LHand')
			
	def moveHead(self, angles):
		# Test if the stiffness of Nao is set to 1.0
		self.testMotorOn()
		
		current = self.motionProxy.getAngles(self.headNames, True)
		
		# this boucle is used to dynamically calculate the speed of the movement
		for i in range(len(angles)):
			speed = abs(angles[i]-current[i])/2.0
			
			if speed < 0.05:
				speed = 0.05
			if speed > 0.8:
				speed = 0.8
				
			# Make the movement
			self.motionProxy.setAngles(self.headNames[i], angles[i], speed)
			
	def detectFace(self):
		# Check that it is not already running.
		if (not self.behaviourProxy.isBehaviorRunning("faceDetect")):
			# Launch behavior. This is a blocking call, use post if you do not
			# want to wait for the behavior to finish.
			self.behaviourProxy.post.runBehavior("faceDetect")
		
	def naoMoveTwoArms(self, listPos):
		# Test if the stiffness of Nao is set to 1.0
#.........这里部分代码省略.........

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

#.........这里部分代码省略.........
    def moveHead(self, pitch, yaw, radians=False):
        """
        :param pitch: the future pitch of NAO's head
        :param yaw: the future yaw of NAO's head
        :param radians: True if the angles are given in radians. False by default => angles in degree.
        """
        joint_names = ["HeadYaw", "HeadPitch"]
        if not radians:
            yaw = math.radians(yaw)
            pitch = math.radians(pitch)
        angles = [yaw, pitch]
        self.motion_proxy.angleInterpolation(joint_names, angles, 1., True)

    def compareToLeftHandPosition(self, coord, must_print=False):
        """
        :param coord: the 6D coordinates to compare with the left hand position
        :type coord: list
        :return: the difference of position from the hand to the given coordinates [x-axis, y-axis]
        :rtype: np.array
        """
        hand_coord = self.getLeftHandPosition()
        if must_print:
            print coord
            print hand_coord
        return geom.vectorize(hand_coord[0:2], coord[0:2])

    def playDisc(self, hole_coordinates):
        """
        :param hole_coordinates: The 6D translation + rotation vector of the hole
        """
        self.stand()
        self.setLeftHandPosition(hole_coordinates, mask=63)
        self.motion_proxy.openHand("LHand")
        self.motion_proxy.closeHand("LHand")
        self.setLeftArmAlongsideBody()

    def setLeftArmRaised(self, secure=False):
        """
        Raise NAO's left arm to the sky
        """
        self.stand()
        if secure:
            self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_BOTTOM, 2., True)
            self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_TOP, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, RAISED, 2., True)

    def setLeftArmAlongsideBody(self):
        """
        Move the left arm of NAO alongside his body.
        """
        self.stand()
        self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_TOP, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_BOTTOM, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, ARM_ALONGSIDE_BODY, 2., True)

    def setRightArmAlongsideBody(self):
        """
        Move the left arm of NAO alongside his body.
        """
        self.stand()
        self.motion_proxy.angleInterpolation(RARM_CHAIN, ARM_ALONGSIDE_BODY_R, 2., True)

    def setLeftArmToAskingPosition(self):
        """
        Move the left arm of NAO in a "asking disc" position, and open his left hand.
        """

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

    def __init__(self):
        self.animated_speech_proxy = None
        self.motion_proxy = None
        self.posture_proxy = None
        self.awareness_proxy = None
        self.autonomous_move_proxy = None
        self.autonomous_life_proxy = None

    def connect(self, host, port):
        """Takes connection params and builds list of ALProxies"""
        print 'Core - Connecting to robot on {0}:{1}...'.format(host, port)
        try:
            self.animated_speech_proxy = ALProxy("ALAnimatedSpeech", host, port)
            self.motion_proxy = ALProxy("ALMotion", host, port)
            self.posture_proxy = ALProxy("ALRobotPosture", host, port)
            self.awareness_proxy = ALProxy("ALBasicAwareness", host, port)
            self.autonomous_move_proxy = ALProxy("ALAutonomousMoves", host, port)
            self.autonomous_life_proxy = ALProxy("ALAutonomousLife", host, port)
        except Exception as exception: # pylint: disable=broad-except
            raise Exception('Could not create proxy:{0}', format(exception))
        self.set_autonomous_life(False)

    def say(self, animated_speech):
        self.animated_speech_proxy.say(animated_speech) 

    def move(self, rotation, distance):
        motion = self.motion_proxy
        motion.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
        motion.setMotionConfig([["ENABLE_STIFFNESS_PROTECTION", True]])
        motion.setCollisionProtectionEnabled('Arms', True)
        motion.setExternalCollisionProtectionEnabled('All', True)
        motion.moveTo(0, 0, rotation)
        motion.moveTo(distance, 0, 0)
        
    def open_hand(self, hand):
        self.motion_proxy.openHand(hand)
 
    def close_hand(self, hand):
        self.motion_proxy.closeHand(hand)

    def set_stiffness(self, joint, stiffness):
        print 'Setting {0} to stiffness {1}...'.format(joint, stiffness)
        self.motion_proxy.stiffnessInterpolation(joint, stiffness, 1.0) 
        
    def set_joint_angle(self, joint, angle_degrees, speed=0.1):
        print 'Setting {0} to {1} angle in degrees...'.format(joint, angle_degrees)
        self.motion_proxy.angleInterpolationWithSpeed(joint, math.radians(angle_degrees), speed)
        
    def set_pose(self, pose):
        self.posture_proxy.goToPosture(pose, 0.5) 
      
    def set_autonomous_life(self, set_on):
        self.print_sub_system_update(set_on, 'Autonomous Life')
        target_state = 'solitary' if set_on else 'disabled'
        self.autonomous_life_proxy.setState(target_state)
        
    def get_autonomous_life_state(self):
        return self.autonomous_life_proxy.getState()
  
    def set_autonomous_moves(self, set_on):
        self.print_sub_system_update(set_on, 'Autonomous Moves')
        target_state = 'backToNeutral' if set_on else 'none'
        self.autonomous_move_proxy.setBackgroundStrategy(target_state)
        
    def set_awareness(self, set_on):
        self.print_sub_system_update(set_on, 'Basic Awareness')
        if set_on:
            self.awareness_proxy.startAwareness() 
        else:
            self.awareness_proxy.stopAwareness()

    def set_breathing(self, set_on):
        self.print_sub_system_update(set_on, 'body breathing')
        self.motion_proxy.setBreathEnabled('Body', set_on)
        self.print_sub_system_update(set_on, 'arm breathing')
        self.motion_proxy.setBreathEnabled('Arms', set_on)

    def set_move_arms_enabled(self, left_arm, right_arm):
        self.motion_proxy.setMoveArmsEnabled(left_arm, right_arm)

    @staticmethod
    def print_sub_system_update(set_on, sub_process):
        on_off = ['off', 'on']
        print 'Turning {0} {1}...'.format(on_off[set_on], sub_process)