Python ALProxy.angleInterpolationWithSpeed方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import angleInterpolationWithSpeed [as 别名]
class MoveNao:
    def __init__(self, ip, port):
        self.__proxy = ALProxy("ALMotion", ip, port)
        self.__proxyPosture = ALProxy("ALRobotPosture", ip, port)
        self.__walk_sub = rospy.Subscriber(
            _Constants.joy_topic,
            Walk_control,
            self.walk)
            
        self.__subs = rospy.Subscriber(
		    _Constants.msg_topic,
		    String,
		    self.go_to_posture)	
		    	    
        self.__move_head_sub = rospy.Subscriber(
            _Constants.move_head_topic,
            MoveHead,
            self.look )
            
    def walk(self, msg):
        angular = msg.angular
        linear = msg.linear
       
        self.__proxy.move(
            linear * _Constants.linear_factor, # Forwards
            0.0, #Sideways
            angular * _Constants.angular_factor ) #Turning
            
    def go_to_posture(self, msg):
        self.__proxyPosture.goToPosture(msg.data, 1.0)
        
    def look(self, msg):
        joint_names = ["HeadYaw", "HeadPitch"]
        current = self.__proxy.getAngles( joint_names, False )
        yaw = current[0] + msg.yaw
        pitch = current[1] + msg.pitch
        #rospy.logwarn("got here 0")
        #rospy.logwarn(yaw)
        #rospy.logwarn(pitch)
        #Make sure we don't exceed angle limits
        if yaw < _Constants.yaw_limits[0]:
            yaw = _Constants.yaw_limits[0]
        elif yaw > _Constants.yaw_limits[1]:
            yaw = _Constants.yaw_limits[1]

        if pitch < _Constants.pitch_limits[0]:
            pitch = _Constants.pitch_limits[0]
        elif pitch > _Constants.pitch_limits[1]:
            pitch = _Constants.pitch_limits[1]
        #rospy.logwarn("got here 1")
        self.__proxy.setStiffnesses("Head", 1.0)
        self.__proxy.angleInterpolationWithSpeed(
            joint_names,
            [ yaw, pitch ],
            _Constants.head_speed)
        time.sleep(0.5)
        self.__proxy.setStiffnesses("Head", 0.0)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import angleInterpolationWithSpeed [as 别名]
class Golf(ALModule):
    def __init__(self, name, robotIP, port):
        ALModule.__init__(self, name)
        self.robotIP = robotIP
        self.port = port
        self.memory = ALProxy("ALMemory")
        self.motion = ALProxy("ALMotion")
        self.posture = ALProxy("ALRobotPosture")
        self.memory.subscribeToEvent("ALChestButton/TripleClickOccurred", "myGolf", "chestButtonPressed")
        self.memory.subscribeToEvent("FrontTactilTouched", "myGolf", "frontTactilTouched")
        self.memory.subscribeToEvent("MiddleTactilTouched", "myGolf", "middleTactilTouched")
        self.memory.subscribeToEvent("RearTactilTouched", "myGolf", "rearTactilTouched")
        self.memory.subscribeToEvent("robotHasFallen", "myGolf", "fallDownDetected")

    def frontTactilTouched(self):
        print "frontTactilTouched!!!!!!!!!!!!!"
        self.memory.unsubscribeToEvent("FrontTactilTouched", "myGolf")
        global holeFlag
        holeFlag = 1
        self.memory.subscribeToEvent("FrontTactilTouched", "myGolf", "frontTactilTouched")

    def middleTactilTouched(self):
        print "middleTactilTouched!!!!!!!!!!!!!!!!!!"
        self.memory.unsubscribeToEvent("MiddleTactilTouched", "myGolf")
        global holeFlag
        holeFlag = 2
        self.memory.subscribeToEvent("MiddleTactilTouched", "myGolf", "middleTactilTouched")

    def rearTactilTouched(self):
        print "rearTactilTouched!!!!!!!!!!!!!!!!!!"
        self.memory.unsubscribeToEvent("RearTactilTouched", "myGolf")
        global holeFlag
        holeFlag = 3
        self.memory.subscribeToEvent("RearTactilTouched", "myGolf", "rearTactilTouched")

    def chestButtonPressed(self):
        print "chestButtonPressed!!!!!!!!!!!!!!!!"
        self.memory.unsubscribeToEvent("ALChestButton/TripleClickOccurred", "myGolf")
        global holeFlag
        holeFlag = 0
        self.motion.angleInterpolationWithSpeed("LHand", 0.8, 1)
        time.sleep(2)
        self.motion.angleInterpolationWithSpeed("LHand", 0.2, 1)
        self.motion.rest()
        self.memory.subscribeToEvent("ALChestButton/TripleClickOccurred", "myGolf", "chestButtonPressed")

    def fallDownDetected(self):
        print "fallDownDetected!!!!!!!!!!!!!!!!"
        self.memory.unsubscribeToEvent("robotHasFallen", "myGolf")
        global robotHasFallenFlag
        robotHasFallenFlag = 1
        self.posture.goToPosture("StandInit", 0.5)
        releaseStick(self.robotIP, self.port)
        time.sleep(15)
        catchStick(self.robotIP, self.port)
        standWithStick(0.1, self.robotIP, self.port)
        self.memory.subscribeToEvent("robotHasFallen", "myGolf", "fallDownDetected")

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import angleInterpolationWithSpeed [as 别名]
class NaoSoundTrack(ALModule):
    '''
    This is the module that connects remotely to the NAO microphones
    '''
    def __init__(self, name, ip, port):
        #Init parent
        super(NaoSoundTrack, self).__init__(name)
        
        self.__ip = ip
        self.__port = port
        
        #self.__pub = rospy.Publisher(_Constants.topic, MoveHead , latch=True, queue_size =1000)
        
        self.__facepub = rospy.Publisher(_Constants.face_det_topic, Face_detection , latch=True, queue_size =1000)
        
        self.__CNCsub = rospy.Subscriber(_Constants.face_track_topic, String, self.start_sound_track)
        
        self.__faceRessub = rospy.Subscriber(_Constants.face_res_topic, Face_detection, self.stop_sound_track)
       
    
    def start_sound_track(self, msg):
        self.__proxyTTS = ALProxy("ALAnimatedSpeech", self.__ip, self.__port)
        
        # set the local configuration
        sayconfig = {"bodyLanguageMode":"contextual"}
        
        self.__proxyTTS.say("Can you help me find you by clapping your hand?", sayconfig)
        
        self.__proxyMotion = ALProxy("ALMotion", self.__ip, self.__port)
        
        #initialise microphone
        #self.__audioProxy = ALProxy("ALAudioDevice", self.__ip, self.__port)
        #initialise soundsourcelocalisation
        self.__sslProxy = ALProxy("ALSoundLocalization", self.__ip, self.__port)
        #initialise almemory
        self.__memoryProxy = ALProxy("ALMemory", self.__ip, self.__port)
        #debugging purpose
        #self.__audioProxy.setClientPreferences( self.getName() , 16000, 3, 0 )
        #self.__audioProxy.subscribe(self.getName())
        
        #configure sound detection
        self.__sslProxy.setParameter("Sensitivity",0.1)

        #callback from memory      
        try:
            self.__memoryProxy.unsubscribeToEvent("ALSoundLocalization/SoundLocated","soundtracking")
        except:
            pass
        
        self.__sslProxy.subscribe("sound_source_locator")
        self.__memoryProxy.subscribeToMicroEvent(
            "ALSoundLocalization/SoundLocated",
            self.getName(), 
            "AnotherUserDataToIdentifyEvent", 
            "sound_callback")

              
    def stop_sound_track(self, msg):
        self.__memoryProxy.unsubscribeToMicroEvent("ALSoundLocalization/SoundLocated",self.getName())
        self.__sslProxy.unsubscribe("sound_source_locator")
        rospy.logwarn("sound localisation stopped")
        #self.__audioProxy.unsubscribe(self.getName())
	
    def	reset_sensitivity(self,msg):
		if(msg.enable):
			self.__sslProxy.setParameter("Sensitivity", msg.sensitivity)
			rospy.logwarn(msg.sensitivity)
			
    def sound_callback(self, event, val, msg):
        """Deal with sound localisation event"""
        rospy.loginfo('sound location' + str(val[1][0])+str(val[1][1]))
        rospy.loginfo('confidence' + str(val[1][2]))    
        #self.__TTSproxy.say("sound detected")
        #self.__pub.publish(val[1][0],val[1][1])
        
        rospy.logwarn("Moving Head to Sound")
        
        joint_names = ["HeadYaw", "HeadPitch"]
        current = self.__proxyMotion.getAngles( joint_names, False )
        yaw = current[0] + val[1][0]
        pitch = current[1] + val[1][1]

        #Make sure we don't exceed angle limits
        if yaw < _Constants.yaw_limits[0]:
            yaw = _Constants.yaw_limits[0]
        elif yaw > _Constants.yaw_limits[1]:
            yaw = _Constants.yaw_limits[1]

        if pitch < _Constants.pitch_limits[0]:
            pitch = _Constants.pitch_limits[0]
        elif pitch > _Constants.pitch_limits[1]:
            pitch = _Constants.pitch_limits[1]

        self.__proxyMotion.setStiffnesses("Head", 1.0)
        self.__proxyMotion.angleInterpolationWithSpeed(
            joint_names,
            [ yaw, pitch ],
            _Constants.head_speed)
        time.sleep(2.0)
        #self.__proxyMotion.setStiffnesses("Head", 0.0)
#.........这里部分代码省略.........

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

#.........这里部分代码省略.........
            torsoAngle = 0

        LeftLeg  = [0,  wideAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2, -wideAngle]
        RightLeg = [0, -wideAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2,  wideAngle]

        if start_wide:
            RightArm = [0, -90,  30,  20]
        else:
            RightArm = [75, -15,  30,  20]
        
        if start_wide:
            LeftArm  += [-90, 1 / self.TO_RAD]
            RightArm += [90, 1 / self.TO_RAD]
        else:
            LeftArm  += [-60, 1 / self.TO_RAD]
            RightArm += [60, 1 / self.TO_RAD]

        # Gather the joints together
        pTargetAngles = Head + LeftArm + LeftLeg + RightLeg + RightArm

        # Convert to radians
        pTargetAngles = [x * self.TO_RAD for x in pTargetAngles]

        ###############################################################################
        # SEND THE COMMANDS
        ###############################################################################
        # We use the "Body" name to signify the collection of all joints
        pNames = "Body"

        # We set the fraction of max speed
        pMaxSpeedFraction = 0.2

        # Ask motion to do this with a blocking call
        self.__Motion.angleInterpolationWithSpeed(pNames, pTargetAngles, pMaxSpeedFraction)

        # Disable stiffness of the arms, but not the legs
        self.set_stifness(['LArm', 'RArm', 'Head'], [0, 0, 0], [0.25, 0.25, 0.25])

    def look_down(self):
        """
        Makes the Nao look completely down.
        """
        self.get_proxy("motion").setStiffnesses("Head", 1.0)
        self.get_proxy("motion").angleInterpolation("HeadPitch", 25 * almath.TO_RAD, 1.0, True)

    def look_horizontal(self):
        """
        Makes the Nao look completely horizontal.
        """
        self.get_proxy("motion").setStiffnesses("Head", 1.0)
        self.get_proxy("motion").angleInterpolation("HeadPitch", 0, 1.0, True)


    def sit_down(self):
        """
        This method lets the NAO sit down in a stable crouching position and 
        removes stiffness when done.
        """
        # Enable stiffness to move
        self.set_stifness(['Body'], [0.25], [0.25])

        ###############################################################################
        # PREPARE THE ANGLES
        ###############################################################################
        # Define The Sitting Position
        pNames  = ['LAnklePitch', 'LAnkleRoll', 'LHipPitch', 'LHipRoll', 'LHipYawPitch', 'LKneePitch', 

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import angleInterpolationWithSpeed [as 别名]
def main(robotIP, PORT = 9559):
    myBroker = ALBroker("myBroker", #needed for subscribers and to construct naoqi modules
        "0.0.0.0",
        0,
        robotIP,
        PORT)

    meanx = 0.21835
    meany = 0.035625

    global ReactToTouch, go_to_center, global_time
    global server, client
    client = OSCClient()
    client.connect(("192.168.0.5",1234))

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

    motionProxy.setStiffnesses("Body",0)
    motionProxy.setStiffnesses(jointNames,1)

    #motionProxy.openHand("LHand")
    #motionProxy.closeHand("LHand")
    maxSpeedFraction  = 0.3
    for i in range(1):
        motionProxy.angleInterpolationWithSpeed(jointNames, p[i], maxSpeedFraction)
        time.sleep(1.0)    

    minx = -999
    maxx = 999
    miny = -999
    maxy = 999
    # Copy from inverse kinematics
    effector   = "LArm"
    space      = motion.FRAME_ROBOT
    axisMask   = almath.AXIS_MASK_VEL    # just control position
    isAbsolute = False

    # Since we are in relative, the current position is zero
    currentPos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    center = currentPos

    # Define the changes relative to the current position
    dx         =  0.03      # translation axis X (meters)
    dy         =  -0.04     # translation axis Y (meters)
    dz         =  0.03      # translation axis Z (meters)
    dwx        =  0.00      # rotation axis X (radians)
    dwy        =  0.00      # rotation axis Y (radians)
    dwz        =  0.00      # rotation axis Z (radians)
    targetPos  = [dx, dy, dz, dwx, dwy, dwz]

    # Go to the target and back again
    path       = [targetPos]
    times      = [1.5] # seconds

    motionProxy.positionInterpolation(effector, space, path,
                                      axisMask, times, isAbsolute)
    dz=0.00

    currentPos = motionProxy.getPosition(effector, space, True)
    offx = 0.034
    offy = 0.02
    xy=[1345+103-(currentPos[0]+offx)*400/(0.07), (currentPos[1]+offy)*400/(0.07)+11-87-45]
    #print xy

    input('Press 1 to begin: ')
    k=2.5
    L=0.5
    n=10
    speed = 0.15
    #Initialize listener
    ReactToTouch = ReactToTouch("ReactToTouch")

    center = motionProxy.getPosition(effector, space, True)
    print "center: " + str(center[0]) + ", " + str(center[1])
    
    try:
      while 1:#for i in range(n):

        if go_to_center:
            if random.random()<0.95:
                k=max(0.9,k-0.15)
            speed = 0.4
            go_to_center = False
            #print 'going to center!'
            # Get current Position & define target position
            if random.random()<0.8:
                currentPos = motionProxy.getPosition(effector, space, True)
                maxx = min(maxx,abs(currentPos[0]-meanx))
                maxy = min(maxy,abs(currentPos[1]-meany))
                
            #targetPos = currentPos
            targetPos[0] = center[0]
            targetPos[1] = center[1]
            targetPos[2] = center[2]
            #print 'new target'
            #print targetPos
            # Move to position, being able to interrupt
#.........这里部分代码省略.........

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


robot_ip = '192.168.1.100'
robot_port = 9559
motionProxy=ALProxy("ALMotion",robot_ip,robot_port)
postureProxy = ALProxy("ALRobotPosture", robot_ip,robot_port)
motionProxy.wakeUp()
time.sleep(1)
lowerReference()
motionProxy.wbEnable(True)
supportLeg = "RLeg"
duration   = 1.0
motionProxy.wbGoToBalance(supportLeg, duration)
motionProxy.wbEnable(False)
'''
names  = ["LHipRoll","LHipPitch","LKneePitch","LAnklePitch","LAnkleRoll","RHipPitch","RHipRoll","RKneePitch","RAnklePitch","RAnkleRoll"]
angles = [0.2,0.13,-0.09,0.09,-0.23,0.1,0.13,-0.09,0.09,-0.2]
fractionMaxSpeed  = 0.1
motionProxy.angleInterpolationWithSpeed(names, angles, fractionMaxSpeed)
'''

names  = ["LHipRoll","LHipPitch","LKneePitch","LAnklePitch","LAnkleRoll","RHipPitch","RHipRoll","RKneePitch","RAnklePitch","RAnkleRoll"]
angles = [0.3,0.23,0.23,-0.23,-0.3,0.1,0.13,-0.09,0,-0.23]
fractionMaxSpeed  = 0.1
motionProxy.angleInterpolationWithSpeed(names, angles, fractionMaxSpeed)

'''
names  = ["LHipRoll","LHipPitch","LKneePitch","LAnklePitch","LAnkleRoll","RHipPitch","RHipRoll","RKneePitch","RAnklePitch","RAnkleRoll"]
angles = [0.2,0.13,-0.09,0.09,-0.23,0.1,0.13,-0.09,0.09,-0.2]
fractionMaxSpeed  = 0.1

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

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

                        while( self.managerProxy.isBehaviorRunning(behaviorName) == True):
                            time.sleep(0.2)

                    else:
                        rospy.loginfo( "Behavior is already running.")

                else:
                    rospy.loginfo( "Behavior not found.")

                self.nextBehavior = behaviorNumber +1

                self.activeBehavior =0
                return
            elif self.activeBehavior ==1:
                print( str(behaviorNumber) +'queud')




    def stop(self,behaviorName):
            # Stop the behavior.
            if (self.managerProxy.isBehaviorRunning(behaviorName.data)):
                self.managerProxy.stopBehavior(behaviorName.data)
                rospy.loginfo("Behavior stopped.")

                time.sleep(1.0)
            else:
                rospy.loginfo("Behavior is already stopped.")

    def wakeup(self):
        self.motionProxy.wakeUp()
    def rest(self):
        self.motionProxy.rest()
        self.motionProxy.stiffnessInterpolation("Body", 0, 0.5)

   #just some movement test
    def Action(self,action):
        # Define The Initial Position for the upper body
        HeadYawAngle = + 0.0
        HeadPitchAngle = + 0.0
        ShoulderPitchAngle = +80.0
        ShoulderRollAngle = +20.0
        ElbowYawAngle = -80.0
        ElbowRollAngle = -60.0
        WristYawAngle = + 0.0
        HandAngle = + 0.0
        # Define legs position
        kneeAngle = +40.0
        torsoAngle = + 0.0  # bend the torso
        spreadAngle = + 0.0  # spread the legs

        # set joint angles for standing
        if action != 'me':
            Head = [HeadYawAngle, HeadPitchAngle]

            LeftArm = [ShoulderPitchAngle, +ShoulderRollAngle, +ElbowYawAngle, +ElbowRollAngle, WristYawAngle,
                       HandAngle]
            RightArm = [ShoulderPitchAngle, -ShoulderRollAngle, -ElbowYawAngle, -ElbowRollAngle, WristYawAngle,
                        HandAngle]

            LeftLeg = [0.0,  # hipYawPitch
                       spreadAngle,  # hipRoll
                       -kneeAngle / 2 - torsoAngle,  # hipPitch
                       kneeAngle,  # kneePitch
                       -kneeAngle / 2,  # anklePitch
                       -spreadAngle]  # ankleRoll
            RightLeg = [0.0, -spreadAngle, -kneeAngle / 2 - torsoAngle, kneeAngle, -kneeAngle / 2, spreadAngle]


            # Gather the joints together
        pTargetAngles = Head + LeftArm + LeftLeg + RightLeg + RightArm

        # Convert to radians
        pTargetAngles = [x * almath.TO_RAD for x in pTargetAngles]

        # ------------------------------ send stiffness -----------------------------
        self.motionProxy.stiffnessInterpolation("Body", 1.0, 0.5)

        # ------------------------------ send the commands -----------------------------
        # We use the "Body" name to signify the collection of all joints
        pNames = "Body"
        # We set the fraction of max speed
        pMaxSpeedFraction = 0.2
        # Ask motion to do this with a blocking call
        self.motionProxy.angleInterpolationWithSpeed(pNames, pTargetAngles, pMaxSpeedFraction)

    def getBehaviors(self):
        ''' Know which behaviors are on the robot '''

        names = self.managerProxy.getInstalledBehaviors()
        for x in names:
            rospy.loginfo(x)

       # rospy.loginfo(names)

        #names = managerProxy.getRunningBehaviors()
    def on_shutdown(self):
        self.disableAuto()
        self.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import angleInterpolationWithSpeed [as 别名]
            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)


    # leave if requested
    if nao_action < 1 or nao_action > 9 or options.nao_action != 0:
        print '----- The script was stopped'
        break

exit (0)

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

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

        #  take last step?
        self.stand()
        logObj.logWrite(time.time().__str__() + "_{0}_{1}_{2}_{3}_{4}".format(action, x, y, theta, Config.SPEED))
        theta = theta*DEG2RAD # sends theta in radians

        return [time.time().__str__(), action, x, y, theta, Config.SPEED]


    # stop the walking gracefully
    def stop(self):
        #self.motionProxy.stopMove()
        self.motionProxy.setWalkTargetVelocity(0.0, 0.0, 0.0, 0.0)
        self.stand()
        #logObj.logWrite(time.time().__str__() + "_4_0_0_0_0")

    # TODO put this in the sounds class
    #def talk(self, word):
        #self.talkProxy.say(word)

    def moveHeadPitch(self, theta, speed):
        theta = float(theta)
        speed = float(speed)
        self.motionProxy.setAngles("HeadPitch", theta, 0.1)

    def chillOut(self):
        self.stiffnessOn(motionProxy=self.motionProxy)
        self.postureProxy.goToPosture("LyingBack", 1.0)
        self.stiffnessOff(motionProxy=self.motionProxy)

    # one of Roel's things
    #def measureAngle(self):
        self.stand()
        #name = "HeadPitch"
        #c = self.motionProxy.getAngles(name, True)
        #return 90.0 - (180.0/math.pi)*c[0]

    def correctHipPitch(self):
        names = ['LHipPitch', 'RHipPitch']
        angles = [0, 0]
        fractionMaxSpeed = 0.1
        self.motionProxy.setAngles(names, angles, fractionMaxSpeed)

    def simpleKickRight(self):
        # TODO
        names = ['RShoulderRoll', 'RShoulderPitch', 'LShoulderRoll', 'LShoulderPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll']
        angles = [[-0.3], [0.4], [0.5], [1.0], [0.0], [-0.4, -0.2], [0.95, 1.5], [-0.55, -1], [0.2], [0.0], [-0.4], [0.95], [-0.55], [0.2]]
        times =  [[ 0.5], [0.5], [0.5], [0.5], [0.5], [ 0.4,  0.8], [ 0.4, 0.8],  [0.4, 0.8], [0.4], [0.5], [ 0.4], [ 0.4], [ 0.4],  [0.4]]
        self.motionProxy.angleInterpolation(names, angles, times, True)

        self.motionProxy.angleInterpolationWithSpeed(['RShoulderPitch', 'RHipPitch', 'RKneePitch', 'RAnklePitch'], [1.5, -0.7, 1.05, -0.5], 1.0 , True)

        self.motionProxy.angleInterpolation(['RHipPitch', 'RKneePitch', 'RAnklePitch'], [-0.5, 1.1, -0.65], [[0.25], [0.25], [0.25]], True)
        self.normalPose(True) #TODO
        pass

    def simpleKickLeft(self):
        names = ['LShoulderRoll', 'LShoulderPitch', 'RShoulderRoll', 'RShoulderPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll']
        angles = [[0.3], [0.4], [-0.5], [1.0], [0.0], [-0.4, -0.2], [0.95, 1.5], [-0.55, -1], [-0.2], [0.0], [-0.4], [0.95], [-0.55], [-0.2]]
        times =  [[0.5], [0.5], [0.5], [0.5], [0.5], [0.4, 0.8], [ 0.4, 0.8], [ 0.4, 0.8], [0.4], [0.5], [ 0.4], [0.4], [0.4], [0.4]]        
        self.motionProxy.angleInterpolation(names, angles, times, True)
        motionProxy.angleInterpolationWithSpeed(['LShoulderPitch', 'LHipPitch', 'LKneePitch', 'LAnklePitch'], [1.5, -0.7, 1.05, -0.5], 1.0, True)
        self.motionProxy.angleInterpolation(['LHipPitch', 'LKneePitch', 'LAnklePitch'],[-0.5, 1.1, -0.65], [[0.25], [0.25], [0.25]], True)    
        self.normalPose(True) #TODO
        pass

    def simpleKick(self):
        angle = 4.0
        self.motionProxy.setAngles('RShoulderPitch', 2, 1)
        self.motionProxy.setAngles(['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'RHipPitch', 'RKneePitch', 'RAnklePitch'],
                [0.75*angle, 0.1, -0.1 -0.25*angle , -0.2 + -0.25 * angle, 0, 0.1 - 0.075*angle], 0.9)
        time.sleep(0.4 + 0.3 * angle)
        # return to start position
        self.motionProxy.angleInterpolation(['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch',
            'LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'],
            [[0], [0],[-0.4], [0.95], [-0.55],[0.15], [0], [-0.4], [0.95], [-0.55], [0.15]],
            [[0.65],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.65],[0.75]], True)
        time.sleep(0.2)
        self.motionProxy.setAngles(['LShoulderRoll', 'LShoulderPitch', 'RShoulderRoll', 'RShoulderPitch'], [0, 1.2, 0, 1.2], 0.4)
        self.motionProxy.setAngles(['LAnkleRoll', 'RAnkleRoll'], [0, 0], 0.05)

# soft kick towards right, left leg
    def sideLeftKick(self):
        self.motionProxy.angleInterpolation(['RAnkleRoll', 'LAnkleRoll'], [-0.2, -0.2], [[0.4], [0.4]], True)

        names = list()
        angles = list()
        times = list()
        
        names = ['RShoulderRoll','LHipRoll', 'LHipPitch', 'LKneePitch','LAnklePitch','LAnkleRoll','RHipRoll','RHipPitch','RKneePitch','RAnklePitch','RAnkleRoll']
        angles = [[-0.4], [0.0, 0.3], [-0.4, -0.8],[0.95, 0.2], [-0.55, 0.6], [-0.25,-0.3],[0.05 ], [-0.4], [0.95], [-0.5],       [-0.25]]
        times  = [[0.2],          [0.3, 0.75],[0.5,   1.0],[0.5,  1.0], [ 0.5,  1.0], [0.5,   1.0],[ 1.0],    [0.5],      [0.5 ],      [ 0.5],       [ 0.5]]
        
        self.motionProxy.angleInterpolation(names, angles, times, True)
        
        self.motionProxy.angleInterpolation('LHipRoll', [-0.05], [0.1], True)
        time.sleep(0.1)
        self.motionProxy.angleInterpolation(['LHipRoll','LHipPitch','LKneePitch','LAnklePitch'], 
                                     [[0.0],    [-0.55],    [1.2],       [-0.6]], 
                                     [[0.5],    [0.6],      [0.6],       [0.6]], True)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import angleInterpolationWithSpeed [as 别名]
def hitBall(hitBallSpeed=0.1, robotIP="127.0.0.1", port=9559):
    MOTION = ALProxy("ALMotion", robotIP, port)
    names, keys, times = positionHitBall()
    MOTION.angleInterpolation(names, keys, times, True)
    time.sleep(2)
    MOTION.angleInterpolationWithSpeed("LWristYaw", -30*math.pi/180.0, hitBallSpeed)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import angleInterpolationWithSpeed [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)