def align_poses(self, ps1, ps2):

        self.aul.update_frame(ps1)
        ps2.header.stamp = rospy.Time.now()
        self.tfl.waitForTransform(ps2.header.frame_id, 'lh_utility_frame',
                                  rospy.Time.now(), rospy.Duration(3.0))
        ps2_in_ps1 = self.tfl.transformPose('lh_utility_frame', ps2)

        ang = math.atan2(-ps2_in_ps1.pose.position.z,
                         -ps2_in_ps1.pose.position.y) + (math.pi / 2)
        q_st_rot = transformations.quaternion_about_axis(ang, (1, 0, 0))
        q_st_new = transformations.quaternion_multiply([
            ps1.pose.orientation.x, ps1.pose.orientation.y,
            ps1.pose.orientation.z, ps1.pose.orientation.w
        ], q_st_rot)
        ps1.pose.orientation = Quaternion(*q_st_new)

        self.aul.update_frame(ps2)
        ps1.header.stamp = rospy.Time.now()
        self.tfl.waitForTransform(ps1.header.frame_id, 'lh_utility_frame',
                                  rospy.Time.now(), rospy.Duration(3.0))
        ps1_in_ps2 = self.tfl.transformPose('lh_utility_frame', ps1)
        ang = math.atan2(ps1_in_ps2.pose.position.z,
                         ps1_in_ps2.pose.position.y) + (math.pi / 2)

        q_st_rot = transformations.quaternion_about_axis(ang, (1, 0, 0))
        q_st_new = transformations.quaternion_multiply([
            ps2.pose.orientation.x, ps2.pose.orientation.y,
            ps2.pose.orientation.z, ps2.pose.orientation.w
        ], q_st_rot)
        ps2.pose.orientation = Quaternion(*q_st_new)
        return ps1, ps2

	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 __init__(self, full_dof=False):
        # Waypoint set
        self._waypoints = None

        # True if the path is generated for all degrees of freedom, otherwise
        # the path will be generated for (x, y, z, yaw) only
        self._is_full_dof = full_dof

        # The parametric variable to use as input for the interpolator
        self._s = list()
        self._segment_to_wp_map = list()
        self._cur_s = 0
        self._s_step = 0.0001

        self._start_time = None
        self._duration = None

        self._termination_by_time = True

        self._final_pos_tolerance = 0.1

        self._init_rot = quaternion_about_axis(0.0, [0, 0, 1])
        self._last_rot = quaternion_about_axis(0.0, [0, 0, 1])

        self._markers_msg = MarkerArray()
        self._marker_id = 0

	def test_to_axis_angle(self):
		for _ in xrange(1000):
			axis = tft.random_vector(3)
			axis = axis / np.linalg.norm(axis)
			for angle in list(np.linspace(-pi, pi, 10)) + [0]:
			
				q = tft.quaternion_about_axis(angle, axis)
				axis2,angle2 = tb_angles(q).axis_angle
				q2 = tft.quaternion_about_axis(angle2, axis2)
				
				self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg="axis %s and angle %f don't match %s, %f" % (tuple(axis),angle,tuple(axis2),angle2))

    def _compute_rot_quat(self, dx, dy, dz):
        rotq = quaternion_about_axis(
            np.arctan2(dy, dx),
            [0, 0, 1])

        if self._is_full_dof:
            rote = quaternion_about_axis(
                -1 * np.arctan2(dz, np.sqrt(dx**2 + dy**2)),
                [0, 1, 0])
            rotq = quaternion_multiply(rotq, rote)
        return rotq

def euler_to_quat_deg(roll, pitch, yaw):
    roll = roll * (math.pi / 180.0)
    pitch = pitch * (math.pi / 180.0)
    yaw = yaw * (math.pi / 180.0)
    xaxis = (1, 0, 0)
    yaxis = (0, 1, 0)
    zaxis = (0, 0, 1)
    qx = transformations.quaternion_about_axis(roll, xaxis)
    qy = transformations.quaternion_about_axis(pitch, yaxis)
    qz = transformations.quaternion_about_axis(yaw, zaxis)
    q = transformations.quaternion_multiply(qx, qy)
    q = transformations.quaternion_multiply(q, qz)
    print q

    def __init__(self, plugin):
        super(Theta360ViewWidget, self).__init__()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('theta_viewer'), 'resource', 'Theta360ViewWidget.ui')
        loadUi(ui_file, self)
        self.plugin = plugin

        self.position = (0.0, 0.0, 0.0)
        self.orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
        self.topicName = None
        self.subscriber = None

        # create GL view
        self._glview = MyGLWidget()
        self._glview.setAcceptDrops(True)

        # backup and replace original paint method
        # self.glView.paintGL is callbacked from QGLWidget
        self._glview.paintGL_original = self._glview.paintGL
        self._glview.paintGL = self.glview_paintGL

        # backup and replace original mouse release method
        self._glview.mouseReleaseEvent_original = self._glview.mouseReleaseEvent
        self._glview.mouseReleaseEvent = self.glview_mouseReleaseEvent

        # add GL view to widget layout
        self.layout().addWidget(self._glview)

        # init and start update timer with 40ms (25fps)
        # updateTimeout is called with interval time
        self.update_timer = QTimer(self)
        self.update_timer.timeout.connect(self.update_timeout)
        self.update_timer.start(40)
        self.cnt = 0

def aim_frame_to(target_pt, point_dir=(1,0,0)):
    goal_dir = np.array([target_pt.x, target_pt.y, target_pt.z])
    goal_norm = np.divide(goal_dir, np.linalg.norm(goal_dir))
    point_norm = np.divide(point_dir, np.linalg.norm(point_dir))
    axis = np.cross(point_norm, goal_norm)
    angle = np.arccos(np.vdot(goal_norm, point_norm))
    return tft.quaternion_about_axis(angle, axis)

    def generate_quat(self, s):
        s = max(0, s)
        s = min(s, 1)

        last_s = s - self._s_step
        if last_s == 0:
            last_s = 0

        if s == 0:
            self._last_rot = deepcopy(self._init_rot)
            return self._init_rot

        this_pos = self.generate_pos(s)
        last_pos = self.generate_pos(last_s)
        dx = this_pos[0] - last_pos[0]
        dy = this_pos[1] - last_pos[1]
        dz = this_pos[2] - last_pos[2]

        rotq = self._compute_rot_quat(dx, dy, dz)
        self._last_rot = deepcopy(rotq)

        # Calculating the step for the heading offset
        q_step = quaternion_about_axis(
            self._interp_fcns['heading'](s),
            np.array([0, 0, 1]))
        # Adding the heading offset to the rotation quaternion
        rotq = quaternion_multiply(rotq, q_step)
        return rotq

    def __init__(self, plugin):
        super(PoseViewWidget, self).__init__()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_pose_view'), 'resource', 'PoseViewWidget.ui')
        loadUi(ui_file, self)
        self._plugin = plugin

        self._position = (0.0, 0.0, 0.0)
        self._orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
        self._topic_name = None
        self._subscriber = None

        # create GL view
        self._gl_view = GLWidget()
        self._gl_view.setAcceptDrops(True)

        # backup and replace original paint method
        self._gl_view.paintGL_original = self._gl_view.paintGL
        self._gl_view.paintGL = self._gl_view_paintGL

        # backup and replace original mouse release method
        self._gl_view.mouseReleaseEvent_original = self._gl_view.mouseReleaseEvent
        self._gl_view.mouseReleaseEvent = self._gl_view_mouseReleaseEvent

        # add GL view to widget layout
        self.layout().addWidget(self._gl_view)

        # init and start update timer with 40ms (25fps)
        self._update_timer = QTimer(self)
        self._update_timer.timeout.connect(self.update_timeout)
        self._update_timer.start(40)

def draw_axes(pub, id, ns, pose_stamped, scale=(0.05, 0.1, 0.1), text=""):
    ps = PointStamped()
    ps.header = pose_stamped.header
    ps.point = pose_stamped.pose.position
    q_x = quaternion_to_array(pose_stamped.pose.orientation)
    q_y = quaternion_multiply(q_x, quaternion_about_axis(pi/2, (0, 1, 0)))
    q_z = quaternion_multiply(q_x, quaternion_about_axis(pi/2, (0, 0, 1)))
    if id > 999:
        rospy.logerr('Currently can\'t visualize markers with id > 999.')
        return
    place_arrow(ps, pub, id, ns, (1, 0, 0),\
            scale, array_to_quaternion(q_x))
    place_arrow(ps, pub, id + 1000, ns, (0, 1, 0),\
            scale, array_to_quaternion(q_y))
    place_arrow(ps, pub, id + 2000, ns, (0, 0, 1),\
            scale, array_to_quaternion(q_z), text=text)

def aim_pose_to(ps, pts, point_dir=(1,0,0)):
    if not (ps.header.frame_id.lstrip('/') == pts.header.frame_id.lstrip('/')):
        rospy.logerr("[Pose_Utils.aim_pose_to]:"+
                     "Pose and point must be in same frame: %s, %s"
                    %(ps.header.frame_id, pt2.header.frame_id))
    target_pt = np.array((pts.point.x, pts.point.y, pts.point.z))
    base_pt = np.array((ps.pose.position.x,
                        ps.pose.position.y,
                        ps.pose.position.z)) 
    base_quat = np.array((ps.pose.orientation.x, ps.pose.orientation.y,
                          ps.pose.orientation.z, ps.pose.orientation.w))

    b_to_t_vec = np.array((target_pt[0]-base_pt[0],
                           target_pt[1]-base_pt[1],
                           target_pt[2]-base_pt[2]))
    b_to_t_norm = np.divide(b_to_t_vec, np.linalg.norm(b_to_t_vec))

    point_dir_hom = (point_dir[0], point_dir[1], point_dir[2], 1)
    base_rot_mat = tft.quaternion_matrix(base_quat)
    point_dir_hom = np.dot(point_dir_hom, base_rot_mat.T)
    point_dir = np.array((point_dir_hom[0]/point_dir_hom[3],
                         point_dir_hom[1]/point_dir_hom[3],
                         point_dir_hom[2]/point_dir_hom[3]))
    point_dir_norm = np.divide(point_dir, np.linalg.norm(point_dir))
    axis = np.cross(point_dir_norm, b_to_t_norm)
    angle = np.arccos(np.vdot(point_dir_norm, b_to_t_norm))
    quat = tft.quaternion_about_axis(angle, axis)
    new_quat = tft.quaternion_multiply(quat, base_quat)
    ps.pose.orientation = Quaternion(*new_quat)

 def _odom_callback(self, odom_data):
     vehicle_position_x = odom_data.pose.pose.position.x
     vehicle_position_y = odom_data.pose.pose.position.y
     vehicle_position_z = odom_data.pose.pose.position.z
     self._position = (vehicle_position_x, vehicle_position_y, vehicle_position_z)
     self._mapDrawer.set_position(self._position)
     self._yaw = odom_data.pose.pose.orientation.z
     self._orientation = quaternion_about_axis(math.radians(self._yaw), (0.0, 0.0, 1.0))
     self._mapDrawer.set_orientation(self._orientation,self._yaw)

  def cb_master_state(self, msg):
    self.master_real_pos = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
    pos = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]) - self.center_pos
    vel = np.array([msg.velocity.x, msg.velocity.y, msg.velocity.z])
    self.master_pos = self.change_axes(pos)
    self.master_vel = self.change_axes(vel)

    # Rotate tu use the same axes orientation between master and slave
    real_rot = np.array([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])
    q_1 = tr.quaternion_about_axis(self.angle_rotation_1, self.axes_rotation_1)
    aux_rot = tr.quaternion_multiply(q_1, real_rot)
    q_2 = tr.quaternion_about_axis(self.angle_rotation_2, self.axes_rotation_2)
    aux_rot_2 = tr.quaternion_multiply(q_2, aux_rot)
    q_3 = tr.quaternion_about_axis(self.angle_rotation_3, self.axes_rotation_3)
    self.master_rot = tr.quaternion_multiply(q_3, aux_rot_2)

    #Normalize velocitity
    self.master_dir = self.normalize_vector(self.master_vel)

    def __init__(self, full_dof=False, use_finite_diff=True,
                 interpolation_method='cubic',
                 stamped_pose_only=False):
        """Class constructor."""
        self._logger = logging.getLogger('wp_trajectory_generator')
        out_hdlr = logging.StreamHandler(sys.stdout)
        out_hdlr.setFormatter(logging.Formatter(
            get_namespace().replace('/', '').upper() + ' -- %(asctime)s | %(levelname)s | %(module)s | %(message)s'))
        out_hdlr.setLevel(logging.INFO)
        self._logger.addHandler(out_hdlr)
        self._logger.setLevel(logging.INFO)

        self._path_generators = dict()
        self._logger.info('Waypoint interpolators available:')
        for gen in PathGenerator.get_all_generators():
            self._logger.info('\t - ' + gen.get_label())
            self._path_generators[gen.get_label()] = gen
            self._path_generators[gen.get_label()].set_full_dof(full_dof)
        # Time step between interpolated samples
        self._dt = None
        # Last time stamp
        self._last_t = None
        # Last interpolated point
        self._last_pnt = None
        self._this_pnt = None

        # Flag to generate only stamped pose, no velocity profiles
        self._stamped_pose_only = stamped_pose_only

        self._t_step = 0.001

        # Interpolation method
        self._interp_method = interpolation_method

        # True if the path is generated for all degrees of freedom, otherwise
        # the path will be generated for (x, y, z, yaw) only
        self._is_full_dof = full_dof

        # Use finite differentiation if true, otherwise use motion regression
        # algorithm
        self._use_finite_diff = use_finite_diff
        # Time window used for the regression method
        self._regression_window = 0.5
        # If the regression method is used, adjust the time step
        if not self._use_finite_diff:
            self._t_step = self._regression_window / 30

        # Flags to indicate that the interpolation process has started and
        # ended
        self._has_started = False
        self._has_ended = False

        # The parametric variable to use as input for the interpolator
        self._cur_s = 0

        self._init_rot = quaternion_about_axis(0.0, [0, 0, 1])

    def _compute_rot_quat(self, dx, dy, dz):
        if np.isclose(dx, 0) and np.isclose(dy, 0):
            rotq = self._last_rot
        else:
            heading = np.arctan2(dy, dx)
            rotq = quaternion_about_axis(heading, [0, 0, 1])

        if self._is_full_dof:
            rote = quaternion_about_axis(
                -1 * np.arctan2(dz, np.sqrt(dx**2 + dy**2)),
                [0, 1, 0])
            rotq = quaternion_multiply(rotq, rote)

        # Certify that the next quaternion remains in the same half hemisphere
        d_prod = np.dot(self._last_rot, rotq)
        if d_prod < 0:
            rotq *= -1

        return rotq

    def _getPokePose(self, pos, orient):

        print orient
        if numpy.dot(tft.quaternion_matrix(orient)[0:3,2], (0,0,1)) < 0:
            print "fliiping arm pose"
            orient = tft.quaternion_multiply(orient, (1,0,0,0))
        tilt_adjust = tft.quaternion_about_axis(self.tilt_angle[self.arm_using], (0,1,0))
        new_orient = tft.quaternion_multiply(orient, tilt_adjust)
        pos[2] += self.above #push above cm above table 
#DEBUG
        #print new_orient

        return (pos, orient, new_orient)

    def rot_r_wrist(self, pt):
        out_pose = deepcopy(self.curr_state[1])
        q_r = transformations.quaternion_about_axis(-pt.x, (1, 0, 0))  # Hand frame roll (hand roll)
        q_p = transformations.quaternion_about_axis(-pt.y, (0, 1, 0))  # Hand frame pitch (wrist flex)
        q_h = transformations.quaternion_multiply(q_r, q_p)
        q_f = transformations.quaternion_about_axis(-pt.y, (1, 0, 0))  # Forearm frame rot (forearm roll)

        if pt.x or pt.y:
            self.tf.waitForTransform(
                out_pose.header.frame_id, "r_wrist_roll_link", out_pose.header.stamp, rospy.Duration(3.0)
            )
            hand_pose = self.tf.transformPose("r_wrist_roll_link", out_pose)
            q_hand_pose = (
                hand_pose.pose.orientation.x,
                hand_pose.pose.orientation.y,
                hand_pose.pose.orientation.z,
                hand_pose.pose.orientation.w,
            )
            q_h_rot = transformations.quaternion_multiply(q_h, hand_pose.pose.orientation)
            hand_pose.pose.orientation = Quaternion(*q_h_rot)
            out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose)

        if pt.z:
            self.tf.waitForTransform(
                out_pose.header.frame_id, "r_forearm_roll_link", out_pose.header.stamp, rospy.Duration(3.0)
            )
            hand_pose = self.tf.transformPose("r_forearm_roll_link", out_pose)
            q_hand_pose = (
                hand_pose.pose.orientation.x,
                hand_pose.pose.orientation.y,
                hand_pose.pose.orientation.z,
                hand_pose.pose.orientation.w,
            )
            q_f_rot = transformations.quaternion_multiply(q_f, hand_pose.pose.orientation)
            hand_pose.pose.orientation = Quaternion(*q_f_rot)
            out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose)

        wrist_traj = self.build_trajectory(out_pose, arm=1)

 def imu_cb(self, msg):
     q = (msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w)
     
     # N-W-U to E-N-U is simply a rotation of -90 about the Z axis.
     tform = quaternion_about_axis(math.pi/2, (0,0,1))
     enu = quaternion_multiply(q,tform)
     new_msg = PoseStamped()
     new_msg.header = msg.header
     new_msg.pose.position.x = 0
     new_msg.pose.position.y = 0
     new_msg.pose.position.z = 0
     new_msg.pose.orientation.x = enu[0]
     new_msg.pose.orientation.y = enu[1]
     new_msg.pose.orientation.z = enu[2]
     new_msg.pose.orientation.w = enu[3]
     self.publisher.publish(new_msg)

    def append_pose(self, pose, secs, nsecs):
        x = pose['position']['x']
        pose['position']['x'] = -pose['position']['y'] - 4.1 
        pose['position']['y'] = x - 3.75

        oldq = [pose['orientation']['x'], pose['orientation']['y'],
                pose['orientation']['z'], pose['orientation']['w']]
        rotq = quaternion_about_axis(1.57, [0,0,1])
        newq = quaternion_multiply(oldq, rotq)
        pose['orientation']['x'] = newq[0]
        pose['orientation']['y'] = newq[1]
        pose['orientation']['z'] = newq[2]
        pose['orientation']['w'] = newq[3]
        self.pose.append(pose)
        self.secs.append(secs)
        self.nsecs.append(nsecs)