Python numpy.arctan2函数代码示例

    def zoomToAll(self):
        if self.m_nImgs < 1:
            return

        posA=N.array(self.m_imgPosArr)
        sizA=N.array(self.m_imgSizeArr)
        a=N.array([N.minimum.reduce(posA),
                   N.maximum.reduce(posA+sizA),
                   ])
        from .all import U

        MC = N.array([0.5, 0.5]) # mosaic viewer's center (0.5, 0.5)
        a -= MC
        hypot = N.array((N.hypot(a[0][0], a[0][1]),
                         N.hypot(a[1][0], a[1][1])))
        theta = N.array((N.arctan2(a[0][1], a[0][0]),
                         N.arctan2(a[1][1], a[1][0]))) # radians
        phi = theta + U.deg2rad(self.m_rot)
        mimXY = N.array((hypot[0]*N.cos(phi[0]), hypot[0]*N.sin(phi[0])))
        maxXY = N.array((hypot[1]*N.cos(phi[1]), hypot[1]*N.sin(phi[1])))
        a = N.array((mimXY, maxXY))
        a.sort(0)
        if self.m_aspectRatio == -1:
            a = N.array(([a[0][0],-a[1][1]],[a[1][0],-a[0][1]]))

        self.zoomToRect(x0=a[0][0], y0=a[0][1],
                        x1=a[-1][0],y1=a[-1][1])

def align_magnetism(m, vectors):
    """ Rotates a matrix, to align its components with the direction
  of the magnetism """
    if not len(m) == 2 * len(vectors):  # stop if they don't have
        # compatible dimensions
        raise
    # pauli matrices
    from scipy.sparse import csc_matrix, bmat

    sx = csc_matrix([[0.0, 1.0], [1.0, 0.0]])
    sy = csc_matrix([[0.0, -1j], [1j, 0.0]])
    sz = csc_matrix([[1.0, 0.0], [0.0, -1.0]])
    n = len(m) / 2  # number of sites
    R = [[None for i in range(n)] for j in range(n)]  # rotation matrix
    from scipy.linalg import expm  # exponenciate matrix

    for (i, v) in zip(range(n), vectors):  # loop over sites
        vv = np.sqrt(v.dot(v))  # norm of v
        if vv > 0.000001:  # if nonzero scale
            u = v / vv
        else:  # if zero put to zero
            u = np.array([0.0, 0.0, 0.0])
        #    rot = u[0]*sx + u[1]*sy + u[2]*sz
        uxy = np.sqrt(u[0] ** 2 + u[1] ** 2)  # component in xy plane
        phi = np.arctan2(u[1], u[0])
        theta = np.arctan2(uxy, u[2])
        r1 = phi * sz / 2.0  # rotate along z
        r2 = theta * sy / 2.0  # rotate along y
        # a factor 2 is taken out due to 1/2 of S
        rot = expm(1j * r2) * expm(1j * r1)
        R[i][i] = rot  # save term
    R = bmat(R)  # convert to full sparse matrix
    mout = R * csc_matrix(m) * R.H  # rotate matrix
    return mout.todense()  # return dense matrix

def nominal_q(sx, sy, az_in, el_in, az_out, el_out, dz):
    nx, ny = sx + dz * tan(az_in), sy + dz * tan(el_in)
    nd = sqrt( (nx-sx)**2 + (ny-sy)**2 + dz**2 )
    #plt.subplot(131); plot(nx/5,ny/5,'G: direct flight beam center')

    px, py = sx + dz * tan(az_out), sy + dz * tan(el_out)
    pd = sqrt( (px-sx)**2 + (py-sy)**2 + dz**2 )
    #plt.subplot(122); plot(px/5,py/5,'G: direct flight scattered beam')

    if 0:
        # Correction to move px,py into the q normal plane.  This is
        # insignificant for small angle scattering.
        nx_hat, ny_hat, nz_hat = (nx-sx)/nd, (ny-sy)/nd, dz/nd
        px_hat, py_hat, pz_hat = (px-sx)/pd, (py-sy)/pd, dz/pd
        d = nd / (px_hat*nx_hat + py_hat*ny_hat + pz_hat*nz_hat)
        px, py = sx + px_hat*d, sy + py_hat*d
    #plt.subplot(122); plot((px)/5,(py)/5,'G: scattered beam on q normal plane')

    # Note: px,py is the location of the scattered neutron relative to the
    # beam center without gravity in detector coordinates, not the qx,qy vector
    # in inverse coordinates.  This allows us to compute the scattered angle at 
    # the sample, returning theta and phi.
    qd = sqrt((px-nx)**2 + (py-ny)**2)
    theta, phi = arctan2(qd, nd)/2, arctan2(py-ny, px-nx)

    return theta, phi

def main():
    book = xlrd.open_workbook('sense_hat.xlsx')
    sheet = book.sheet_by_index(0)

    g_deg_x =0
    g_deg_y =0
    g_deg_z =0

    a_deg_x =0
    a_deg_y =0
    a_deg_z =0

    g_deg_x_l = []
    a_deg_x_l = []
    num = []
    for row in range(1, sheet.nrows-1):
        data = []
        for col in range(0, sheet.ncols-2):
            data.append(sheet.cell(row,col).value)
        g_deg_x += np.degrees(data[0])*0.1
        g_deg_y += np.degrees(data[1])*0.1
        g_deg_z += np.degrees(data[2])*0.1

        a_deg_x = np.degrees(np.arctan2(data[3], np.sqrt(data[4]**2 + data[5]**2) ))
        a_deg_y = np.degrees(np.arctan2(data[4], np.sqrt(data[3]**2 + data[5]**2) ))
        a_deg_z = np.degrees(np.arctan2(data[5], np.sqrt(data[3]**2 + data[4]**2) ))

        g_deg_x_l.append(g_deg_x)
        a_deg_x_l.append(a_deg_x)
        num.append(row-1)

    plt.plot(num,g_deg_x_l, 'r-', label='Gyro')
    plt.plot(num,a_deg_x_l, 'b-', label='Accel')
    plt.legend()
    plt.show()

    def find_best_new(self):

        x, y, th = np.unravel_index(self.posecells.argmax(), self.posecells.shape)
        mx = self.posecells[x,y,th]

        # get the sums for each axis
        x_sums = np.zeros(PC_DIM_XY)
        y_sums = np.zeros(PC_DIM_XY)
        z_sums = np.zeros(PC_DIM_TH)

        for i in range(x - PC_CELLS_TO_AVG, x + PC_CELLS_TO_AVG + 1):
            for j in range(y - PC_CELLS_TO_AVG, y + PC_CELLS_TO_AVG + 1):
                for k in range(th - PC_CELLS_TO_AVG, th + PC_CELLS_TO_AVG + 1):
                    # Use modulo for wrapping
                    im = i % PC_DIM_XY
                    jm = j % PC_DIM_XY
                    km = k % PC_DIM_TH
                    x_sums[im] += self.posecells[im ,jm, km]
                    y_sums[jm] += self.posecells[im ,jm, km]
                    z_sums[km] += self.posecells[im ,jm, km]

        # now find the (x, y, th) using population vector decoding to handle the wrap around
        sum_x1 = 0
        sum_x2 = 0
        sum_y1 = 0
        sum_y2 = 0
        
        for i in range(PC_DIM_XY):
            sum_x1 += PC_XY_SUM_SIN_LOOKUP[i] * x_sums[i]
            sum_x2 += PC_XY_SUM_COS_LOOKUP[i] * x_sums[i]
            sum_y1 += PC_XY_SUM_SIN_LOOKUP[i] * y_sums[i]
            sum_y2 += PC_XY_SUM_COS_LOOKUP[i] * y_sums[i]

        x = np.arctan2(sum_x1, sum_x2) * PC_DIM_XY / (2.0 * np.pi) - 1.0
        while x < 0:
            x += PC_DIM_XY
        while x > PC_DIM_XY:
            x -= PC_DIM_XY

        y = np.arctan2(sum_y1, sum_y2) * PC_DIM_XY / (2.0 * np.pi) - 1.0
        while y < 0:
            y += PC_DIM_XY
        while x > PC_DIM_XY:
            y -= PC_DIM_XY

        sum_x1 = 0
        sum_x2 = 0
        for i in range(PC_DIM_TH):
            sum_x1 += PC_TH_SUM_SIN_LOOKUP[i] * z_sums[i]
            sum_x2 += PC_TH_SUM_COS_LOOKUP[i] * z_sums[i]

        th = np.arctan2(sum_x1, sum_x2) * PC_DIM_TH / (2.0 * np.pi) - 1.0
        while th < 0:
            th += PC_DIM_TH
        while x > PC_DIM_TH:
            th -= PC_DIM_TH

        self.best_x = x
        self.best_y = y
        self.best_th = th

def get_params(A):
    """This is a copy of spm's spm_imatrix where

    we already know the rotations and translations matrix,
    shears and zooms (as outputs from fsl FLIRT/avscale)

    Let A = the 4x4 rotation and translation matrix

    R = [          c5*c6,           c5*s6, s5]
        [-s4*s5*c6-c4*s6, -s4*s5*s6+c4*c6, s4*c5]
        [-c4*s5*c6+s4*s6, -c4*s5*s6-s4*c6, c4*c5]


    """
    import numpy as np

    def rang(b):
        a = min(max(b, -1), 1)
        return a
    Ry = np.arcsin(A[0,2])
    #Rx = np.arcsin(A[1,2]/np.cos(Ry))
    #Rz = np.arccos(A[0,1]/np.sin(Ry))

    if (abs(Ry)-np.pi/2)**2 < 1e-9:
        Rx = 0
        Rz = np.arctan2(-rang(A[1,0]), rang(-A[2,0]/A[0,2]))
    else:
        c  = np.cos(Ry)
        Rx = np.arctan2(rang(A[1,2]/c), rang(A[2,2]/c))
        Rz = np.arctan2(rang(A[0,1]/c), rang(A[0,0]/c))

    rotations = [Rx, Ry, Rz]
    translations = [A[0,3], A[1,3], A[2,3]]

    return rotations, translations

	def twistUpdate(self):
		if self.goal is None:
			w = 0
			v = 0
		else:
			errX = self.goal.position.x-self.pose.position.x
			errY = self.goal.position.y-self.pose.position.y
			if errX*errX<0.01 and errY*errY<0.01:
				self.goal = None
				w = 0
				v = 0
				errYaw = 0
			else:
				euler = tf.transformations.euler_from_quaternion([self.pose.orientation.x,self.pose.orientation.y,self.pose.orientation.z,self.pose.orientation.w])
				goalYaw = numpy.arctan2(errY,errX)
				errYaw  = goalYaw-euler[2]
				errYaw  = numpy.arctan2(numpy.sin(errYaw),numpy.cos(errYaw))

				w = -self.Kp*errYaw
				if abs(w)>self.w_max:
					w = self.w_max*(abs(w)/w)
				if abs(errYaw)<0.75:
					v = self.v_max / ( abs(w) + 1 )**0.5
				else:
					v = 0
			rospy.logdebug("%s pX:%f pY:%f eX:%f eY:%f eYaw:%f -> w:%f v:%f"
			,	self.nodeName,self.pose.position.x,self.pose.position.y,errX,errY,errYaw,w,v
			)

		self.twist.linear.x = v
		self.twist.angular.z = w

def car2sph(xyz):
    ptsnew = numpy.zeros(xyz.shape)
    xy = xyz[:,0]**2 + xyz[:,1]**2
    ptsnew[:,0] = numpy.sqrt(xy + xyz[:,2]**2)  # r
    ptsnew[:,1] = numpy.arctan2(numpy.sqrt(xy), xyz[:,2]) # tetha
    ptsnew[:,2] = numpy.arctan2(xyz[:,1], xyz[:,0])  # phi
    return ptsnew

 def test_arctan2_invalid(self):
     with pytest.raises(u.UnitsError) as exc:
         np.arctan2(np.array([1, 2, 3]) * u.N, 1. * u.s)
     assert "compatible dimensions" in exc.value.args[0]
     with pytest.raises(u.UnitsError) as exc:
         np.arctan2(np.array([1, 2, 3]) * u.N, 1.)
     assert "dimensionless quantities when other arg" in exc.value.args[0]

def R(r, t):
    xaccel = np.cos(np.arctan2(r[3], r[1])) * (C / (r[1]**2 + r[3]**2)**.5); 
    xvel = r[0];
    yaccel = np.sin(np.arctan2(r[3], r[1])) * C / (r[1]**2 + r[3]**2)**.5;
    yvel = r[2];
    
    return np.array([xaccel, xvel, yaccel, yvel])

def pintadubing(lsec,color = 'b'):
    '''
    This function draws a complete dubing trajectory, departing for a list
    of primary and secundary waypoint such as those generated by secnwayp()
    waypoints and trayectory are draw in blue
    circles are draw in dashed black 
    '''
    
    currutaca = lsec[0][1]
    for i in lsec:
        pintacirculo(i[0]) #Circles involved in Dubbing trajectory
        for j in i:
            pl.plot(j[0],j[1],'o'+color) #waypoint 
         
        pl.plot([currutaca[0],i[1][0]],[currutaca[1],i[1][1]],color)
        
        print i
        #draw the arc covered is radius > 0...            
        if i[0][2]>0:
            print i, 'paso'
            currutaca = i[2] 
            ang = [np.arctan2(i[1][1]-i[0][1],i[1][0]-i[0][0])\
            ,np.arctan2(i[2][1]-i[0][1],i[2][0]-i[0][0])]
            print ang
            ang[1] = ang[1] - i[0][3]*((ang[1]>ang[0])*(i[0][3]>0.)+\
            (ang[1]<ang[0])*(i[0][3]<0.))*2*np.pi
        
            print ang, '\n'        
            pintacirculo(i[0],ang,color,1)
        
    if (lsec[0][0][2] > 0.) and (lsec[-1][0][2]>0.):
        pl.plot([lsec[-1][2][0],lsec[0][1][0]],\
        [lsec[-1][2][1],lsec[0][1][1]],color)

def inv_kin(p, ga, l=[100,100,100]):
    w = np.array([0]*4,dtype=float) #horizontal coordinate
    z = np.array([0]*4,dtype=float) #vertical coordinate

    gripper_angle=ga

    w[3] = np.sqrt ( np.square ( p[2] ) + np.square ( p[0] )) 
    z[3] = p[1]

    w[2] = w[3] - l[2] * np.cos(gripper_angle)
    z[2] = z[3] - l[2] * np.sin(gripper_angle)

    l12 = np.sqrt ( np.square ( w[2] ) + np.square ( z[2] ))
    a12 = np.arctan2(z[2], w[2])

    a = [0]*4
    a[0] = np.arctan2(p[0],p[2])
    a[1] = np.arccos ( ( np.square( l[0] ) + np.square ( l12 ) - np.square ( l[1])) / (2 * l[0] * l12 )) + a12
    w[1] = l[0] * np.cos(a[1])
    z[1] = l[0] * np.sin(a[1])
    a[2] = np.arctan2 ( ( z[2] - z[1] ) , ( w[2] - w[1] ) ) - a[1]
    a[3] = gripper_angle - a[1] - a[2]
  
    a[1]=a[1]-np.pi/2
    return a        

def cv_coord(a,b,c,fr=None,to=None,degr=False):
    if degr:
        degrad = deg2rad
        raddeg = rad2deg
    else:
        degrad = lambda x: x
        raddeg = lambda x: x
    if fr=='sph':
        x=c*cos(degrad(a))*cos(degrad(b))
        y=c*sin(degrad(a))*cos(degrad(b))
        z=c*sin(degrad(b))
    elif fr=='rect':
        x=a
        y=b
        z=c
    elif fr is None:
        raise Exception('You must specify the input coordinate system')
    else:
        raise Exception('Unknown input coordinate system')
    if to=='rect':
        return (x,y,z)
    elif to=='sph':
        ra = raddeg(arctan2(y,x))
        dec = raddeg(arctan2(z,sqrt(x**2+y**2)))
        rad = sqrt(x**2+y**2+z**2)
        return (ra,dec,rad)
    elif to is None:
        raise Exception('You must specify the output coordinate system')
    else:
        raise Exception('Unknown output coordinate system')

def rotzyx_angles(H):
    """Returns the angles such that `H[0:3, 0:3] = R_z(a_z) R_y(a_y) R_x(a_x)`

    :param H: homogeneous matrix
    :type H: 4x4 ndarray
    :rtype: 3-tuple

    **Example:**

    >>> angles = array((3.14/3, 3.14/6, 1))
    >>> (rotzyx_angles(rotzyx(*angles)) == angles).all()
    True

    """
    assert ishomogeneousmatrix(H)
    if abs(H[0,0])<tol and abs(H[1,0])<tol:
        # singularity
        az = 0
        ay = arctan2(-H[2,0], H[0,0])
        ax = arctan2(-H[1,2], H[1,1])
    else:
         az= arctan2(H[1,0],H[0,0])
         sz = sin(az)
         cz = cos(az)
         ay = arctan2(-H[2,0], cz*H[0,0] + sz*H[1,0])
         ax = arctan2(sz*H[0,2] - cz*H[1,2], cz*H[1,1] - sz*H[0,1])
    return (az, ay, ax)

 def _lambet(self):
     """Lower overhead ecliptic longitude and latitude. [deg]"""
     from astropy.coordinates import Angle
     lam = np.arctan2(self._rot.T[1], self._rot.T[0])
     bet = np.arctan2(self._rot.T[2],
                      np.sqrt(self._rot.T[0]**2 + self._rot.T[1]**2))
     return np.degrees(lam), np.degrees(bet)