def test_create_axis_angle(self):
		axis = tft.random_vector(3)
		angle = random.random() * 2 * pi
		
		q = tft.quaternion_about_axis(angle, axis)
		
		q2 = tb_angles(axis,angle).quaternion
		
		self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2)))
		
		q2 = tb_angles(angle,axis).quaternion
		
		self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2)))
		
		q2 = tb_angles((axis,angle)).quaternion
		
		self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2)))
		
		q2 = tb_angles((angle,axis)).quaternion
		
		self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2)))
		
		for _ in xrange(1000):
			axis = tft.random_vector(3)
			angle = random.random() * 2 * pi
			
			q = tft.quaternion_about_axis(angle, axis)
			q2 = tb_angles(axis,angle).quaternion
			
			self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close! for axis %s and angle %f' % (list(q),list(q2),tuple(axis),angle))

    def least_squares_plane_normal(self, points_3d):
        """ returns a pose perpendicular to the plance of the points.
            note: the pose is arbitrary set to have the position as the first point 
                  in points_3d"""

        # form lists for x, y, and z values
        x_list = [a.point.x for a in points_3d]
        y_list = [a.point.y for a in points_3d]
        z_list = [a.point.z for a in points_3d]

        # convert to numpy arrays
        x_data = np.array(x_list)
        y_data = np.array(y_list)
        z_data = np.array(z_list)
        A = np.vstack([x_data, y_data, np.ones(len(x_data))]).T
        result = np.linalg.lstsq(A, z_data)[0]
        normal = np.array([result[0], result[1], -1]) # vector normal to fitted plane

        # get a pose by producing two arbitrary vectors perpendicular to normal
        x_axis = normal
        y_axis = np.array([0.5/x_axis[0], 0.5/x_axis[1], 1]) # arbitrary perpendicular vector
        z_axis = np.cross(x_axis, y_axis)

        # construct the pose
        rotMat = np.vstack((x_axis, y_axis, z_axis)).T
        tbRot = tfx.tb_angles(rotMat).matrix
        quat = tfx.tb_angles(tbRot).quaternion
        position = [x_list[0], y_list[0], z_list[0]]
        pose = tfx.pose(position, quat, frame='left_BC')
        
        if DEBUG:
            self.leastsq_plane_pub.publish(pose.msg.PoseStamped())

        return pose

 def get_orientation_from_lines(self, v0, v1):
     """ 
     Takes in two vectors, v0 and v1, (which are KNOWN to be in the same plane) and finds 
     the normal, and creates a rotation matrix from v0, v1, and the normal; then converts this 
     rotation matrix to a quaternion 
     """
     v0, v1 = np.array(v0), np.array(v1)
     v0 = v0 / np.linalg.norm(v0) 
     v1 = v1 / np.linalg.norm(v1) 
     n = np.cross(v0, v1)
     parallel = average_vectors(v0, v1)
     parallel = parallel / np.linalg.norm(parallel)
     third = np.cross(n, parallel)
     third = third / np.linalg.norm(third)
     #n = n / np.linalg.norm(n)
     #v1 = np.cross(n, v0)
     #v1 = v1 / np.linalg.norm(v1)
     #rotMat = np.vstack((n, v1, v0)).T
     rotMat = np.vstack((parallel, third, n)).T
     matrix = rotMat
     tbRot = tfx.tb_angles(matrix).matrix        
     #tbRot = self.rotate(-90, "yaw", tbRot)    #FIXME: get correct yaw pitch roll values
     #tbRot = self.rotate(60, "pitch", tbRot)
     tbRot = self.rotate(180, "roll", tbRot)
     quat = tfx.tb_angles(tbRot).quaternion
     return list(quat)

def testAngleBetween():
    quat0 = tfx.tb_angles(-82,90,98).msg
    quat1 = tfx.tb_angles(-3,90,-8).msg

    theta = angleBetweenQuaternions(quat0, quat1)

    print("theta = {0}".format(theta))

def deltaPose(currPose, desPose, posFrame=None, rotFrame=None):
    """
    Returns pose0 - pose1
    """

    currPose, desPose = tfx.pose(currPose), tfx.pose(desPose)
    currPoseFrame, desPoseFrame = currPose.frame, desPose.frame
    
    currPos, desPos = currPose.position, desPose.position
    currRot, desRot = currPose.orientation, desPose.orientation

    if posFrame is not None and currPoseFrame is not None:
        tf_currPos_to_posFrame = tfx.lookupTransform(posFrame, currPoseFrame, wait=10)
        currPos = tf_currPos_to_posFrame * currPos

        tf_desPos_to_posFrame = tfx.lookupTransform(posFrame, desPoseFrame, wait=10)
        desPos = tf_desPos_to_posFrame * desPos

    if rotFrame is not None and currPoseFrame is not None:
        tf_currRot_to_rotFrame = tfx.lookupTransform(rotFrame, currPoseFrame, wait=10)
        currRot = tf_currRot_to_rotFrame * currRot

        tf_desRot_to_rotFrame = tfx.lookupTransform(rotFrame, desPoseFrame, wait=10)
        desRot = tf_desRot_to_rotFrame * desRot

    deltaPosition = desPos.array - currPos.array
    
    currQuat, desQuat = tfx.tb_angles(currRot).quaternion, tfx.tb_angles(desRot).quaternion
    deltaQuat = tft.quaternion_multiply(tft.quaternion_inverse(currQuat), desQuat)

    deltaPose = tfx.pose(deltaPosition, deltaQuat)

    return deltaPose

def test_grasp_planner():
    sim = simulator.Simulator(view=True)
    rarm = arm.Arm('right', sim=sim)
    p = planner.Planner('right', sim=sim)
    
    rarm.set_posture('mantis')
    curr_pose = rarm.get_pose()
    curr_pose.orientation = tfx.tb_angles(0,0,180)
    rarm.set_pose(curr_pose)
    
    target_pose = rarm.get_pose() + [0.5, 0.4, -0.3]
    target_pose.orientation *= tfx.tb_angles(25,-20,10)
    sim.plot_transform(target_pose.matrix)

    start_joints = rarm.get_joints()
    n_steps = 20
    joint_traj = p.get_grasp_joint_trajectory(start_joints, target_pose, n_steps=n_steps, ignore_orientation=True, link_name='r_gripper_center_frame')
    
    poses = [tfx.pose(rarm.fk(joints)) for joints in joint_traj]
    for pose in poses:
        sim.plot_transform(pose.matrix)
        
    for n in xrange(n_steps-1):
        angle = (poses[n].orientation.inverse()*poses[-1].orientation).angle
        print('angle[{0}]: {1}'.format(n, angle))
    
    #IPython.embed()
    
    print('Press enter to exit')
    raw_input()

    def poseFromMarkers2(self, markers, target_ids, rotate=False):
        if len(markers) >= 3:
            points = [np.zeros((3,1)), np.zeros((3,1)), np.zeros((3,1))]
            
            for m in markers:
                #print 'Pos',m.id,m.x,m.y,m.z
                points[target_ids.index(m.id)] = np.array([m.x, m.y, m.z])
            
            leftPoint = points[0]
            centerPoint = points[1]
            rightPoint = points[2]
            
            leftVec = leftPoint - centerPoint
            rightVec = rightPoint - centerPoint
            leftVec = leftVec / np.linalg.norm(leftVec) 
            rightVec = rightVec / np.linalg.norm(rightVec) 
            
            zAxis = np.cross(rightVec, leftVec)    
            xAxis = (leftVec + rightVec) / 2
            xAxis= xAxis / np.linalg.norm(xAxis)
            yAxis = np.cross(zAxis, xAxis)

            
            rotMat = np.vstack((xAxis, yAxis, zAxis)).T
            tbRot = tfx.tb_angles(rotMat).matrix
            quat = tfx.tb_angles(tbRot).quaternion
            
            return 1e-3*centerPoint, quat
        return None

    def alignGrippers(self):
        rightArmPose = self.rightArm.get_pose()
        leftArmPose = self.leftArm.get_pose()
        newLeftArmPose = tfx.pose(leftArmPose.position, tfx.tb_angles(0.0, 0.0, 0.0))  # Create new pose

        newRightArmPose = tfx.pose(rightArmPose.position, tfx.tb_angles(0.0, 0.0, 0.0))
        newRightJoints = self.rightArm.ik(newRightArmPose)  # Updates arm pose if valid
        newLeftJoints = self.leftArm.ik(newLeftArmPose)
        if newRightJoints is not None:
            self.rightArm.go_to_joints(newRightJoints)
            print ("new_pose: {0}".format(newRightArmPose))
            print "Aligning right gripper..."
            self.rightArm.open_gripper()
            self.valid = True
        else:
            rospy.loginfo("Invalid Right Arm Pose")
            print "RIGHT ALIGNMENT FAILED"
            self.valid = False
            return self
        rospy.sleep(0.5)
        if newLeftJoints is not None:
            self.leftArm.go_to_joints(newLeftJoints)
            print "Aligning left gripper..."
            self.leftArm.open_gripper()
        else:
            rospy.loginfo("Invalid Left Arm Pose")
            print "LEFT ALIGNMENT FAILED"
        rospy.sleep(0.5)

    def _handles_loop(self):
        """
        For each handle in HandleListMsg,
        calculate average pose
        """
        while not rospy.is_shutdown():
            
            self.handle_list_msg = None
            self.camera_pose = None
            while not rospy.is_shutdown() and (self.handle_list_msg is None or self.camera_pose is None):
                rospy.sleep(.01)
            handle_list_msg = self.handle_list_msg
            camera_pose = self.camera_pose
            
            pose_array = gm.PoseArray()
            pose_array.header.frame_id = 'base_link'
            pose_array.header.stamp = rospy.Time.now()
            
            avg_pose_array = gm.PoseArray()
            avg_pose_array.header.frame_id = 'base_link'
            avg_pose_array.header.stamp = rospy.Time.now()
    
            if handle_list_msg.header.frame_id.count('base_link') > 0:
                cam_to_base = np.eye(4)
            else:
                cam_to_base = camera_pose.matrix #tfx.lookupTransform('base_link', handle_list_msg.header.frame_id).matrix
            switch = np.matrix([[0, 1, 0],
                                [1, 0, 0],
                                [0, 0, 1]])        
            for handle in handle_list_msg.handles:
                all_poses = [tfx.pose(cylinder.pose, stamp=rospy.Time.now(), frame=handle_list_msg.header.frame_id) for cylinder in handle.cylinders]
                
                rotated_poses = [tfx.pose(p.position, tfx.tb_angles(p.orientation.matrix*switch)) for p in all_poses]
                filtered_poses = list()
                for rot_pose in rotated_poses:
                    r_base = cam_to_base[:3,:3]*rot_pose.orientation.matrix
                    if r_base[0,0] > 0:
                        if r_base[2,2] > 0:
                            rot_pose.orientation = tfx.tb_angles(rot_pose.orientation.matrix*tfx.tb_angles(0,0,180).matrix) 
                        filtered_poses.append(rot_pose)
                
                filtered_poses = [tfx.pose(cam_to_base*pose.matrix, frame='base_link') for pose in filtered_poses]
                filtered_poses = filter(lambda pose: min(self.min_positions < pose.position.array) and min(pose.position.array < self.max_positions), filtered_poses)
                pose_array.poses += [pose.msg.Pose() for pose in filtered_poses]
                
                if len(filtered_poses) > 0:
                    avg_position = sum([p.position.array for p in filtered_poses])/float(len(filtered_poses))
                    avg_quat = sum([p.orientation.quaternion for p in filtered_poses])/float(len(filtered_poses))
                    avg_pose_array.poses.append(tfx.pose(avg_position, avg_quat).msg.Pose())
                

            self.handles_pose_pub.publish(pose_array)
            self.avg_handles_pose_pub.publish(avg_pose_array)
            
            self.logger_pub.publish('handles {0}'.format(len(avg_pose_array.poses)))

def testRotation():
    rospy.init_node('ar_image_detection')

    imageDetector = ARImageDetector()
    listener = tf.TransformListener()
    tf_br = tf.TransformBroadcaster()
    

    while not rospy.is_shutdown():
          if imageDetector.hasFoundGripper(Constants.Arm.Left):
                obj = tfx.pose([0,0,0], imageDetector.normal).msg.PoseStamped()
                gripper = imageDetector.getGripperPose(Constants.Arm.Left)

                print('gripper')
                print(gripper)

                # obj_tb = tfx.tb_angles(obj.pose.orientation)
                gripper_tb = tfx.tb_angles(gripper.pose.orientation)
                print "gripper ori", gripper_tb
                obj_tb = tfx.tb_angles(imageDetector.normal)
                print "obj ori", obj_tb
                pt = gripper.pose.position
                ori = imageDetector.normal
                tf_br.sendTransform((pt.x, pt.y, pt.z), (ori.x, ori.y, ori.z, ori.w),
                                    gripper.header.stamp, '/des_pose', Constants.AR.Frames.Base)
                
                
                between = Util.angleBetweenQuaternions(ori, gripper_tb.msg)
                print('Angle between')
                print(between)

                quat = tft.quaternion_multiply(gripper_tb.quaternion, tft.quaternion_inverse(obj_tb.quaternion))
                print 'new', tfx.tb_angles(quat)

                #rot = gripper_tb.rotation_to(ori)
                rot = gripper_tb.rotation_to(obj_tb)
                print('Rotation from gripper to obj')
                print(rot)

                deltaPoseTb = tfx.pose(Util.deltaPose(gripper, obj)).orientation
                print('deltaPose')
                print(deltaPoseTb)

                deltaPose = tfx.pose([0,0,0], deltaPoseTb).msg.PoseStamped()
                time = listener.getLatestCommonTime('0_link', 'tool_L')
                deltaPose.header.stamp = time
                deltaPose.header.frame_id = '0_link'
                deltaPose = listener.transformPose('tool_L', deltaPose)
                print "transformed", tfx.tb_angles(deltaPose.pose.orientation)

                endQuatMat = gripper_tb.matrix * rot.matrix
                print 'desOri', tfx.tb_angles(endQuatMat)
                

          rospy.sleep(1)

	def test_to_tf(self):
		tb = tb_angles(45,-5,24)
		self.assertTrue(tb.to_tf().flags.writeable)
		
		for yaw in self.yaw:
			for pitch in self.pitch:
				for roll in self.roll:
					tb = tb_angles(yaw,pitch,roll)
					m = tb.to_tf()
					m2 = tft.euler_matrix(yaw * pi / 180, pitch * pi / 180, roll * pi / 180, 'rzyx')
					self.assertTrue(np.allclose(m, m2),msg='%s and %s not close!' % (m,m2))

	def test_create_quat(self):
		for yaw in self.yaw:
			for pitch in self.pitch:
				for roll in self.roll:
					q = tft.quaternion_from_euler(yaw * pi / 180., pitch * pi / 180., roll * pi / 180., 'rzyx')
					tb = tb_angles(q)
					self.assertTbEqual(tb, yaw, pitch, roll)
					
					q2 = list(q)
					tb = tb_angles(q2)
					self.assertTbEqual(tb, yaw, pitch, roll)

 def rotate(self, angle, axis, matrix):
     """ 
     Takes a rotation matrix and rotates it a certain angle about a certain axis 
     """
     if axis == "yaw":
         rot = tfx.tb_angles(angle, 0, 0).matrix
     elif axis == "pitch":
         rot = tfx.tb_angles(0, angle, 0).matrix
     elif axis == "roll":
         rot = tfx.tb_angles(0, 0, angle).matrix
     return matrix*rot

def testSwitchPlaces(armNames=[MyConstants.Arm.Left,MyConstants.Arm.Right],fixedPose=False):
    rospy.init_node('testSwitchPlaces',anonymous=True)
    rp = RavenPlanner(armNames)
    rospy.sleep(2)
    
    leftStartJoints = None
    rightStartJoints = None
    
    if fixedPose:
        rightCurrPose = tfx.pose([-0.068,-0.061,-0.129],tfx.tb_angles(-139.6,88.5,111.8),frame=MyConstants.Frames.Link0)
        
        leftCurrPose = tfx.pose([-.072,-.015,-.153],tfx.tb_angles(43.9,78.6,100.9),frame=MyConstants.Frames.Link0)
    
        leftStartJoints = rp.getJointsFromPose(MyConstants.Arm.Left, leftCurrPose, grasp=0)
        rightStartJoints = rp.getJointsFromPose(MyConstants.Arm.Right, rightCurrPose, grasp=0)
    else:
        leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0)
        rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0)
        
    
    #trajoptpy.SetInteractive(True)
    
    if fixedPose:
        rp.getTrajectoryFromPose(MyConstants.Arm.Left, rightCurrPose, endGrasp=0, startJoints=leftStartJoints, debug=True)
        rp.getTrajectoryFromPose(MyConstants.Arm.Right, leftCurrPose, endGrasp=0, startJoints=rightStartJoints, debug=True)
    else:
        rp.getTrajectoryFromPose(MyConstants.Arm.Left, rightCurrPose, leftStartJoints, debug=True)
        rp.getTrajectoryFromPose(MyConstants.Arm.Right, leftCurrPose, rightStartJoints, debug=True)
    
    """
    for index in range(len(armNames)):
        armName = armNames[index]
        otherArmName = armNames[(index+1)%len(armNames)]
        desPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[otherArmName]),MyConstants.Frames.Link0)
        
        rp.getTrajectoryFromPose(armName, desPose, debug=True)
    """
    
    rp.getTrajectoryFromPoseThread(once=True)
#     while rp.trajRequest[armNames[0]] and rp.trajRequest[armNames[1]] and not rospy.is_shutdown():
#         rospy.sleep(.05)
        
    leftTraj = rp.jointTraj[MyConstants.Arm.Left]
    rightTraj = rp.jointTraj[MyConstants.Arm.Right]
    
    rp.env.SetViewer('qtcoin')
    
    for left, right in zip(leftTraj,rightTraj):
        rospy.loginfo('Press enter to go to next step')
        raw_input()
        rp.updateOpenraveJoints('L', left)
        rp.updateOpenraveJoints('R', right)

    def calculateDeltaPose(self, prev_frame, curr_frame, grip, tipDistance):  #TODO: MEASURE CHANGE IN POSE -- ABSOLUTE? 
        #ROTATION = tfx.pose([0,0,0],[0,180,90]).matrix 
        #print "here"
        prevPose = self.prevPose
        #prevPose = ROTATION*prevPose 
        prev_x, prev_y, prev_z = prevPose.translation.x, prevPose.translation.y, prevPose.translation.z
        prevOrientation = prevPose.orientation
        if type(prev_frame) != type(None) and type(curr_frame) != type(None): 
            # IF we actually have frames
            translation, rotation, palm_normal, palm_direction = calculateTransform(prev_frame, curr_frame, 0)
            if type(translation) != type(None) and type(rotation) != type(None): 
                # IF we have a valid translation/rotation

                # translation stuff
                dx, dy, dz = (translation[0]*self.x_scale, translation[2]*self.y_scale, translation[1]*self.z_scale)         
                
                # orientation stuff
                
                if self.FIXED_ORIENTATION:
                    #print "fixed"
                    currOrientation = tfx.tb_angles(-90,90,0)
                else:
                    y = palm_direction.yaw 
                    p = palm_direction.pitch 
                    r = palm_normal.roll
                    y *= 57.2957795   # convert to degrees
                    p *= 57.2957795
                    r *= 57.2957795

                    y += -90  # adjust values for frame transformation
                    p += 90
                    r += 0
                    print "YPR: "+str(y) + ", "+str(p) + ", "+str(r)
                    currOrientation = tfx.tb_angles(y, p, r)
                    print "CURR ORIENTATION: "+str(currOrientation)
                    print "PALM NORMAL:      "+str(palm_normal)

                if math.fabs(dx) > DX_UPPER_BOUND or math.fabs(dy) > DY_UPPER_BOUND or math.fabs(dz) > DZ_UPPER_BOUND: 
                    # IF the movement goes beyond bounds, don't do it
                    (dx, dy, dz) = (0,0,0)
                    currOrientation = prevOrientation
            else:  
                # IF we don't have a valid translation or rotation, don't move
                dx, dy, dz = (0,0,0)
                currOrientation = prevOrientation
        else: 
            # IF we don't have frames, also don't move
            dx, dy, dz = (0,0,0)
            currOrientation = prevOrientation
        curr_x, curr_y, curr_z = (prev_x+dx, prev_y+dy, prev_z+dz) # ADD IN THE DELTA COMMAND
        return (dx, dy, dz), currOrientation 

 def teleop(self):
     print('{0} arm teleop'.format(self.lrlong))
     
     pos_step = .01
     delta_position = {'a' : [0, pos_step, 0],
                       'd' : [0, -pos_step, 0],
                       'w' : [pos_step, 0, 0],
                       'x' : [-pos_step, 0, 0],
                       '+' : [0, 0, pos_step],
                       '-' : [0, 0, -pos_step]}
     
     angle_step = 2.0
     delta_angle = {'o' : [angle_step, 0, 0],
                    'p' : [-angle_step, 0, 0],
                    'k' : [0, angle_step, 0],
                    'l' : [0, -angle_step, 0],
                    'n' : [0, 0, angle_step],
                    'm' : [0, 0, -angle_step]}
     
     char = ''
     while char != 'q':
         char = utils.Getch.getch()
         pose = self.get_pose()
         new_pose = tfx.pose(pose)
         if delta_position.has_key(char):
             new_pose.position = pose.position.array + delta_position[char]
         ypr = np.array([pose.tb_angles.yaw_deg, pose.tb_angles.pitch_deg, pose.tb_angles.roll_deg])
         if delta_angle.has_key(char):
             ypr += np.array(delta_angle[char])
             new_pose = tfx.pose(pose.position, tfx.tb_angles(ypr[0], ypr[1], ypr[2]))
         self.set_pose(new_pose)    
         time.sleep(.01)
         
     print('{0} arm end teleop'.format(self.lrlong))

    def __init__(self, arm=Constants.Arm.Left):
        self.arm = arm

        if self.arm == Constants.Arm.Left:
            self.toolframe = Constants.Frames.LeftTool
        else:
            self.toolframe = Constants.Frames.RightTool

        self.transFrame = Constants.Frames.Link0
        self.rotFrame = self.toolframe
            
        self.gripperControl = GripperControlClass(arm, tf.TransformListener())
        self.imageDetector = ARImageDetectionClass()
                
        # manually add by checking print_state
        self.startPose = None

        # ar_frame is the target
        self.ar_frame = '/stereo_32'
        self.objectPose = tfx.pose([0,0,0],tfx.tb_angles(-90,90,0),frame=self.ar_frame)
        
        self.listener = tf.TransformListener()
        self.tf_br = tf.TransformBroadcaster()
        self.delta_pub = rospy.Publisher('delta_pose', PoseStamped)
        self.obj_pub = rospy.Publisher('object_pose', PoseStamped)

        rospy.sleep(3)

    def __init__(self, arm=Constants.Arm.Left):
        self.arm = arm

        if self.arm == Constants.Arm.Left:
            self.toolframe = Constants.Frames.LeftTool
        else:
            self.toolframe = Constants.Frames.RightTool

        self.transFrame = Constants.Frames.Link0
        self.rotFrame = self.toolframe
            
        self.gripperControl = GripperControlClass(arm, tf.TransformListener())
        self.imageDetector = ARImageDetectionClass()
                
        self.homePose = self.imageDetector.getHomePose()

        # ar_frame is the target
        self.ar_frame = '/stereo_33'
        self.objectPose = tfx.pose([0,0,0], frame=self.ar_frame)
        tf_ar_to_link0 = tfx.lookupTransform(Constants.Frames.Link0, self.ar_frame, wait=5)
        self.objectPose = tf_ar_to_link0 * self.objectPose
        self.objectPose = tfx.pose(self.objectPose.position, tfx.tb_angles(-90,90,0))
        self.objectPose.position.y -= .002

        rospy.sleep(3)

 def _handles_loop(self):
     """
     For each handle in HandleListMsg,
     calculate average pose
     """
     rospy.sleep(5)
     
     while not rospy.is_shutdown() and not self.exited:
         rospy.sleep(.01)
         
         handle_list_msg = self.handle_list_msg
         
         pose_array = gm.PoseArray()
         pose_array.header.frame_id = handle_list_msg.header.frame_id
         pose_array.header.stamp = rospy.Time.now()
         
         avg_pose_array = gm.PoseArray()
         avg_pose_array.header.frame_id = handle_list_msg.header.frame_id
         avg_pose_array.header.stamp = rospy.Time.now()
 
         cam_to_base = tfx.lookupTransform('base_link', handle_list_msg.header.frame_id).matrix[:3,:3]
         switch = np.matrix([[0, 1, 0],
                             [1, 0, 0],
                             [0, 0, 1]])        
         for handle in handle_list_msg.handles:
             all_poses = [tfx.pose(cylinder.pose, stamp=rospy.Time.now(), frame=handle_list_msg.header.frame_id) for cylinder in handle.cylinders]
             
             rotated_poses = [tfx.pose(p.position, tfx.tb_angles(p.orientation.matrix*switch)) for p in all_poses]
             filtered_poses = list()
             for rot_pose in rotated_poses:
                 r_base = cam_to_base*rot_pose.orientation.matrix
                 if r_base[0,0] > 0:
                     if r_base[2,2] > 0:
                         rot_pose.orientation = tfx.tb_angles(rot_pose.orientation.matrix*tfx.tb_angles(0,0,180).matrix) 
                     filtered_poses.append(rot_pose)
             
             pose_array.poses += [pose.msg.Pose() for pose in filtered_poses]
             
             if len(filtered_poses) > 0:
                 avg_position = sum([p.position.array for p in filtered_poses])/float(len(filtered_poses))
                 avg_quat = sum([p.orientation.quaternion for p in filtered_poses])/float(len(filtered_poses))
                 avg_pose_array.poses.append(tfx.pose(avg_position, avg_quat).msg.Pose())
             
             
         if len(pose_array.poses) > 0:
             self.handles_pose_pub.publish(pose_array)
             self.avg_handles_pose_pub.publish(avg_pose_array)

def iterate_angles():
    roll = np.linspace(120, 160, num=10)
    pitch = np.linspace(-50, 50, num=10)
    yaw = np.linspace(50, 120, num=10)
    time.sleep(2)
    initial_angle = (88.0984719856, -12.0248544998, 131.680496265)
    quaternion = tfx.tb_angles(initial_angle[0], initial_angle[1], initial_angle[2])
    frame = get_frame_rot(quaternion)
    psm1.move_cartesian_frame(frame)
    psm1.open_gripper(80.0)

    for i in range(10):
        angle = (yaw[i], initial_angle[1], initial_angle[2])
        quaternion = tfx.tb_angles(angle[0], angle[1], angle[2])
        frame = get_frame_rot(quaternion)
        psm1.move_cartesian_frame(frame)
        time.sleep(0.2)