Python ALProxy.setStiffnesses方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
class Nao:
	def __init__(self,ip,port):
		self.tts = ALProxy("ALTextToSpeech", ip, port)
		self.motion = ALProxy("ALMotion", ip, port)
		self.posture = ALProxy("ALRobotPosture", ip, port)
		self.memory = ALProxy("ALMemory")
		self.speech = ALProxy("ALSpeechRecognition",ip,port)
		
		#Word Recognition
		self.speech.setLanguage("English") 
		wordList=["hello","goodbye","yes","no", "exit", "sit down"]
		self.speech.setWordListAsVocabulary(wordList)
		self.memory.subscribeToEvent("WordRecognized","pythonModule", "onSpeechRecognized") #  event is case sensitive !


	def sit_down(self):
		self.posture.goToPosture("Sit", 0.3)
		time.sleep(0.5)
		self.motion.setStiffnesses("Body", 0.3)
		fractionMaxSpeed=0.1
		self.motion.setAngles(["LArm"],[ 0.96, 0.03,-0.71,-1.20, 0.00, 0.30],fractionMaxSpeed)
		self.motion.setAngles(["RArm"],[ 0.96,-0.05, 0.71, 1.20, 0.00, 0.30],fractionMaxSpeed)
		self.motion.setAngles(["RLeg"],[-0.84,-0.30,-1.50, 1.02, 0.92, 0.00],fractionMaxSpeed)
		self.motion.setAngles(["LLeg"],[-0.84, 0.30,-1.50, 1.02, 0.92, 0.00],fractionMaxSpeed)
		time.sleep(0.5)
		self.motion.setStiffnesses("Body", 0.0)
		time.sleep(0.25)
		
		
	def __del__(self):
		print "memory.unsubscribeToEvent(WordRecognized,pythonModule)"
		self.memory.unsubscribeToEvent("WordRecognized","pythonModule") #  event is case sensitive !		

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
class Nao:
	def __init__(self,ip,port):
		self.tts = ALProxy("ALTextToSpeech", ip, port)
		self.motion = ALProxy("ALMotion", ip, port)
		self.posture = ALProxy("ALRobotPosture", ip, port)
		self.memory = ALProxy("ALMemory")
		self.speech = ALProxy("ALSpeechRecognition",ip,port)

		#Word Recognition
		self.speech.setLanguage("English") 
		wordList=["hello","goodbye","yes","no", "exit", "sit down"]
		pythonModule.WordDetection(wordList)


	def sit_down(self):
		self.posture.goToPosture("Sit", 0.3)
		time.sleep(0.5)
		self.motion.setStiffnesses("Body", 0.3)
		fractionMaxSpeed=0.1
		self.motion.setAngles(["LArm"],[ 0.96, 0.03,-0.71,-1.20, 0.00, 0.30],fractionMaxSpeed)
		self.motion.setAngles(["RArm"],[ 0.96,-0.05, 0.71, 1.20, 0.00, 0.30],fractionMaxSpeed)
		self.motion.setAngles(["RLeg"],[-0.84,-0.30,-1.50, 1.02, 0.92, 0.00],fractionMaxSpeed)
		self.motion.setAngles(["LLeg"],[-0.84, 0.30,-1.50, 1.02, 0.92, 0.00],fractionMaxSpeed)
		time.sleep(0.5)
		self.motion.setStiffnesses("Body", 0.0)
		time.sleep(0.25)
		
		
	def __del__(self):

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
def main():
    """ Parse command line arguments,
    run recordData and write the results
    into a csv file

    """
    if len(sys.argv) < 2:
        nao_ip = ROBOT_IP
    else:
        nao_ip = sys.argv[1]

    motion = ALProxy("ALMotion", nao_ip, 9559)
    # Set stiffness on for Head motors
    motion.setStiffnesses("Head", 1.0)
    # Will go to 1.0 then 0 radian
    # in two seconds
    motion.post.angleInterpolation(
        ["HeadYaw"],
        [1.0, 0.0],
        [1  , 2],
        False
    )
    data = recordData(nao_ip)
    # Gently set stiff off for Head motors
    motion.setStiffnesses("Head", 0.0)

    output = os.path.abspath("record.csv")

    with open(output, "w") as fp:
        for line in data:
            fp.write("; ".join(str(x) for x in line))
            fp.write("\n")

    print "Results written to", output

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
	def init(self):
		rospy.init_node('stop', anonymous=True)
		motionProxy = ALProxy("ALMotion",IP,PORT)
		postureProxy = ALProxy("ALRobotPosture", IP,PORT)
		
		motionProxy.setStiffnesses("Head", 0.0)
		motionProxy.setStiffnesses("LArm", 0.0)
		motionProxy.setStiffnesses("RArm", 0.0)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
def init(ip='127.0.0.1', port=9559):
    global myBroker
    myBroker = ALBroker('myBroker', '0.0.0.0', 0, ip, port)
    motion = ALProxy("ALMotion")
    motion.setStiffnesses('Body', .3)
    motion.setAngles('Body', motion.getAngles('Body', True), 1)
    time.sleep(1)
    motion.setStiffnesses('Body', 0)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
 def exe(self, args=None, addr=None):
     
     # get proxy
     motion = ALProxy("ALMotion", Settings.naoHostName, Settings.naoPort)
     
     # set stiffness
     if len(args) > 1:
         motion.setStiffnesses( str(args[0]), float(args[1]) )

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [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 setStiffnesses [as 别名]
def test_retraction(ip, kicking_leg, direction, lambd=5, dmax=50):
    """
    kicking_leg: LLeg or RLeg
    direction: a direction vector as python list
    """
    # create a motion proxy
    import sys
    sys.path.append("SDK")
    from naoqi import ALProxy
    mp = ALProxy("ALMotion", ip, 9559)
    import CoM
    fk = CoM.CenterOfMass(ip, 9559)
    import ik_jacobian
    import time
    # set the other leg's name
    other_leg = "RLeg" if kicking_leg == "LLeg" else "LLeg"

    mp.setStiffnesses("Body", 1)
    normalPose(mp, True)

    
    kick_pos = [i[0,0] for i in fk.get_locations_dict(other_leg)[ kicking_leg[0] +
        "AnkleRoll"]]
    kick_pos = kick_pos[:-1]
    print "kickpos", kick_pos
    other_pos = [0,0,0]
    contact_point ,point = find_point("This is irrelevant!", direction, kick_pos, other_pos)
    print contact_point
    print point
    print "point", point
    print "contact", contact_point

    angle_list = ik_jacobian.set_position(ip, kicking_leg, np.matrix(point).T,
                                          lambd=lambd, dmax=dmax)
    joints = []
    angles = []
    for joint in angle_list:
        joints = joints + [joint]
        angles = angles + [angle_list[joint]]
    mp.setAngles(joints, angles, 0.3)
    #time.sleep(3)
    joints = []
    angles = []
    contact_point = np.matrix(contact_point).T
    contact_point -=30
    angle_list = ik_jacobian.set_position(ip, kicking_leg,
                                          contact_point,
                                          lambd=lambd, dmax=dmax)
    for joint in angle_list:
        joints = joints + [joint]
        angles = angles + [angle_list[joint]]
    mp.setAngles(joints, angles, 1)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
class NaoMonkey:
	PART = {
		'LShoulder': ['LShoulderPitch', 'LShoulderRoll'],
		'RShoulder': ['RShoulderPitch', 'RShoulderRoll'],
		'LElbow': ['LElbowYaw', 'LElbowRoll'],
		'RElbow': ['RElbowYaw', 'RElbowRoll'],
		'Head': ['HeadYaw', 'HeadPitch'],
	}
	LIMITS = {
		'Head': [[-2.0, 2.0], [-0.67, 0.51]],
		'LShoulder': [[-2.0, 2.0], [-0.31, 1.32]],
		'RShoulder': [[-2.0, 2.0], [-1.32, 0.31]],
		'LElbow': [[-2.0, 2.0], [-1.54, -0.03]],
		'RElbow': [[-2.0, 2.0], [0.03, 1.54]],
	}

	def __init__(self):
		rospy.init_node('nao_mykinect', anonymous=True)

		self.listener = rospy.Subscriber('nao', NaoCoords, self.move)
                ip = rospy.get_param('~ip', '10.104.16.141')
		port = int(rospy.get_param('~port', '9559'))

		self.al = ALProxy("ALAutonomousLife", ip, port)
		self.postureProxy = ALProxy("ALRobotPosture", ip, port)
		self.motionProxy = ALProxy("ALMotion", ip, port)
		
		self.al.setState("disabled")
		for part in ["Head", "LArm", "RArm"]:
			self.motionProxy.setStiffnesses(part, 1.0)
		rospy.loginfo(self.motionProxy.getSummary())

	def move(self, coords):
		part = coords.Part.data
		angles1 = coords.Angles1
		angles2 = coords.Angles2
		angles = [float(angles1.data), float(angles2.data)]
		speed = 1.0
		if part not in NaoMonkey.PART:
			error_msg = 'Wat? I Do not have ' + str(part)
			rospy.loginfo(error_msg)
			return
		if len(NaoMonkey.PART[part]) != len(angles):
			error_msg = 'Wat? What shall i do with rest joint?'
			rospy.loginfo(error_msg)
			return
		angles = map(lambda x: float(x)*math.pi/180.0, angles)
		for limit, angle in zip(NaoMonkey.LIMITS[part], angles):
			if angle < limit[0] or angle > limit[1]:
				error_msg = 'Wat? Limits man!'
				rospy.loginfo(error_msg)
		self.motionProxy.setAngles(NaoMonkey.PART[part], angles, speed);

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
class HeadController():
    def __init__(self,ip,port):
        self.motion = ALProxy("ALMotion",ip,port)
        self.motion.setStiffnesses("Head",1.0)
        # contains 5 hard coded head yaw and pitch angles
        self.angles = [[-0.829349559873, -0.0117052046163],[-0.334127543218, 0.0594786450711],[-0.003150604247 , 0.047208706762],[0.0499896272058 ,0.437312890742],[ 0.524700755346 , 0.0525692697978]]
        self.num_objects = 5
        self.cur_object = 0

    def moveHeadToNextObject(self):
        self.motion.setAngles("Head",self.angles[self.cur_object],0.1)
        self.cur_object = (self.cur_object+1)%self.num_objects
        time.sleep(2) # give the robot some time to stabilize, that the image is not blurry

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
    def run(self, rs):
      if self.graspCB is not None and self.graspCB.check():
        print 'grasping pose'
        self.graspCB = None
        self.isRunning = True
        self.offset_X_box_hd = self.offset_X_box_hd_grasp 

      if self.stopCB is not None and self.stopCB.check():
        print 'stopping'
        self.stopCB = None
        self.isRunning = True
        self.hsCB = stopMotion(robots, qpsolver, postureTask1, None, rbdList(romeo.mbc.q))
        try:
          PORT = 9559
          robotIP = "198.18.0.1"
          motionProxy = ALProxy("ALMotion", robotIP, PORT)
        except Exception,e:
          print "Could not create proxy to ALMotion"
          print "Error was: ",e
          sys.exit(1)

        # Close the hand 
        handName  = 'RHand'
        #motionProxy.closeHand(handName)
        motionProxy.setStiffnesses(handName, 1.0)
        angle  = 0.10
        fractionMaxSpeed  = 0.2
        motionProxy.setAngles(handName, angle, fractionMaxSpeed)
        time.sleep(1.0)

        # Motion to raise the box
        chainName = "RArm"
        frame     = 0 # TORSO
        useSensor = True
        # Get the current position of the chainName in the same frame
        current = motionProxy.getPosition(chainName, frame, useSensor)

        target = [
            current[0] + 0.00,
            current[1] + 0.00,
            current[2] + 0.09,
            current[3] + 0.0,
            current[4] + 0.0,
            current[5] + 0.0]

        fractionMaxSpeed = 0.3
        axisMask         = 7 # just control position
        motionProxy.setPositions(chainName, frame, target, fractionMaxSpeed, axisMask)
        time.sleep(1.0)

        self.fsm = self.waitHS

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
class ObjectTrackerModule(ALModule):
    def __init__(self, name):
        ALModule.__init__(self, name)
        self.tts = ALProxy("ALTextToSpeech")
        self.gestureProxy = ALProxy("NAOObjectGesture", myBroker)
        self.motionProxy = ALProxy("ALMotion", myBroker)
        self.memProxy = ALProxy("ALMemory", myBroker)

        self.motionProxy.setStiffnesses("Head", 1.0)
        self.gestureProxy.startTracker(15, 0)

        self.gestureProxy.addGesture("Drink", [2,6])
        self.gestureProxy.addGesture("FrogL", [1,0,7])
        self.gestureProxy.addGesture("FrogR", [3,4,5])

    def startTracker(self, camId):
        self.gestureProxy.startTracker(15, camId)
        self.gestureProxy.focusObject(-1)

    def stopTracker(self):
        self.gestureProxy.stopTracker()
        self.gestureProxy.stopFocus()

    def load(self, path, name):
        self.gestureProxy.loadDataset(path)
        self.gestureProxy.trackObject(name, -len(self.kindNames))
        self.memProxy.subscribeToMicroEvent(name, "ObjectTracker", name, "onObjGet")

    def onObjGet(self, key, value, message):
        id = -1
        if (key in self.kindNames):
            id = self.kindNames.index(key)
        else:
            return
        if (value != None):
            if (value[0] != 0):
                if (value[5]!=None):
                    print (value[5])
            else:
                if (value[1]!=None):
                    print (value[1])

    def unload(self):
        self.gestureProxy.stopTracker()
        for i in range(0, len(self.exists)):
            self.gestureProxy.removeObjectKind(0)
            self.gestureProxy.removeEvent(self.kindNames[i])
        self.gestureProxy.removeGesture("Drink")
        self.gestureProxy.removeGesture("FrogL")
        self.gestureProxy.removeGesture("FrogR")

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
def main():
    """ Some simple robot processes.

    """
    global motion, posture, memory

    motion = ALProxy("ALMotion", NAO_IP, 9559)
    posture = ALProxy("ALRobotPosture", NAO_IP, 9559)
    memory = ALProxy("ALMemory", NAO_IP, 9559)    

    # Set stiffness on for Head motors and go to start pose.
    print "Starting tests...."
    motion.setStiffnesses("Head", 1.0)
    print "\n---------------------------------------\n"
    # Goto start position, and run test1Procedural
    print "test1Procedural starting ..."
    posture.goToPosture("Crouch", 2.0)
    t1 = (timeit("test1Procedural()", setup = "from __main__ import test1Procedural", number = TESTREPS))
    print "...end test1Procedural, time: ", t1
    print "\n---------------------------------------\n"
    # Goto start position, and run test2Post
    print "test2Post starting ..."
    posture.goToPosture("Crouch", 2.0)
    t2 = (timeit("test2Post()", setup = "from __main__ import test2Post", number = TESTREPS))
    print "...end test2Post, time: ", t2
    print "\n---------------------------------------\n"
    # Goto start position, and run test3Threading
    print "test3Threading starting ..."
    posture.goToPosture("Crouch", 2.0)
    t3 = (timeit("test3Threading()", setup = "from __main__ import test3Threading", number = TESTREPS))
    print "...end test3Threading, time: ", t3
    print "\n---------------------------------------\n"
    # Goto start position, and run test4Multiprocessing - NOT WORKING
    print "test4Multiprocessing - not working"
    #print "test4Multiprocessing starting ..."
    #posture.goToPosture("Crouch", 2.0)
    #test4Multiprocessing()
    #t4 = (timeit("test4Multiprocessing()", setup = "from __main__ import test3Threading", number = TESTREPS))
    #print "...end test4Multiprocessing, time: ", t4
    print "\n---------------------------------------\n"
    # Goto start position, and run test5Coroutine
    print "test5Coroutine starting ..."
    posture.goToPosture("Crouch", 2.0)
    t5 = (timeit("test5Coroutine()", setup = "from __main__ import test5Coroutine", number = TESTREPS))
    print "...end test5Coroutine, time: ", t5
    print "\n---------------------------------------\n"
    # Gently set stiff off for Head motors and relax.
    print "...ending tests!"
    motion.setStiffnesses("Head", 0.0)
    motion.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
def main(robot_IP, robot_PORT=9559):
	# ----------> Connect to robot <----------
	global tts, motion
	tts = ALProxy("ALTextToSpeech", robot_IP, robot_PORT)
	motion = ALProxy("ALMotion", robot_IP, robot_PORT)

	# ----------> get angle <----------
#	myGetAngles('Body', False)
#	myGetAngles('Body', True) 		# 使用传感器检测会更加准确
#	myGetAngles('Joints', False)
	
	motion.setStiffnesses("Body", 1.0)
	while True:
		print motion.getAngles('HeadPitch', True)
		time.sleep(0.5)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setStiffnesses [as 别名]
def main(robotIP, PORT = 9559):
    global yaw
    global pitch
    thread.start_new_thread(DataReceiver,())
    motion = ALProxy("ALMotion", robotIP, PORT)
    motion.setStiffnesses("Head", 1.0)
    #motion.setAngles(["HeadYaw"],1.0,1.0);
    avgYaw=MovingAverage(AVG_WINDOW_SIZE)
    avgPitch=MovingAverage(AVG_WINDOW_SIZE)
    while True:
        #print("************************* Head:"+msg)
        yaw2=avgYaw.add(yaw)
        pitch2=avgPitch.add(pitch)
#        print(str(pitch2) + " .. " + str(yaw2))
        motion.setAngles(["HeadYaw", "HeadPitch"], [yaw2,pitch2], 0.5)
        time.sleep(0.01)