Python ALProxy.moveTo方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveTo [as 别名]
def main(robotIP, PORT=9559):

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

    # Wake up robot
    motionProxy.wakeUp()

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

    x     = 0.3
    y     = 0.0
    theta = 0.0

    # This example show customization for the both foot
    # with all the possible gait parameters
    try:
        motionProxy.moveTo(x, y, theta,
            [ ["MaxStepX", 0.02],         # step of 2 cm in front
              ["MaxStepY", 0.16],         # default value
              ["MaxStepTheta", 0.4],      # default value
              ["MaxStepFrequency", 0.0],  # low frequency
              ["StepHeight", 0.01],       # step height of 1 cm
              ["TorsoWx", 0.0],           # default value
              ["TorsoWy", 0.1] ])         # torso bend 0.1 rad in front
    except Exception, errorMsg:
        print str(errorMsg)
        print "This example is not allowed on this robot."
        exit()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveTo [as 别名]
def main(robotIP, PORT=9559):
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

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

    # Example showing how to get a simplified robot position in world.
    useSensorValues = False
    result = motionProxy.getRobotPosition(useSensorValues)
    print "Robot Position", result

    # Example showing how to use this information to know the robot's diplacement.
    useSensorValues = False
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))

    # Make the robot move
    motionProxy.moveTo(0.1, 0.0, 0.2)

    endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))

    # Compute robot's' displacement
    robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
    print "Robot Move:", robotMove

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveTo [as 别名]
def main(robotIP, PORT = 9559, steps=1):
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    motionProxy.setMoveArmsEnabled(True, True)
    motionProxy.setExternalCollisionProtectionEnabled("All", False)
    motionProxy.moveTo(steps / 4.0, 0, 0) # X, Y, Theta

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveTo [as 别名]
class NaoWalks(ALModule, Thread):
    """docstring for NaoWalks"""
    def __init__(self, name):
        Thread.__init__(self)
        ALModule.__init__(self, name)

        # Create new object display a colored message on computer's terminal.
        self.logs = logs()

        # Create an ALMotion proxy.
        self.motion = ALProxy("ALMotion")
        self.logs.display("Subscribed to an ALMotion proxy", "Good")


    # Method called by the Thread.start() method.
    def run(self):
        global direction
        global alpha
        global directions

        self.motion.post.angleInterpolation(["HeadYaw"],
                                             alpha,
                                             1,
                                             False)

        direction = self.motion.getAngles("HeadYaw", True)

        try:
            if (direction.pop() != directions.pop()):
                directions.append(direction.pop())

                self.stop()

                self.logs.display("Nao is walking in direction (radian):",
                                  "Default",
                                  str(direction.pop()))
                self.motion.moveTo(0.2, 0, direction.pop())

        except IndexError:
            pass

        global AnalyseANewPict
        AnalyseANewPict = True


    # Method called to stop Nao.
    def stop(self):
        self.motion.killMove()


    # Method called to properly delete the thread.
    def delete(self):
        self.motion.killMove()

# ############################### END OF CLASS ############################## #

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveTo [as 别名]
    def __walk_to(self, data):
        #self.__posture("Stand")
        motion = ALProxy("ALMotion", Settings.naoHostName, Settings.naoPort)
        motion.setMoveArmsEnabled( data['arms'], data['arms'] )
        
        self.__motionActive = True
        start_new_thread( self.__motion_background_task, () )

        collisionDetectionEnabled = motion.getExternalCollisionProtectionEnabled("Move")
        motion.setExternalCollisionProtectionEnabled("Move", False)
        motion.moveTo( data['x'], data['y'], radians(data['theta']) )
        motion.setExternalCollisionProtectionEnabled("Move", collisionDetectionEnabled) 
        
        self.__motionActive = False

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveTo [as 别名]
def robotRotate(angle, robotIP, port):
    MOTION = ALProxy("ALMotion", robotIP, port)
    if abs(angle) > math.pi/2:
        if angle >= 0:
            thetaz1 = angle - math.pi/2
            MOTION.setMoveArmsEnabled(False, False)
            MOTION.moveTo(0.0,0.0,math.pi/2,moveConfig)
            time.sleep(1)
            MOTION.setMoveArmsEnabled(False, False)
            MOTION.moveTo(0.0,0.0,thetaz1,moveConfig)
        else:
            thetaz1 = angle + math.pi/2
            MOTION.setMoveArmsEnabled(False, False)
            MOTION.moveTo(0.0,0.0,-math.pi/2,moveConfig)
            time.sleep(1)
            MOTION.setMoveArmsEnabled(False, False)
            MOTION.moveTo(0.0,0.0,thetaz1,moveConfig)
    else:
        MOTION.setMoveArmsEnabled(False, False)
        MOTION.moveTo(0.0,0.0,angle,moveConfig)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveTo [as 别名]
def main(robotIP, x, y, theta, PORT=9559):
    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    # Maybe not needed
    motionProxy.wakeUp()

    # Send robot to Pose Init
    # Maybe not needed
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing the moveTo command
    # The units for this command are meters and radians
    current_head_angle = motionProxy.getAngles("HeadPitch", True)
    print "Command angles <True>:"
    print str(current_head_angle)
    print ""

    motionProxy.moveTo(x, y, theta - current_head_angle[0])

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

	def __init__(self):
		self.init()

	
		
		
	def init(self):
		rospy.init_node('bouger', anonymous=True)
		self.motionProxy = ALProxy("ALMotion",IP,PORT)
		#self.postureProxy = ALProxy("ALRobotPosture", IP,PORT)
		rospy.Timer(rospy.Duration(1), self.timer_callback)
		rospy.spin()
		
	def timer_callback(self,data):
		
			Head = self.motionProxy.getPosition('Head',0, True)
			self.motionProxy.moveTo(x, y, theta)
			#PositionRight = self.motionProxy.getPosition('RForeArm',0, True)
			print Head

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveTo [as 别名]
maxStepXRight = ["MaxStepX", 0.07]
maxStepFrequencyRight = ["MaxStepFrequency", 1]
stepHeightRight = ["StepHeight", 0.015]
torsoWxRight = ["TorsoWx", 0.0] 
torsoWyRight = ["TorsoWy", 0.0]

#LEFT FOOT
maxStepXLeft = ["MaxStepX", 0.7]
maxStepFrequencyLeft = ["MaxStepFrequency", 1]
stepHeightLeft = ["StepHeight", 0.015]
torsoWxLeft = ["TorsoWx", 0.0] 
torsoWyLeft = ["TorsoWy", 0.0]    

motionProxy.setWalkTargetVelocity(0.0, 0.0, 0.0, 0.0)

motionProxy.moveTo(.70, 0.0, -.789)

motionProxy.moveTo(1.0, 0.0, 0.0)
time.sleep(1)

motionProxy.setWalkTargetVelocity(0.0, 0.0, 0.0, 0.0)

while True:
    # Get head position sensor value
    key = "Device/SubDeviceList/HeadYaw/Position/Sensor/Value"
    value = memoryProxy.getData(key)
    print value
    # Check if move to left
    if value>=0.01:
        print "heading left"
        #motionProxy.stopMove()

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

if __name__ == '__main__':

    motion = ALProxy("ALMotion", "nao.local", 9559)
    tts = ALProxy("ALTextToSpeech", "nao.local", 9559)

    motion.setStiffnesses("Body", 1.0)
    
    motion.moveInit()
    motion.moveTo(0.5, 0, 0)

    id = motion.post.moveTo(0.5, 0, 0)
    motion.wait(id, 0)
    tts.say("I am walking")

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

#.........这里部分代码省略.........
    # to "Sitting".This is a common variable for all move methods  which is used to help recognize every time the position of the robot 
    # Also a check is done depending if the command comes from voice or from keyboard.
    # if the command has been given by voice inserts a relevant text to the list on the right otherwise sets a text saying that the command comes from the keyboard        
    def relax(self):
        if(self.robot_position.get()) == "Sitting":
            pass
        else:
            self.posture.goToPosture("SitRelax", 0.8)
            if (self.command_origin.get()) == 0:
                self.log_list.insert('end' ,("Relax (Keyboard Command)") )  
            else:
                 pass             
            self.tts.say("I am chilling now")      
            self.robot_position.set("Sitting")     
    
    
    
            
    # Checks initially the position of the robot and then acts according to some conditions. 
    # if robot is sitting down a random voice message from a predefined list of expressions (declared at the top) informs the 
    # the user that the robot has to stand up first if needs to move in some direction.If robot though is on an upright position
    # moves forward to the x axis.Also for the forward movement the arms of the robot are enabled and moved accordingly. 
    # Also a check is done depending if the command comes from voice or from keyboard.
    # if the command has been given by voice inserts a relevant text to the list on the right otherwise sets a text saying that the command comes from the keyboard 
    def move_forward(self):
        
        #sets the arms of the robot true to enable them during  forward movement
        self.leftArmEnable  = True
        self.rightArmEnable  = True
        
        if(self.robot_position.get()) == "Sitting":
             self.tts.say(random.choice(self.expressions))
        else:
             self.motion.moveTo(0.2, 0, 0)
             self.motion.setWalkArmsEnabled(self.leftArmEnable, self.rightArmEnable)
             if (self.command_origin.get()) == 0:
                self.log_list.insert('end' ,("Move forward (Keyboard Command)") )
             else:
                 pass
             #print ('LeftArmEnabled: ',  self.arms_status[0])
             #print ('LeftArmEnabled: ',  self.arms_status[1])

    
    
    
    # Checks  the position of the robot and  acts accordingly. 
    # if robot is sitting down a random voice message from a predefined list of expressions (declared at the top) informs the 
    # the user that the robot has to stand up first if needs to move backwards.If robot though is on an upright position
    # moves back according the x axis.Also for the forward movement the arms of the robot are enables and moved accordingly. 
    # if the command has been given by voice inserts a relevant text to the list on the right otherwise sets a text saying that the command comes from the keyboard 
    def move_back(self):
        if(self.robot_position.get()) == "Sitting":
            self.tts.say(random.choice(self.expressions))
        else:
            self.motion.moveTo(-0.1 , 0, 0)
            if (self.command_origin.get()) == 0:
                self.log_list.insert('end' ,("Move back (Keyboard  Command)") ) 
            else:
                pass
            

    # Checks the position of the robot and if the robot is sitting down a random voice message from a predefined list of expressions 
    # (declared at the top) informs the the user that the robot has to stand up first if needs to move in any direction.If robot
    # though is on an upright position moves right regarding the y axis.
    # Also head function is called which allows the robot to move its  head and look at the direction is going to move . 
    # if the command has been given by voice inserts a relevant text to the list on the right otherwise sets a text saying that the command comes from the keyboard             

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

#.........这里部分代码省略.........
            self.vocabulary = ["yes", "no", "please", "hello","the","be","to","of","and","in","that","have","it","robot"]

            self.asr.setVocabulary(self.vocabulary, True)
            self.asr.setVisualExpression(True)
            self.asr.setAudioExpression(True)

            self.asr.subscribe("ASR")


        self.asr.pause(False)
        nodetectionCount = 0
       # print  msg.azimuth.data
        while True:
            time.sleep(0.8)
            speech = self.memoryProxy.getData(self.memValue, 0)
            voice = self.memoryProxy.getData('SpeechDetected', 0)
            if (voice ==1 ):
                if speech[1] > 0.1:
                    nodetectionCount =0
                    self.speechPub.publish(True)
                    return
            else:
                nodetectionCount +=1
            if nodetectionCount >10:
                self.asr.pause(True)
                return




    def navigate(self,x):
        self.checkawake()

        self.motionProxy.moveTo(x,0,0)
    def rotate(self,z):
        self.checkawake()
        self.motionProxy.moveTo(0, 0, z)



    def run_callback(self, data):

        # add to queue
        behavior = data.data
        if behavior == 'wakeup':
            self.l_behaviors.append(behavior)
            self.l_type.append("wakeup")

        elif behavior == 'rest':
            self.l_behaviors.append(behavior)
            self.l_type.append('rest')

        elif behavior == "help":
            self.getBehaviors()
        elif behavior.startswith('say ') == True:
            # cut after say and aspace
            self.l_behaviors.append(behavior[4:])
            self.l_type.append('say')
        elif behavior.startswith('wait'):
            self.l_behaviors.append(behavior[5:])
            self.l_type.append('wait')
        elif behavior.startswith('sayanimated'):
            self.l_behaviors.append(behavior[11:])
            self.l_type.append('sayanimated')
        elif behavior.startswith('move '):
            self.l_behaviors.append(behavior[5:])

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveTo [as 别名]
     #####################
     ## Enable arms control by Walk algorithm
     #####################
     motionProxy.setWalkArmsEnabled(True, True)
     #~ motionProxy.setWalkArmsEnabled(False, False)
 
     #####################
     ## FOOT CONTACT PROTECTION
     #####################
     motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
     
     start = time.time()
    
     
     if forward :
          navigationProxy.moveTo(1.0, 0.0, 0.0)
          motionProxy.waitUntilMoveIsFinished()
     elif backward :
          navigationProxy.moveTo(-1.0, 0.0, 0.0)
          motionProxy.waitUntilMoveIsFinished()
     elif right :
          navigationProxy.moveTo(0.0, 0.0, -1.0)
          motionProxy.waitUntilMoveIsFinished()
     elif left :
          navigationProxy.moveTo(0.0, 0.0, 1.0)
          motionProxy.waitUntilMoveIsFinished()
             
             
     
 
     'Small message of end'

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

#.........这里部分代码省略.........
		dx = math.sin(alfa)*lenght_to_ground
		#print(dx)

		# przesuniecie y medzy reka a punktem na podlodze
		dy = math.sin(beta)*lenght_to_ground
		#print(dy)

		#punkt na podlodze w stosunku do kinecta
		pkt = Vector3()
		pkt.z = 0
		pkt.x = B.x-dx
		pkt.y = B.y +dy
		print (str(pkt.x) +"\t\t" +str(pkt.y))
		#self.nao_go(pkt.x,pkt.y)
		#key_input = self.getch()
		if (key_input == 'g'):
			self.nao_go(pkt.x,pkt.y)
		if (key_input == 'q'):
			break
		self.r.sleep()

    def nao_go(self,x,y):
	behaviors = self.behavior.getInstalledBehaviors()
	self.behavior.stopAllBehaviors()
	#self.postureProxy.goToPosture("StandInit",1.0)
	position = almath.Pose2D(self.motionProxy.getRobotPosition(False))
	#print("Znajduje sie w: " + str(self.naoXPosition) + " " + str(self.naoYPosition) )
	#print("A Nao :"+ str(position-self.mistake))
	print("Ide do:" + str(x) + " " + str(y))
	print("Czyli zmieniam pozycje o: " + str(x - (position-self.mistake).x) + " " + str(y - (position-self.mistake).y))
	#self.motionProxy.moveTo((x - self.naoXPosition),(y - self.naoYPosition),0.0)
	#self.naoXPosition = x
	#self.naoYPosition = y
	self.motionProxy.moveTo((x - (position-self.mistake).x),(y - (position-self.mistake).y),0.0)
	self.behavior.runBehavior('nao-14eaf5/BogdAL')
	#for string in behaviors:
		#print(string)
		#self.behavior.runBehavior(string)
	#print("Znajduje sie w: " + str(self.naoXPosition) + " " + str(self.naoYPosition) )

    def tome_process(self):
	#plik = open('./plik.txt','w')
	#plik.write('#RekaPrawa (x,y,z)\t LokiecPrawy(x,y,z)\t RekaLewa(x,y,z)\t LokiecLewy(x,y,z)\n')
	#count_test = 500
	count = 0
	total = 0
	oldrx = 0
	oldlx = 0
	oldelz = 0
	olderz = 0
	#while count_test > 0:
	while not rospy.is_shutdown():
		trans_rh, rot = self.tf.lookupTransform('/openni_depth_frame', self.j_r_hand, rospy.Duration())
		trans_lh, rot = self.tf.lookupTransform('/openni_depth_frame', self.j_l_hand, rospy.Duration())
		trans_erh, rot = self.tf.lookupTransform('/openni_depth_frame', self.j_r_ehand, rospy.Duration())
		trans_elh, rot = self.tf.lookupTransform('/openni_depth_frame', self.j_l_ehand, rospy.Duration())
		trans_torso, rot = self.tf.lookupTransform('/openni_depth_frame', self.j_torso, rospy.Duration())
		vec_torso = Vector3(*trans_torso)
		vec_rh = Vector3(*trans_rh)
		vec_lh = Vector3(*trans_lh)
		vec_reh = Vector3(*trans_erh)
		vec_leh = Vector3(*trans_elh)
		#rece pod katem 90 stopni do powierzchni
		if(  (-0.05 < (vec_rh.z - vec_reh.z) < 0.05) and (-0.05 < (vec_lh.z - vec_leh.z) < 0.05) and count == 0 ):
			print("start")
			count = 1

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

    def __init__(self):
	rospy.init_node('control_by_keys', anonymous=True)
	self.myFrame = 'control'
	self.r = rospy.Rate(1)		#rospy.Rate(1) = 1Hz

#Nao init and connect
	self.nao_init()
	rospy.loginfo("\n\nYou can control nao by keys!\n\n")
	
    
    def toRAD(self,angle):
	return angle*almath.TO_RAD

    def nao_init(self):
	#NAO CONNECT AND INIT
	#PORT AND IP NAO ROBOT
	#ip = rospy.get_param('~ip', '127.0.0.1')
	#port = int(rospy.get_param('~port', '45404'))
	ip = rospy.get_param('~ip', '10.104.16.53')
	port = int(rospy.get_param('~port', '9559'))

	self.al = ALProxy("ALAutonomousLife", ip, port)
	self.postureProxy = ALProxy("ALRobotPosture", ip, port)
	self.tts = ALProxy("ALTextToSpeech", ip, port)
	self.motionProxy = ALProxy("ALMotion", ip, port)
	self.al.setState("disabled")
	self.postureProxy.goToPosture("StandInit", 0.5)	

    def getch(self):   # define non-Windows version
        import sys, tty, termios
        fd = sys.stdin.fileno()
        old_settings = termios.tcgetattr(fd)
        try:
            tty.setraw(sys.stdin.fileno())
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch


    def control(self):
	rospy.loginfo("\nPress key W,A,S,D to control nao. Press 'c' to close application!\n\n")
	key_input = 'o'
	tmp_key = 'o'

	while(key_input != 'c'):
		while(True):
			tmp_key = self.getch()
			if (tmp_key == 'a' or tmp_key == 's' or tmp_key == 'd' or tmp_key == 'w' or tmp_key == 'c'):
				key_input = tmp_key
				break
		if(key_input == 'w'):
			self.motionProxy.moveTo(0.1,0.0,0.0)
		if(key_input == 'a'):
			self.motionProxy.moveTo(0.0,0.0,self.toRAD(-90))
		if(key_input == 's'):
			self.motionProxy.moveTo(-0.1,0.0,0.0)
		if(key_input == 'd'):
			self.motionProxy.moveTo(0.0,0.0,self.toRAD(90))
		self.r.sleep()
	rospy.loginfo("\n\nExit Application!\n\n")