Python ALProxy.changeAngles方法代码示例

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

#.........这里部分代码省略.........
            self.__Motion.stiffnessInterpolation(names[i], stiffnessLists[i], timeLists[i])

    def get_range(self, name, radian=False):
        """
        This method wraps the getLimits function of naoqi; it caches the results
        because each call to ALMotion.getLimits takes a lot of time
        """
        if not name in self.__joint_range:
            limits = self.__Motion.getLimits(name)
            self.__joint_range[name] = limits[0]

        val = self.__joint_range[name]
        if not radian:
            val = (val[0] / self.TO_RAD,
                   val[1] / self.TO_RAD)

        return val

    def change_angles(self, names, angles, max_speed, disable_stiffness=False, radians=False):
        """
        This method will change 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.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"):

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

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import changeAngles [as 别名]
#Motion code snippet for continuously centering head to a visible nao mark (tests naoMarkModule getMarkAngles function)
#Mitch Deplazes
#2/2/16

from naoqi import ALProxy
import NaoMarkModule
import time

#IP of the Robot
IP = "10.0.0.6"
#Port Number of the Robot
PORT = 9559

#create a motion proxy
motionProxy = ALProxy("ALMotion", IP, PORT)

while True:
    #Get angle of the mark
    wzCamera, wyCamera = NaoMarkModule.getMarkAngles(IP, PORT)

    #Update head postion to center naomark
    motionProxy.setStiffnesses("Body", 1.0)
    motionProxy.changeAngles("HeadYaw", wzCamera, 0.1)
    motionProxy.changeAngles("HeadPitch", wyCamera, 0.1)
    time.sleep(1)

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

#.........这里部分代码省略.........
					
			if(not (errorX  > tX2 or errorX  < -tX2)): #error band for X
				KpPitch = 0
				KdPitch = 0
			if(not (errorY   > tY2 or errorY  < -tY2)):  #error band for Y
				KpRoll = 0
				KdRoll = 0

			if timeTaken - self.previousTimeTaken < 2.5: #low-pass filter
				AngleoffsetHipPitch = (KpPitch + KdPitch) * self.smoothedTimetaken
				AngleoffsetHiproll =  (KpRoll + KdRoll)  * self.smoothedTimetaken
			else:
				AngleoffsetHipPitch = 0
				AngleoffsetHiproll =  0
				#just for the plot:
				errorX = 0
				errorY = 0
				tX2 = 0
				tY2 = 0
				
			self.previousTimeTaken = timeTaken
			self.previousErrorX = errorX
			self.previousErrorX = errorY
			
			if self.plot:
				self.plotX.append(errorX)
				self.plotY.append(errorY)
				self.plotT.append(Time)
				self.plotTx.append(tX2)
				self.plotTy.append(tY2)
				
			if self.supLeg:		
				try:
					self.motproxy.changeAngles(["LHipRoll","LHipPitch"], [AngleoffsetHiproll,AngleoffsetHipPitch],0.2)
				except:
					try:
						self.motproxy.changeAngles(["LHipRoll","LHipPitch"], [0,AngleoffsetHipPitch],0.2)
					except:
						print "Hiproll out of range"
					try:
						self.motproxy.changeAngles(["LHipRoll","LHipPitch"], [AngleoffsetHiproll,0],0.2)
					except:
						print "Hiproll out of range"
			else:
				try:
					self.motproxy.changeAngles(["RHipRoll","RHipPitch"], [AngleoffsetHiproll,AngleoffsetHipPitch],0.2)
				except:
					try:
						self.motproxy.changeAngles(["RHipRoll","RHipPitch"], [0,AngleoffsetHipPitch],0.2)
					except:
						print "Hiproll out of range"
					try:
						self.motproxy.changeAngles(["RHipRoll","RHipPitch"], [AngleoffsetHiproll,0],0.2)
					except:
						print "Hiproll out of range"				
	
	def pyPlot(self):
		self.plotT = [e - self.plotT[0] for e in self.plotT]
		self.plotTxMax = self.plotTx
		self.plotTxMin = [e * -1 for e in self.plotTx]
		self.plotTyMax = self.plotTy
		self.plotTyMin = [e * -1 for e in self.plotTy]
		
		fig = plt.figure()
		ax = fig.add_subplot(111)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import changeAngles [as 别名]
                    elif key_map[button] == 'rest':
                        motion.rest()
                    elif key_map[button] == 'enable':
                        print('Remote control disabled')
                        enabled = False
                        started = False
                    elif key_map[button] == 'say':
                        tts.say('Hello! I am NAO robot. I am here to inform you that I suck.')
                    elif key_map[button] == 'none':
                        pass
                    else:
                        try:
                            if motion.robotIsWakeUp():
                                behaviour.runBehavior(key_map[button])
                        except:
                            print('Behaviour %s not installed' % key_map[button])
                            raise
                if e.type == pygame.locals.JOYAXISMOTION and motion.robotIsWakeUp() and started:
                    x_turn, y_turn = dead_zone(-joy.get_axis(1), -joy.get_axis(0), 0.05)
                    theta = math.atan2(y_turn, x_turn)
                    x, y = dead_zone(-joy.get_axis(3), -joy.get_axis(2), 0.05)
                    motion.moveToward(x, y, theta/math.pi)
                if e.type == pygame.locals.JOYHATMOTION and started:
                    yaw, pitch = e.dict['value']

                    motion.changeAngles("HeadYaw", -yaw*0.087, 0.1)
                    motion.changeAngles("HeadPitch", -pitch*0.087, 0.1)

except KeyboardInterrupt:
    exit()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import changeAngles [as 别名]
    def run(self):
        nNaoMove=ALProxy("ALMotion", self.IP, self.PORT)
        postureProxy = ALProxy("ALRobotPosture", self.IP, self.PORT)
        nNaoMove.wakeUp()
        postureProxy.goToPosture("Stand", 0.5)
        pygame.init()
        screen = pygame.display.set_mode((454, 603), 0, 32)
        background = pygame.image.load(r"F:\NaoTest\NaoSource\mythreads\naonew.png").convert_alpha()
        pygame.display.set_caption("Dragon's dance")
        screen.blit(background, (0, 0))
        pygame.display.update()
        while True:
            for event in pygame.event.get():
                if event.type == QUIT:
                    exit()
                elif event.type == KEYDOWN:
                    if event.key == K_UP:
                        x = 0.7
                        y = 0.0
                        theta = 0.0
                        frequency = 0.4
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])

                    elif event.key == K_DOWN:
                        x = -0.4
                        y = 0.0
                        theta = 0.0
                        frequency = 0.2
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])
                    elif event.key == K_LEFT:
                        x = 0.0
                        y = 0.2
                        theta = 0.6
                        frequency = 0.4
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])
                    elif event.key == K_RIGHT:
                        x = 0.0
                        y = -0.2
                        theta = -0.6
                        frequency = 0.4
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])
                    elif event.key == K_q:
                        nNaoMove.stopMove()
                        time.sleep(1)
                        nNaoMove.rest()
                    elif event.key == K_r:
                        nNaoMove.wakeUp()
                        postureProxy.goToPosture("StandInit", 0.5)
                        time.sleep(1.5)
                    elif event.key == K_s:
                        nNaoMove.stopMove()

                    elif event.key == K_z:
                        # 头部左转
                        nNaoMove.setStiffnesses("Head", 1.0)
                        names = "HeadYaw"
                        changes = 0.5
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_c:
                        # 头部右转
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadYaw"
                        changes = -0.5
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_a:
                        # 向上看
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadPitch"
                        changes = 0.2
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_d:
                        # 向下看
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadPitch"
                        changes = -0.2
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_x:
                        # 头部恢复
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadYaw"
                        angles = 0.0
                        fractionMaxSpeed = 0.2
                        nNaoMove.setAngles(names, angles, fractionMaxSpeed)
                        # 左手运动
                    elif event.key == K_1:
                        changes = -0.25
                        fractionMaxSpeed = 0.1
                        nNaoMove.setStiffnesses("LShoulderPitch", 1)
                        nNaoMove.changeAngles("LShoulderPitch", changes, 0.1)  # (关节，改变的角度，快慢）
                    elif event.key == K_2:
                        changes = 0.25
                        nNaoMove.changeAngles("LShoulderPitch", changes, 0.1)
                    elif event.key == K_3:
                        changes = 0.2
                        nNaoMove.setStiffnesses("LShoulderRoll", 1)
                        nNaoMove.changeAngles("LShoulderRoll", changes, 0.1)
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import changeAngles [as 别名]
class wsNaoMotion:
  MAX_HEADPITCH=0.32
  MIN_HEADPITCH=-0.5
  _motion=None
  _behavior=None
  _robotip=""
  def __init__(self,ip="192.168.143.101"):
    self._robotip=ip
    self._motion=ALProxy("ALMotion",self._robotip,9559)
    self._behavior=ALProxy("ALBehaviorManager",self._robotip, 9559)
    
  def getBehaviors(self):
    return self._behavior.getInstalledBehaviors()
  def getRunningBehaviors(self):
    return self._behavior.getRunningBehaviors()
  def runBehavior(self,name):
    if (self._behavior.isBehaviorInstalled(name)):
      if (not self.isBehaviorRunning(name)):
        self._behavior.post.runBehavior(name)
        time.sleep(0.5)
      else:
        print "Behavior is already running."
    else:
      print "Behavior not found."
  def stopBehavior(self):
    pass
  def isBehaviorRunning(self,name):
    return self._behavior.isBehaviorRunning(name)
  def stiffnessOn(self):
    self._motion.stiffnessInterpolation("Body", 1.0, 1.0)
  def stiffnessOff(self):
    self._motion.stiffnessInterpolation("Body", 0.0, 1.0)
  def getAngle(self,jname,sensor=True):
    return self._motion.getAngles(jname,sensor)[0]
  def getHeadPitchAngle(self):
    return self._motion.getAngles('HeadPitch',True)[0]
  def headRelativePitch(self,ang,speed=0.05):
    self._motion.changeAngles('HeadPitch',ang,speed)
  def headPitchTo(self,ang,speed=0.1):
    self._motion.setAngles("HeadPitch", ang,speed)
  def sitDown(self):
    self.runBehavior("sitdown")
  def standUp(self):
    self.runBehavior("standup")
  def hula(self,n,cw=1):
    self.poseInit()
    if cw!=1:cw=-1
# Define the changes relative to the current position
    dx         = 0.07                    # translation axis X (meter)
    dy         = 0.07*cw                    # translation axis Y (meter)
    dwx        = 0.15*cw                    # rotation axis X (rad)
    dwy        = 0.15                    # rotation axis Y (rad)

    # Define a path of two hula hoop loops
    tpath = [ [+dx, 0.0, 0.0,  0.0, -dwy, 0.0],  # point 01 : forward  / bend backward
             [0.0, -dy, 0.0, -dwx,  0.0, 0.0],  # point 02 : right    / bend left
             [-dx, 0.0, 0.0,  0.0,  dwy, 0.0],  # point 03 : backward / bend forward
             [0.0, +dy, 0.0,  dwx,  0.0, 0.0]  # point 04 : left     / bend right
             ] 
    path=[]
    for i in range(n):
      path+=tpath
    timeOneMove  = 0.4 #seconds
    times = []
    for i in range(len(path)):
        times.append( (i+1)*timeOneMove )
    # call the cartesian control API
    effector   = "Torso"
    space      =  motion.SPACE_NAO
    axisMask   = almath.AXIS_MASK_ALL
    isAbsolute = False
    self._motion.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
    self.poseInit()
  def poseInit(self):
    names = list()
    times = list()
    keys = list()
    
    names.append("HeadYaw")
    times.append([ 2.00000])
    keys.append([ [ 0.00000, [ 3, -0.66667, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append("HeadPitch")
    times.append([ 2.00000])
    keys.append([ [ 0.00000, [ 3, -0.66667, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append("LShoulderPitch")
    times.append([ 2.00000])
    keys.append([ [ 1.39626, [ 3, -0.66667, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append("LShoulderRoll")
    times.append([ 2.00000])
    keys.append([ [ 0.34907, [ 3, -0.66667, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append("LElbowYaw")
    times.append([ 2.00000])
    keys.append([ [ -1.39626, [ 3, -0.66667, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append("LElbowRoll")
    times.append([ 2.00000])
#.........这里部分代码省略.........