Python ALProxy.wbEnableEffectorControl方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableEffectorControl [as 别名]
def moveheadGoDown():
	print "::moveheadChange:"
	MP = ALProxy("ALMotion", ROBOT_IP_GLOBAL, PORT_GLOBAL)
	#MP.changeAngles(names,changes,fractionMaxSpee)
	#MP.changeAngles("HeadPitch", 0.15, 0.05)#Right	 --backup
	#MP.changeAngles("HeadPitch", ,,)
	MP.changeAngles("HeadPitch", 0.35, 0.20)#Right		was 0.35
	MP.wbEnableEffectorControl("Head", False)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableEffectorControl [as 别名]
def main(robotIP, PORT=9559):
    ''' Example of a whole body head orientation control
        Warning: Needs a PoseInit before executing
                 Whole body balancer must be inactivated at the end of the script
    '''

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    effectorName = "Head"

    # Active Head tracking
    isEnabled    = True
    motionProxy.wbEnableEffectorControl(effectorName, isEnabled)

    # Example showing how to set orientation target for Head tracking
    # The 3 coordinates are absolute head orientation in NAO_SPACE
    # Rotation in RAD in x, y and z axis

    # X Axis Head Orientation feasible movement = [-20.0, +20.0] degree
    # Y Axis Head Orientation feasible movement = [-75.0, +70.0] degree
    # Z Axis Head Orientation feasible movement = [-30.0, +30.0] degree

    targetCoordinateList = [
    [+20.0,  00.0,  00.0], # target 0
    [-20.0,  00.0,  00.0], # target 1
    [ 00.0, +70.0,  00.0], # target 2
    [ 00.0, +70.0, +30.0], # target 3
    [ 00.0, +70.0, -30.0], # target 4
    [ 00.0, -75.0,  00.0], # target 5
    [ 00.0, -75.0, +30.0], # target 6
    [ 00.0, -75.0, -30.0], # target 7
    [ 00.0,  00.0,  00.0], # target 8
    ]

    # wbSetEffectorControl is a non blocking function
    # time.sleep allow head go to his target
    # The recommended minimum period between two successives set commands is
    # 0.2 s.
    for targetCoordinate in targetCoordinateList:
        targetCoordinate = [target*math.pi/180.0 for target in targetCoordinate]
        motionProxy.wbSetEffectorControl(effectorName, targetCoordinate)
        time.sleep(3.0)

    # Deactivate Head tracking
    isEnabled = False
    motionProxy.wbEnableEffectorControl(effectorName, isEnabled)

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableEffectorControl [as 别名]
			elif (remoteInput[2] == 'KEY_POWER' or keyboard=="9"):
				break
			#								#
			# Main Menu for All Commands	#
			#								#
		
		
			if(keyboard=="9911" or keyboard=="9927"):#Only if Standing!
				print "::Standing"
			elif(curpos == newpos):
				print("::HEY!:: same position!!")
				#motionProxy.rest()
				#time.sleep(0)
			else:
				motionProxy.setStiffnesses("Head", 1.0)
				motionProxy.wbEnableEffectorControl("Head", True)
				movehead(target,motionProxy)
				motionProxy.wbEnableEffectorControl("Head", False)
				curpos = newpos
		# End While
	except KeyboardInterrupt:
		print
		print "Interrupted by user"
		print "Stopping..."

	# Stop tracker.
	if remoteInput[2] == 'KEY_POWER':
		print("Power hit")		
	motionProxy.rest()
	

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableEffectorControl [as 别名]
def main(robotIP, PORT, chainName):
    ''' Example of a whole body Left or Right Arm position control
        Warning: Needs a PoseInit before executing
                 Whole body balancer must be inactivated at the end of the script
    '''

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    #postureProxy.goToPosture("StandInit", 0.5)

    frame     = motion.FRAME_ROBOT
    useSensor = False

    effectorInit = motionProxy.getPosition(chainName, frame, useSensor)

    # Active LArm tracking
    isEnabled = True
    motionProxy.wbEnableEffectorControl(chainName, isEnabled)

    # Example showing how to set position target for LArm
    # The 3 coordinates are absolute LArm position in FRAME_ROBOT
    # Position in meter in x, y and z axis.

    # X Axis LArm Position feasible movement = [ +0.00, +0.12] meter
    # Y Axis LArm Position feasible movement = [ -0.05, +0.10] meter
    # Y Axis RArm Position feasible movement = [ -0.10, +0.05] meter
    # Z Axis LArm Position feasible movement = [ -0.10, +0.10] meter

    coef = 1.0
    if chainName == "LArm":
        coef = +1.0
    elif chainName == "RArm":
        coef = -1.0

    targetCoordinateList = [
    [ +0.12, +0.00*coef, +0.00], # target 0
    [ +0.12, +0.00*coef, -0.10], # target 1
    [ +0.12, +0.05*coef, -0.10], # target 1
    [ +0.12, +0.05*coef, +0.10], # target 2
    [ +0.12, -0.10*coef, +0.10], # target 3
    [ +0.12, -0.10*coef, -0.10], # target 4
    [ +0.12, +0.00*coef, -0.10], # target 5
    [ +0.12, +0.00*coef, +0.00], # target 6
    [ +0.00, +0.00*coef, +0.00], # target 7
    ]


    # wbSetEffectorControl is a non blocking function
    # time.sleep allow head go to his target
    # The recommended minimum period between two successives set commands is
    # 0.2 s.
    for targetCoordinate in targetCoordinateList:
        targetCoordinate = [targetCoordinate[i] + effectorInit[i] for i in range(3)]
        motionProxy.wbSetEffectorControl(chainName, targetCoordinate)
        time.sleep(4.0)

    # Deactivate Head tracking
    isEnabled    = False
    motionProxy.wbEnableEffectorControl(chainName, isEnabled)

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableEffectorControl [as 别名]
        port = 9559
        myBroker = ALBroker("myBroker", #I'm not sure that pyrobots doesn't already have one of these open called NAOqi?
                "0.0.0.0",   # listen to anyone
                0,           # find a free port and use it
                NAO_IP,      # parent broker IP
                port)        # parent broker port
        motionProxy = ALProxy("ALMotion", NAO_IP, port)

        postureProxy = ALProxy("ALRobotPosture", NAO_IP, port)
        textToSpeech = ALProxy("ALTextToSpeech", NAO_IP, port)   
        textToSpeech.setLanguage(LANGUAGE.capitalize())
        #textToSpeech.setVolume(1.0)
        if naoWriting:
            if naoStanding:
                postureProxy.goToPosture("StandInit",0.2)
                motionProxy.wbEnableEffectorControl(effector, True) #turn whole body motion control on
            else:
                motionProxy.rest()
                motionProxy.setStiffnesses(["Head", "LArm", "RArm"], 0.5)
                motionProxy.setStiffnesses(["LHipYawPitch", "LHipRoll", "LHipPitch", "RHipYawPitch", "RHipRoll", "RHipPitch"], 0.8)
                motionProxy.wbEnableEffectorControl(effector, False) #turn whole body motion control off

            armJoints_standInit = motionProxy.getAngles(effector,True)

    #initialise word manager (passes feedback to shape learners and keeps history of words learnt)
    InteractionSettings.setDatasetDirectory(datasetDirectory)
    wordManager = ShapeLearnerManager(InteractionSettings.generateSettings, SHAPE_LOGGING_PATH)
    textShaper = TextShaper()
    screenManager = ScreenManager(0.2, 0.1395)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableEffectorControl [as 别名]
    # NAOqi modules and subscribe to other modules
    # The broker must stay alive until the program exists
    port = 9559;
    myBroker = ALBroker("myBroker", #I'm not sure that pyrobots doesn't already have one of these open called NAOqi?
        "0.0.0.0",   # listen to anyone
        0,           # find a free port and use it
        NAO_IP,      # parent broker IP
        port)        # parent broker port
    hasFallen = False;
    motionProxy = ALProxy("ALMotion", NAO_IP, port);
    memoryProxy = ALProxy("ALMemory", NAO_IP, port);
    postureProxy = ALProxy("ALRobotPosture", NAO_IP, port)
    fallResponder = FallResponder("fallResponder",motionProxy,memoryProxy);
    pub_traj = rospy.Subscriber(TRAJ_TOPIC, Path, on_traj)
    
    motionProxy.wbEnableEffectorControl(effector,False); #if robot has fallen it will have a hard time getting up if the effector is still trying to be kept in a particular position
    postureProxy.goToPosture("StandInit", 0.5)

    tl = tf.TransformListener()

    motionProxy.wbEnableEffectorControl(effector,True);
    rospy.sleep(2)

    space      = 2
    axisMask   = AXIS_MASK_X+AXIS_MASK_Y+AXIS_MASK_Z+AXIS_MASK_WX#+AXIS_MASK_WY#+AXIS_MASK_WY#control all the effector's axes 7 almath.AXIS_MASK_VEL    # just control position
    isAbsolute = True
    
    
    rospy.spin()
    myBroker.shutdown()
    #nao.execute([naoqi_request("motion","rest",[])]);

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

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

        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)
        time.sleep(0.5)
        
        
        ## obtain current postion and elevate the arm        

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

	# Subscribe to NAO Speech function
	# Test To Speech is setup with Proxy: ALTextToSpeech	
	try:
		tts = ALProxy("ALTextToSpeech", ROBOT_IP_GLOBAL, PORT_GLOBAL)
	except Exception, e:
		print "::ALTextToSpeech::",e
	
	try:												# Sets up proxy so head movement is enabled
		motionProxy = ALProxy("ALMotion", ROBOT_IP_GLOBAL, PORT_GLOBAL)
	except Exception, e:
		print "::ALMotion::",e
		
	motionProxy.setStiffnesses("Head", 1.0)				# enables stiffness 
	motionProxy.wbEnableEffectorControl("Head", True)	# enables head movement
	MOTION_PROXY_GLOBAL = motionProxy
	moveHeadOrigin(MOTION_PROXY_GLOBAL)					# moves head to origin or face the participant
	counter = 0											# initialize the counter to 0
	# End Setup
	
														# 1=English
	naoTalksLangauage(tts,1,"Ready, Lets begin")		# Speech before condition starts
	while True:
		changeSlideOnTabletDirection('right')			# Change slide
		time.sleep(0.50)								# Pause
		naoTalksLangauage(tts,2,str(word_list[int(counter)])) #JAP Reading of Words 
		time.sleep(1.00)								# Pause
		time.sleep(1.0)									# Pause
		naoTalksLangauage(tts,2,str(word_list[int(counter)])) #JAP Reading of Words 
		time.sleep(3.5)									# Pause