def flipOrbToNDT (orbPose):
    qOrb = [orbPose.qx, orbPose.qy, orbPose.qz, orbPose.qw]
    orbFlip = trafo.concatenate_matrices(
        trafo.quaternion_matrix(qOrb),
        trafo.rotation_matrix(np.pi/2, (1,0,0)),
        trafo.rotation_matrix(np.pi/2, (0,0,1))
    )
    return trafo.quaternion_from_matrix(orbFlip)

def perpendicular_vector(v):
  """ Finds an arbitrary perpendicular vector to *v*. The idea here is finding a perpendicular vector then rotating abitraryly it around v
  @type v np.array([x,y,z])
  """
  # for two vectors (x, y, z) and (a, b, c) to be perpendicular,
  # the following equation has to be fulfilled
  #  0 = ax + by + cz
  # x = y = z = 0 is not an acceptable solution
  if v[0] == v[1] == v[2] == 0:
    raise ValueError('zero-vector')

  # If one dimension is zero, this can be solved by setting that to
  # non-zero and the others to zero. Example: (4, 2, 0) lies in the
  # x-y-Plane, so (0, 0, 1) is orthogonal to the plane.
  random_rotation = tr.rotation_matrix(np.random.uniform(0, np.pi),v)[:3,:3]
  if v[0] == 0:
    return np.dot(random_rotation,np.array([1, 0, 0]))
  if v[1] == 0:
    return np.dot(random_rotation,np.array([0, 1, 0]))
  if v[2] == 0:
    return np.dot(random_rotation,np.array([0, 0, 1]))

  # set a = b = 1
  # then the equation simplifies to
  #  c = -(x + y)/z
  return np.dot(random_rotation,np.array([1, 1, -1.0 * (v[0] + v[1]) / v[2]]))

    def align_with(self, target):
        """
        Aligns the normal of this triangle to target
        
        @param target - 3 dim np.array
        @return (rotated triangle, rotation matrix)
        """
        target = np.array(target)
        source = np.cross(self.b - self.a, self.c - self.a)
        source /= np.linalg.norm(source)
    
        rotation = np.eye(3)
        
        dot = np.dot(source, target)
        if not np.isnan(dot):
            angle = np.arccos(dot)
            if not np.isnan(angle):
                cross = np.cross(source, target)
                cross_norm = np.linalg.norm(cross)
                if not np.isnan(cross_norm) and not cross_norm < epsilon:
                    cross = cross / cross_norm
                    rotation = tft.rotation_matrix(angle, cross)[:3,:3]

        return (Triangle(np.dot(rotation, self.a),
                        np.dot(rotation, self.b),
                        np.dot(rotation, self.c)),
                rotation)

def fourdof_to_matrix(translation, yaw):
    """
    Convert from a Cartesian translation and yaw to a homogeneous transform.
    """
    T        = transformations.rotation_matrix(yaw, [0,0,1])
    T[0:3,3] = translation
    return T

def GetPlaceOnTSRList( T_target, T_ee, Bw_T, Bw_R, avalues = [0] ):
    T0_w = T_target 
    T0_p = TSRUtil.E2P( T_ee )
    Tw_p = linalg.inv( T0_w ) * T0_p 
    Tw_p[0,3] = 0
    Tw_p[1,3] = 0

    # backoff distance from center of target 
    backoff = -0.03

    # determine if EE is perpendicular or parallel to target
    [x_t, y_t, z_t, t_t] = MatrixToAxes( T_target )
    [x_e, y_e, z_e, t_e] = MatrixToAxes( T_ee )
    t_backoff = array( [0, 0, backoff * ( 1 - fabs( dot( z_t, z_e ) ) )] )
    G_backoff = mat( Rt2G( eye( 3 ), t_backoff) )

    tsrList = []
    for i in range( len( avalues ) ):
        th = avalues[i] * pi / 180
        Rz = mat( tr.rotation_matrix( th, array( [0, 0, 1.] ) ) )
        Tw_p_i = Rz * Tw_p * G_backoff
        T0_p_i = T0_w * Tw_p_i
        tsr_i = TSRUtil.PalmToTSR( T0_p_i, T0_w, Bw_T, Bw_R )

        tsrList.append( copy.deepcopy( tsr_i ) )

    return tsrList

 def transformation_matrix(rot_angle, rot_dir, trans):
     # print "rot_angle: ", rot_angle
     # print "rot_dir: ", rot_dir
     # print "trans: ", trans
     return tfs.concatenate_matrices(
         tfs.rotation_matrix(rot_angle, rot_dir),
         tfs.translation_matrix(trans))

def GetManipGraspList( objectName, objectPose, tablePose, actionName ):
    loginfo( "Generating %s grasp for %s" %( actionName, objectName ) )
    graspList = []

    grasp = graspit_msgs.msg.Grasp()
    T0_o = PoseToTransform( objectPose )
    To_g_list = []
    if "darpadrill" in objectName:
        # Rx1 has 2 fingers along +ve y-axis
        # Rx2 has 2 fingers along -ve y-axis
        Rx1 = tr.rotation_matrix( -pi/2, array( [0, 1., 0] ) )
        Rx2 = tr.rotation_matrix( pi/2, array( [0, 1., 0] ) )
        for th in arange( 15, 75, 15) * pi/180:
            Rz1 = tr.rotation_matrix( th, array( [0, 0, 1.] ) )
            To_g = mat( Rz1 ) * mat( Rx1 )
            To_g_list.append( copy.deepcopy( To_g ) )
            Rz2 = tr.rotation_matrix( pi - th, array( [0, 0, 1.] ) )
            To_g = mat( Rz2 ) * mat( Rx2 )
            To_g_list.append( copy.deepcopy( To_g ) )
        
        G_fin = mat( Rt2G( eye( 3 ), array( [0.,0., -0.07] ) ) )
        G_pre = mat( Rt2G( eye( 3 ), array( [0.,0., -0.14] ) ) )
        G_tra = mat( Rt2G( eye( 3 ), array( [0.,0.,  0.00] ) ) )
        for To_g in To_g_list:
            grasp.pre_grasp_pose = TransformToPose( G_tra *T0_o * To_g * G_pre )
            grasp.final_grasp_pose = TransformToPose( T0_o * To_g * G_fin )
            grasp.epsilon_quality = 0.1
            grasp.volume_quality = 0.1
            graspList.append( copy.deepcopy( grasp ) )
    else:
        success = 0
        reason = "No trigger grasps for object %s" % objectName
        return success, graspList, reason

    loginfo( "Trigger grasps: %d grasps" %( len( graspList ) ) )

    success = 1
    reason = "Done"
    return success, graspList, reason

 def get_camera_position(self, marker):
     """ Outputs the position of the camera in the global coordinates """
     euler_angles = euler_from_quaternion((marker.pose.pose.orientation.x,
                                           marker.pose.pose.orientation.y,
                                           marker.pose.pose.orientation.z,
                                           marker.pose.pose.orientation.w))
     translation = np.array([marker.pose.pose.position.y,
                             -marker.pose.pose.position.x,
                             0,
                             1.0])
     translation_rotated = rotation_matrix(self.yaw-euler_angles[2], [0,0,1]).dot(translation)
     xy_yaw = (translation_rotated[0]+self.position[0],translation_rotated[1]+self.position[1],self.yaw-euler_angles[2])
     return xy_yaw

def rotation_matrix_from_axes(newaxis, oldaxis=Z_AXIS):
  """
  Return the rotation matrix that aligns two vectors.
  """
  oldaxis = tr.unit_vector(oldaxis)
  newaxis = tr.unit_vector(newaxis)
  c = np.dot(oldaxis, newaxis)
  angle = np.arccos(c)
  if np.isclose(c, -1.0) or np.allclose(newaxis, oldaxis):
    v = perpendicular_vector(newaxis)
  else:
    v = np.cross(oldaxis, newaxis)
  return tr.rotation_matrix(angle, v)

def fk(robot, lr, joint_values):
    pose_mat = robot.GetLink(start_link).GetTransform()
    

    R = np.eye(4)
    for i, j in enumerate(joint_values):
        rot = tft.rotation_matrix(j, arm_joint_axes[i])
        trans = arm_link_trans[i]
        R[:3,:3] = rot[:3,:3]
        R[:3,3] = trans
        pose_mat = np.dot(pose_mat, R)
        
    return pose_mat

	def calENU(self):
		if(self.body.isNone() or self.headingAngle is None):
			return False
		else:
			br = self.body.getRotationAroundZ()
			
			q = rotation_matrix(br+self.headingAngle, (0,0,1))
			
			t = translation_from_matrix(self.body.getMatrix())
			t = translation_matrix(t)
			
			self.enu.setMatrix(t).transformByMatrix(q)
			return True
#eoc

    def callback(self, data):
        # rospy.loginfo('callback called')
        
        # Set parameters
        # TODO: get the parameters from the gps transform!!!
        xoff = -0.45
        yoff = 0.0
        std = .03
        rot = math.pi/2

        # Rotate the base pose
        origin, zaxis = (0, 0, 0), (0, 0, 1) 
        Rz = rotation_matrix(rot, zaxis, origin)
        pt = numpy.matrix((data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z, 0),dtype=numpy.float64).T
        xy = Rz*pt
        
        # Add the angle rotation
        oldAng = data.pose.pose.orientation
        ang = euler_from_quaternion([ oldAng.x, oldAng.y, oldAng.z, oldAng.w ])
        ang = ang[0], ang[1], ang[2] + rot
        
        # Offset the GPS reading by the lever arm offset
        x = xy[0] + xoff*math.cos(ang[2]) - yoff*math.sin(ang[2])
        y = xy[1] + xoff*math.sin(ang[2]) + yoff*math.cos(ang[2])
        #x = xy[0] + xoff*math.cos(ang[2])
        #y = xy[1] + yoff*math.sin(ang[2])
        gps_x = x + random.gauss(0,std)
        gps_y = y + random.gauss(0,std)
        print "testing"
        print xy
        print x,y
        print gps_x, gps_y    

        # Populate the angle
        # Note... GPS doesnt really return a heading!! So we cant use to localize... rememmmmberrrrr
        # gps_msg.pose.pose.orientation = Quaternion(*quaternion_from_euler(*ang))
        self.gps_msg.pose.orientation = Quaternion(*quaternion_from_euler(*ang))
        
        # Populate x,y
        self.gps_msg.header.stamp = data.header.stamp
        self.gps_msg.header.frame_id = "map_gps"
        #self.gps_msg.pose.pose.position.x = gps_x
        #self.gps_msg.pose.pose.position.y = gps_y
        self.gps_msg.pose.position.x = gps_x
        self.gps_msg.pose.position.y = gps_y

        # Publish
        self.pub.publish(self.gps_msg)
        self.pubStat.publish(self.gps_stat)

    def get_camera_position(self, marker):
        """ Outputs the position of the camera in the global coordinates """
        translation, rotation = TransformHelpers.convert_pose_inverse_transform(marker.pose.pose)
        euler_angles = euler_from_quaternion(rotation)
        # correct for coordinate system convention changes between camera and world
        # Also, the Neato can't fly!
        translation = np.array([-translation[1], translation[0], 0, 1])
        # correct for the possibility that the marker is rotated relative to the world coordinate frame
        rot_mat = rotation_matrix(self.yaw, [0,0,1])
        translation_rotated = rot_mat.dot(translation)

        xy_yaw = (translation_rotated[0]+self.position[0],
                  translation_rotated[1]+self.position[1],
                  self.yaw+euler_angles[2])
        return xy_yaw

	def convert_pose_inverse_transform(pose):
		translation = np.zeros((4,1))
		translation[0] = -pose.position.x
		translation[1] = -pose.position.y
		translation[2] = -pose.position.z
		translation[3] = 1.0

		rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
		euler_angle = euler_from_quaternion(rotation)
		rotation = np.transpose(rotation_matrix(euler_angle[2], [0,0,1]))		# the angle is a yaw
		transformed_translation = rotation.dot(translation)

		translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2])
		rotation = quaternion_from_matrix(rotation)
		return (translation, rotation)

    def convert_pose_inverse_transform(pose):
        """ Helper method to invert a transform (this is built into the tf C++ classes, but ommitted from Python) """
        translation = np.zeros((4,1))
        translation[0] = -pose.position.x
        translation[1] = -pose.position.y
        translation[2] = -pose.position.z
        translation[3] = 1.0

        rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
        euler_angle = euler_from_quaternion(rotation)
        rotation = np.transpose(rotation_matrix(euler_angle[2], [0,0,1]))       # the angle is a yaw
        transformed_translation = rotation.dot(translation)

        translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2])
        rotation = quaternion_from_matrix(rotation)
        return (translation, rotation)

def addGeometryToEnvironment(endpoint1, endpoint2):
    #TODO: How do we add in the doodler?
    radius = 0.0005
    with env:
        prism = orpy.RaveCreateKinBody(env, '')
        prism.SetName(str(np.random.rand())) #give it a random name - we'll never reference it
        points_diff = (endpoint2 - endpoint1)[:-1, 3] #difference vector
        offset = endpoint1[:-1, 3]
        norm = np.linalg.norm(points_diff, 2)
        #prism.InitFromBoxes(np.array([[0., 0., 0., radius*2., radius*2., norm]]), True)
        prism.InitFromBoxes(np.array([[-radius, -radius, 0., radius, radius, norm/2.]]), True)
        z = np.array([0., 0., norm])
        cross = np.cross(z, points_diff)
        angle = np.arccos(np.dot(z, points_diff) / (norm * norm) )
        tr_matrix = tr.rotation_matrix(angle, cross)
        tr_matrix[:-1, 3] = (endpoint1[:-1, 3] + endpoint2[:-1, 3])/2.
        env.Add(prism, True)
        prism.SetTransform(tr_matrix)

    def transformWorldToPixels(self, aSpriteCordinates, cameraLocation, cameraAngle):
        #initialize the array which has elements of coins
        #initialize the camera coordinate points for each coin image
        aSpritePixels = []

        for aCoord in aSpriteCordinates:
            #rotational matrix, rotate by self.theta, along the z-axis
            rotat = rotation_matrix(cameraAngle,[0,0,1])
            #translation matrix which maps the location of a robot(in world) to camera coordinate origin
            trans = np.matrix([[1,0,0,-(cameraLocation[0])],[0,1,0,-(cameraLocation[1])],[0,0,1,-(cameraLocation[2])],[0,0,0,1]])
            #this calculates the points of the aCoin in camera coordinate system
            new_coord = np.dot(rotat, np.dot(trans, aCoord))
            #calls the pixelate function which maps the camera coordinate points into 2D screen
            aSpritePixels.append(self.pixelate(new_coord[:3]))
        #append to the array which takes an array for each coin
        return aSpritePixels


        """

 def fk(self, joint_values):
     self.handles = list()
     pose_mat = self.origin
     
     R = np.eye(4)
     for i, j in enumerate(joint_values[:]):
         rot = tft.rotation_matrix(j, self.arm_joint_axes[i])
         trans = self.arm_link_trans[i]
         R[:3,:3] = rot[:3,:3]
         R[:3,3] = trans
         print('{0}: {1}\n'.format(i,R))
         pose_mat = np.dot(pose_mat, R)
         
         self.handles += utils.plot_transform(self.env, pose_mat, .3)
       
     pose_mat = np.dot(pose_mat, self.gripper_tool_to_sensor)
     self.handles += utils.plot_transform(self.env, pose_mat, .3)
         
     return tfx.pose(pose_mat)

def rotation_matrix_from_axes(newaxis, oldaxis=Z_AXIS):
  """
  Returns the rotation matrix that aligns two vectors.
  @type  newaxis: np.array
  @param newaxis: The goal axis
  @type  oldaxis: np.array
  @param oldaxis: The initial axis
  @rtype: array, shape (3,3)
  @return: The resulting rotation matrix that aligns the old to the new axis.
  """
  oldaxis = tr.unit_vector(oldaxis)
  newaxis = tr.unit_vector(newaxis)
  c = np.dot(oldaxis, newaxis)
  angle = np.arccos(c)
  if np.isclose(c, -1.0) or np.allclose(newaxis, oldaxis):
    v = perpendicular_vector(newaxis)
  else:
    v = np.cross(oldaxis, newaxis)
  return tr.rotation_matrix(angle, v)

def get_rotation_matrix(angle,axis):
  """
  This function returns a rot matrix (The new frame has the x axis aligned with the GIVEN axis)
  @type angle  : float  
  @param angle : radian
  @type axis   : array([x,y,z])
  @param axis  : new x-axis (normalized)
  @type rot_matrix: 3x3 matrix
  @param rot_matrix: rotation matrix
  """
  old_x = np.array([1,0,0])
  v = np.cross(old_x, axis)
  I = np.eye(3)
  K = skew(v)
  c = np.dot(old_x, axis)
  s = np.linalg.norm(v)
  R = I + s*K + (1-c)*np.linalg.matrix_power(K,2)
  R_angle =  tr.rotation_matrix(angle, old_x)
  rot_matrix = np.dot(R, R_angle[:3,:3])
  return rot_matrix