Python ALProxy.getPosition方法代码示例

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

    """
    contains all services that can be called by the client
    """

    def __init__(self, ip):
        self.motionProxy = ALProxy("ALMotion", ip, 9559)
        self.broadcaster = tf.TransformBroadcaster()
        rospy.Subscriber("/joint_states", JointState, self.callback)

    def callback(self, data):

        position = self.motionProxy.getPosition("Torso", 1, True)

        # trans = [data.transforms[0].transform.translation.x,
        #          data.transforms[0].transform.translation.y,
        #          data.transforms[0].transform.translation.z]
        # rot = [data.transforms[0].transform.rotation.x,
        #        data.transforms[0].transform.rotation.y,
        #        data.transforms[0].transform.rotation.z,
        #        data.transforms[0].transform.rotation.w
        #        ]

        # euler = euler_from_quaternion(rot)
        trans = (position[0], position[1], position[2])
        rot = quaternion_from_euler(position[3], position[4], position[5])

        # rot_bis = quaternion_conjugate(rot)
        self.broadcaster.sendTransform(trans, rot, rospy.Time.now(),
                                       "/odom_bis",
                                       "/base_link")

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getPosition [as 别名]
def send():
	motion = ALProxy("ALMotion","127.0.0.1",9559)

	rospy.init_node('observations')

	headpub = rospy.Publisher('headLoc', Head)
	armpub = rospy.Publisher('RArmLoc',Arm)
	rarm_angle_pub = rospy.Publisher('RArmAnglesLoc',ArmAngles)
	while not rospy.is_shutdown():
		x = int((motion.getAngle("HeadYaw")+.7)*256/1.4)
		y = int((motion.getAngle("HeadPitch")+.7)*256/1.4)
		head = Head()
		head.x = x
		head.y = y
		headpub.publish(head)
		stiff = motion.getChainStiffnesses("RArm")
		rarm = motion.getChainAngles("RArm")
		armAngles = ArmAngles()
		armAngles.duration = 0.0
		for i in range(len(rarm)):
			armAngles.armAngles[i] = rarm[i]
		if (stiff[0] < .1):
			motion.gotoChainAngles("RArm",rarm,0.2,motion.INTERPOLATION_SMOOTH)
		armPos = motion.getPosition("RArm",0)
		arm = Arm()
		for i in range(len(armPos)):
			arm.armPose[i] = armPos[i]
		arm.hand = motion.getAngle("RHand")
		armpub.publish(arm)
		rarm_angle_pub.publish(armAngles)
		rospy.sleep(.1)
	pass

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

    # Example showing how to get the position of the top camera
    name            = "CameraTop"
    frame           = motion.FRAME_WORLD
    useSensorValues = True
    result          = motionProxy.getPosition(name, frame, useSensorValues)
    print "Position of", name, " in World is:"
    print result

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getPosition [as 别名]
def etat():


	rospy.init_node('etat_node', anonymous=True)
	
	motionProxy = ALProxy("ALMotion","127.0.0.1",9559)

	while 1:
		initialTransform = motionProxy.getPosition('LArm',0, True)

		print(initialTransform)
		print("=================================")

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getPosition [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 getPosition [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)
			#PositionRight = self.motionProxy.getPosition('RForeArm',0, True)
			print Head

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getPosition [as 别名]
RHipRoll = -5.729578
RHipPitch = 7.544204
RKneePitch = -5.156620
RAnklePitch = 2.413832
RAnkleRoll = 8.275593
LHipRoll = 5.729578
LHipPitch = 7.543314
LKneePitch = -5.156620
LAnklePitch = 2.413832
LAnkleRoll = -8.275593
setUpAngles(RShoulderPitch,RShoulderRoll,RElbowYaw,RElbowRoll,LShoulderPitch,LShoulderRoll,LElbowYaw,LElbowRoll,RHipRoll,RHipPitch,RKneePitch,RAnklePitch,RAnkleRoll,LHipRoll,LHipPitch,LKneePitch,LAnklePitch,LAnkleRoll)


frame = motion.FRAME_TORSO
useSensorValues = True
RShoulder = motionProxy.getPosition("RShoulderPitch",frame, useSensorValues)
RElbow = motionProxy.getPosition("RElbowRoll",frame, useSensorValues)
RHand = motionProxy.getPosition("RWristYaw",frame, useSensorValues)

LShoulder = motionProxy.getPosition("LShoulderPitch",frame, useSensorValues)
LElbow = motionProxy.getPosition("LElbowRoll",frame, useSensorValues)
LHand = motionProxy.getPosition("LWristYaw",frame, useSensorValues)

print "RSE = ["+str(RElbow[0]-RShoulder[0])+","+str(RElbow[1]-RShoulder[1])+","+str(RElbow[2]-RShoulder[2])+"]';"
print "REH = ["+str(RHand[0]-RElbow[0])+","+str(RHand[1]-RElbow[1])+","+str(RHand[2]-RElbow[2])+"]';"
print "LSE = ["+str(LElbow[0]-LShoulder[0])+","+str(LElbow[1]-LShoulder[1])+","+str(LElbow[2]-LShoulder[2])+"]';"
print "LEH = ["+str(LHand[0]-LElbow[0])+","+str(LHand[1]-LElbow[1])+","+str(LHand[2]-LElbow[2])+"]';"

name = "Body"
frame = motion.FRAME_WORLD
useSensors = True

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getPosition [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 getPosition [as 别名]
    from naoqi import ALProxy
     motions as m
    import time
    mot = ALProxy("ALMotion", "127.0.0.1", 9559)
    # set in normalPose
    m.normalPose()
    (xTorso, yTorso, zTorso) = m.getPosition("Torso", 1, True)

    # TODO make positions relative to the nao
    # do all ranges
    for x in range(-size_range, size_range, stepwidth):
        for y in range(-size_range, size_range, stepwidth):
            for z in range(-size_range, size_range, stepwidth):
                mot.setPosition("RLeg", SPACE_WORLD, (x, y, z), 10, 7)
                time.sleep(3)
                (v, w, u, _, _, _) = mot.getPosition("RLeg", 1, True)
                # in case the same position occurs, write it to file
                x = round(3, x)
                y = round(3, y)
                z = round(3, z)
                v = round(3, v)
                w = round(3, w)
                u = round(3, u)
                if(x == v && y == w && z == u):
                    f = open('possible_positions.txt', 'w')
                    f.write("( " + str(x) + " " + str(y) + " " str(z) + " )" )
                    # TODO also write the angles
                    f.closed
                    # if it is also a balanced position write it to another file
                    if(mot.getCOM("Body", SPACE_WORLD, false) ):
                        f = open('possible_positions_Com.txt', 'w')

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getPosition [as 别名]
class maThread(threading.Thread):
    def __init__(self, *args):
        """Initialisation de la thread, partie qui ne se joue qu'une seule fois."""
        threading.Thread.__init__(self)
        self.running = True
        self.lost = False
        self.countLost = 0
        global memory
        memory = ALProxy("ALMemory")
        self.motionProxy = ALProxy("ALMotion", NAO_IP, NAO_PORT)
        self.postureProxy= ALProxy("ALRobotPosture", NAO_IP, NAO_PORT)
        self.redBallTracker = ALProxy("ALRedBallTracker", NAO_IP, NAO_PORT)


        

    def run(self):
        """Partie de programme qui se répète."""
        global memory
        self.postureProxy.applyPosture("Sit",0.5)
        self.motionProxy.setStiffnesses("Head", 1.0)
        self.motionProxy.setAngles("HeadYaw", 0, 0.3)
        self.motionProxy.setAngles("HeadPitch", 0, 0.3)
        self.redBallTracker.startTracker()
        while self.running == True:
            #print "Bam : "+str(self.lost)
            if self.lost == False:

                #data = memory.getData("redBallDetected",1)
                
                if self.redBallTracker.isNewData():
                    position = self.redBallTracker.getPosition()
                    self.lost = False
                    #print "Position : "
                    #print "   x = "+str(position[0])+" y = "+str(position[1])+" z = "+str(position[2])
                    self.countLost = 0
                else:
                    # S'il n'y a pas de nouvelles données, la balle est perdue de vu et on attends 10 itérations histoire d'être sur
                    self.countLost=self.countLost+1
                    #print "CountLost = "+str(self.countLost)
                    time.sleep(1.0)

                    if (self.countLost > 5):
                        print "Balle perdue"
                        self.lost = True
                        self.countLost = 0


            if self.lost == True:
            		# Dans le cas ou la balle est perdue de vue, on fait des mouvement de la tête pour la retrouver
                    #TODO : faire un mouvement de la tête et des bras cool
                    

                    self.motionProxy.setAngles("HeadYaw", 0, 0.3)
                    self.motionProxy.setAngles("HeadPitch", 0, 0.3)
                    time.sleep(1.0)



                    self.motionProxy.setAngles("HeadPitch",0.5, 0.3)
                    time.sleep(0.5)
                    self.motionProxy.setAngles("HeadYaw", 1, 0.5)
                    time.sleep(1.2)
                    if self.redBallTracker.isNewData():
                        self.lost = False
                    else:
                        self.motionProxy.setAngles("HeadYaw", -0.70, 0.5)
                        time.sleep(1.0)
                    
                        if self.redBallTracker.isNewData():
                            self.lost = False
                        else:

                            self.motionProxy.setAngles("HeadPitch",-0.5, 0.3)
                            time.sleep(0.5)
                            self.motionProxy.setAngles("HeadYaw", 0.70, 0.5)
                            time.sleep(1.0)

                            if self.redBallTracker.isNewData():
                                self.lost = False
                            else:
                                self.motionProxy.setAngles("HeadYaw", 0, 0.3)
                                self.motionProxy.post.setAngles("HeadPitch", 0, 0.3)
                                time.sleep(1.0)


    def stop(self):
        """Permet un arrêt propre de la thread."""
        
        self.redBallTracker.stopTracker()
        self.redBallTracker = None
        self.running = False

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

#.........这里部分代码省略.........
        try:
            # noinspection PyBroadException
            try:
                self._motion_proxy.ping()
                self._posture_proxy.ping()
            except:
                self._motion_proxy = ALProxy("ALMotion", pip, pport)
                self._posture_proxy = ALProxy("ALRobotPosture", pip, pport)

                self._motion_proxy.wakeUp()
        except:
            exc_type, exc_value, exc_traceback = sys.exc_info()
            traceback.print_exception(exc_type, exc_value, exc_traceback)
            sys.exit(1)

    def _init_effector(self):
        """Initialize the position and orientation of all effectors."""
        for x in self._effectorNames:
            self._effector[x] = 0.0

    def _init_joint(self):
        """Initialize the position and orientation of all joints."""
        for x in self._jointNames:
            self._joint[x] = 0.0

    def _init_camera(self):
        """Initialize the position and orientation of all cameras."""
        for x in self._cameraNames:
            self._camera[x] = 0.0

    def _update_effector(self):
        """Update the position and orientation of all effectors."""
        for x in self._effectorNames:
            self._effector[x] = self._motion_proxy.getPosition(x, self._default_frame, self._use_sensor_values)

    def _update_joint(self):
        """Update the position and orientation of all joints."""
        for x in self._jointNames:
            self._joint[x] = self._motion_proxy.getPosition(x, self._default_frame, self._use_sensor_values)

    def _update_camera(self):
        """Update the position and orientation of all cameras."""
        for x in self._cameraNames:
            self._camera[x] = self._motion_proxy.getPosition(x, self._default_frame, self._use_sensor_values)

    def _calculate_target_tf(self, effector, delta_tf, frame):
        """Calculate the target transform.

        Calculate the target transform from the current position of the effector
        and the relative position change.

        Parameters
        ----------
        effector : str
            String of effector name.
        delta_tf : ndarray[float]
            The relative position change.
        frame : {FRAME_TORSO, FRAME_WORLD, FRAME_ROBOT}
            The task frame .

        """
        init_tf = almath.Transform(self._motion_proxy.getTransform(effector, frame, self._use_sensor_values))

        delta_tf = listify(delta_tf)
        d = delta_tf.pop(0)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getPosition [as 别名]
    pub_traj = rospy.Subscriber(TRAJ_TOPIC, MultiPaths, on_traj)

    if NAO_WHOLEBODY:
        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.2)

        motionProxy.wbEnableEffectorControl(effector,True);

        space = motion.FRAME_ROBOT  #FRAME_ROBOT
    else:
        #just stiff on motors if NAO is not wholebody
        StiffnessSwitch(motionProxy, on=1)

        space = motion.FRAME_TORSO     #FRAME_TORSO


    tl = tf.TransformListener()
    rospy.sleep(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

    handPos = motionProxy.getPosition("RHand",space,False)
    print handPos

    rospy.spin()

    #release the stiffness
    StiffnessSwitch(motionProxy, on=0)
    myBroker.shutdown()

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

    # Usage
    # Nao().searchBall()
    # Nao().hasBall()
    # Nao().walkToBall()
    # Nao().protectionOff()  should call before moves
    
    def __init__(self):
        self.myBroker = ALBroker("myBroker","0.0.0.0",0,"10.51.5.167",9559)
        self.motion   = ALProxy("ALMotion")
        self.tracker  = ALProxy("ALRedBallTracker")
        self.vision   = ALProxy("ALVideoDevice")
        self.tts      = ALProxy("ALTextToSpeech")
        self.currentCamera = 0
        self.setTopCamera()
        self.tracker.setWholeBodyOn(False)
        self.tracker.startTracker()
        self.ballPosition = []
        self.targetPosition = []


    def __del__(self):
        self.tracker.stopTracker()
        pass


    # If Nao has ball returns True
    def hasBall(self):
        self.checkForBall()
        time.sleep(0.5)
        if self.checkForBall():
            return True
        else:
            return False


    # Nao scans around for the redball
    def searchBall(self):
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        for turnAngle in [0,math.pi/1.8,math.pi/1.8,math.pi/1.8]:
            if turnAngle > 0:
                self.motion.walkTo(0,0,turnAngle)
            if self.hasBall():
                self.turnToBall()
                return
            for headPitchAngle in [((math.pi*29)/180),((math.pi*12)/180)]:
                self.motion.angleInterpolation("HeadPitch", headPitchAngle,0.3,True)
                for headYawAngle in [-((math.pi*40)/180),-((math.pi*20)/180),0,((math.pi*20)/180),((math.pi*40)/180)]:
                    self.motion.angleInterpolation("HeadYaw",headYawAngle,0.3,True)
                    time.sleep(0.3)
                    if self.hasBall():
                        self.turnToBall()
                        return


    # Nao walks close to ball
    def walkToBall(self):
        ballPosition = self.tracker.getPosition()
        headYawTreshold = ((math.pi*10)/180)
        x = ballPosition[0]/2 + 0.05
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.turnToBall()
        self.motion.post.walkTo(x,0,0)
        while True:
            headYawAngle = self.motion.getAngles("HeadYaw", False)
            if headYawAngle[0] >= headYawTreshold or headYawAngle[0] <= -headYawTreshold:
                self.motion.stopWalk()
                self.turnToBall()
                self.walkToBall()
                break
            #dist = self.getDistance()
            # if dist < 0.1:
            #     self.motion.stopWalk()
            #     self.turnToBall()
            #     self.safePosition()
            #     time.sleep(1)
            #     self.tts.say("You have 5 seconds to move the ball")
            #     time.sleep(5)
            #     # naoWalkToPosition()
            #     self.walkToPosition()
            #     self.setTopCamera()
            #     break
                return


    # nao turns to ball 
    def turnToBall(self):
        if not self.hasBall():
            return False
        self.ballPosition = self.tracker.getPosition()
        b = self.ballPosition[1]/self.ballPosition[0]
        z = math.atan(b)
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.motion.walkTo(0,0,z)


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

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getPosition [as 别名]
    return transform_to_str(mm)

def cam_rotation_matrix():
    return np.matrix('0 0 1 0;-1 0 0 0; 0 -1 0 0; 0 0 0 1')

if __name__ == '__main__':
        rospy.init_node('test_tf')
        listener = TransformListener()
        ip = "ra.local"
        port = 9559
        proxy = ALProxy("ALMotion", ip, port)
        print "motionproxy ready"
        space = motion.SPACE_TORSO
        chainName = "Head"
        currentSensor = proxy.getPosition(chainName, space, True)
        current = proxy.getPosition(chainName, space, False)
        print "Position of %s in space %s:"%(chainName, space_to_str(space))
        print currentSensor
        print current
        rpy = currentSensor[3:]
        print rpy
        DCM = apply(transformations.euler_matrix,rpy)
        print DCM
        tTHSensor = proxy.getTransform(chainName, space, True)
        tTH = proxy.getTransform(chainName, space, False)
        print "Transform %s to %s:"%(space_to_str(space),chainName)
        print transform_to_str(tTHSensor)
        #print transform_to_str(tTH)
        chainName = "CameraTop"
        tTCSensor = proxy.getTransform(chainName, space, True)

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

#.........这里部分代码省略.........
        origin in the center of the torso. World Space has the origin at the
        ground between the legs and is left behind as the Nao moves.

        Parameters:
        rect: The rectangle on the image (in relative coordinates) where the
              object is
        distance: The distance in centimeters to the object, in Torso space
        camera: Which camera is being used; 0 for the forward camera, 1
                for the alternative camera.
        lookat: When True, the Nao will look at the object specified
        space: The space in which to return the coordinates, either motion.SPACE_NAO, 
               motion.SPACE_TORSO or motion.SPACE_WORLD
        """
        if not width and not distance:
            raise Exception("No distance and no object width specified; " \
                            + "one is required to calculate position")
        ###############################################################################
        # SET UP PARAMETERS
        ###############################################################################

        # Get the physical location and orientation of the camera
        if 0 == camera: # Head camera
            CAMERA = "CameraTop"
        else:           # Chin camera
            CAMERA = "CameraBottom"

        # Axes for convenience
        axis_x = V3(1, 0, 0)
        axis_y = V3(0, 1, 0)
        axis_z = V3(0, 0, 1)

        # Get camera position from the Aldebaran Motion proxy
        CAMERA_X, CAMERA_Y, CAMERA_Z, CAMERA_ROLL, CAMERA_PITCH, CAMERA_YAW = \
            self.__Motion.getPosition(CAMERA, space, True)

        # We use distances in centimeters, Motion proxy returns in meters
        camera_pos = V3(CAMERA_X * 100, CAMERA_Y * 100, CAMERA_Z * 100)

        # Calculate corners (coordinates in relative values in image)
        left, top, rel_width, rel_height = rect
        corners = [(left, top), 
                   (left, top + rel_height),
                   (left + rel_width, top),
                   (left + rel_width, top + rel_height)]

        ###############################################################################
        # CALCULATE OBJECT POSITION IN SELECTED SPACE
        ###############################################################################

        # If the width is specified, calculate the distance to the object
        if width:
            left_top = corners[0]
            right_top = corners[2]
            
            l_yaw, l_pitch = self.__Video.getAngPosFromImgPos([left_top[0], left_top[1]])
            r_yaw, r_pitch = self.__Video.getAngPosFromImgPos([right_top[0], right_top[1]])

            l_sel_space = V3(1, 0, 0).rotate_around(axis_z, l_yaw + CAMERA_YAW)     \
                                     .rotate_around(axis_y, r_pitch - CAMERA_PITCH) \
                                     .rotate_around(axis_x, CAMERA_ROLL)
            r_sel_space = V3(1, 0, 0).rotate_around(axis_z, r_yaw + CAMERA_YAW)     \
                                     .rotate_around(axis_y, r_pitch - CAMERA_PITCH) \
                                     .rotate_around(axis_x, CAMERA_ROLL)

            # Calculate the distance based on the calculated points
            distance = l_sel_space.x * (width / abs(l_sel_space.y - r_sel_space.y))