Python numpy.c函数代码示例

def GetEulerAngles(R):

    #phi   = atan2(R(3,2),R(3,3));
    #theta = asin(-R(3,1));
    #psi   = atan2(R(2,1),R(1,1));

    EULER = numpy.array([0.0,0.0,0.0])

    sin_theta = -R[2,0]
    sin_theta = numpy.clip(sin_theta,1,-1)
    theta     = numpy.arcsin(sin_theta)
    EULER[1]      = theta

    sin_phi   = R[2,1]/c(theta)
    sin_phi   = numpy.clip(sin_phi,1,-1)
    cos_phi   = R[2,2]/c(theta)
    cos_phi   = numpy.clip(cos_phi,1,-1)
    phi       = numpy.arctan2(sin_phi,cos_phi)
    EULER[0]  = phi

    sin_psi   = R[1,0]/c(theta)
    sin_psi   = numpy.clip(sin_psi,1,-1)
    cos_psi   = R[0,0]/c(theta)
    cos_psi   = numpy.clip(cos_psi,1,-1)
    psi       = numpy.arctan2(sin_psi,cos_psi)
    EULER[2]  = psi

    # EULER[0] = numpy.arctan2(numpy.clip(R[2,1],1,-1),numpy.clip(R[2,2],1,-1));
    # EULER[1] = numpy.arcsin(-numpy.clip(R[2,0],1,-1));
    # EULER[2] = numpy.arctan2(numpy.clip(R[1,0],1,-1),numpy.clip(R[0,0],1,-1));    

    return EULER

def J(ph,th):

    global Ixx 
    global Iyy 
    global Izz 

    return array([
    [Ixx        ,                               0  , -Ixx * s(th)                ],
    [0          , Iyy*(c(ph)**2) + Izz * s(ph)**2  , (Iyy-Izz)*c(ph)*s(ph)*c(th) ],
    [-Ixx*s(th) , (Iyy-Izz)*c(ph)*s(ph)*c(th)      , Ixx*(s(th)**2) + Iyy*(s(th)**2)*(c(th)**2) + Izz*(c(ph)**2)*(c(th)**2)]    
    ])    

    def J(self):

        ixx = self.Ixx 
        iyy = self.Iyy
        izz = self.Izz

        th = self.theta[-1]
        ph = self.phi[-1]
   

        j11 = ixx

        j12 = 0

        j13 = -ixx * s(th)

        j21 = 0

        j22 = iyy*(c(ph)**2) + izz * s(ph)**2

        j23 = (iyy-izz)*c(ph)*s(ph)*c(th)

        j31 = -ixx*s(th)

        j32 = (iyy-izz)*c(ph)*s(ph)*c(th)

        j33 = ixx*(s(th)**2) + iyy*(s(th)**2)*(c(th)**2) + izz*(c(ph)**2)*(c(th)**2)          

        return array([
                     [j11, j12, j13],
                     [j21, j22, j23],
                     [j31, j32, j33]
        	         ])

    def traj_des(self,t):

        from numpy import cos as c
        from numpy import sin as s

        r = self.r
        w = self.w
        
        p = r*w**0*numpy.array([ c(w*t),-s(w*t),0.0]);
        v = r*w**1*numpy.array([-s(w*t),-c(w*t),0.0]);
        a = r*w**2*numpy.array([-c(w*t), s(w*t),0.0]);
        j = r*w**3*numpy.array([ s(w*t), c(w*t),0.0]);
        s = r*w**4*numpy.array([ c(w*t),-s(w*t),0.0]);
        
        return numpy.concatenate([p,v,a,j,s])

def euler_desired(U,psi):
    # desired roll and pitch angles
    n_des     = U/numpy.linalg.norm(U)
    n_des_rot = Rz(-psi).dot(n_des)

    sin_phi   = -n_des_rot[1]
    sin_phi   = bound(sin_phi,1,-1)
    phi       = numpy.arcsin(sin_phi)

    sin_theta = n_des_rot[0]/c(phi)
    sin_theta = bound(sin_theta,1,-1)
    cos_theta = n_des_rot[2]/c(phi)
    cos_theta = bound(cos_theta,1,-1)
    pitch     = numpy.arctan2(sin_theta,cos_theta)
    
    return (phi,pitch)

    def _get_untransformed_point(self, time):

        t = time
        r = self.radius
        w = self.angular_velocity
        rot = self.rotation
        off = self.offset

        p = numpy.zeros(4)
        v = numpy.zeros(4)
        a = numpy.zeros(4)
        j = numpy.zeros(4)
        sn = numpy.zeros(4)
        cr = numpy.zeros(4)

        p[0:3] = r*(numpy.array([c(w * t), -s(w * t), 0.0])-numpy.array([1.0, 0.0, 0.0]))
        v[0:3] = r * w**1 * numpy.array([-s(w * t), -c(w * t), 0.0])
        a[0:3] = r * w**2 * numpy.array([-c(w * t), s(w * t), 0.0])
        j[0:3] = r * w**3 * numpy.array([s(w * t), c(w * t), 0.0])
        sn[0:3] = r * w**4 * numpy.array([c(w * t), -s(w * t), 0.0])
        cr[0:3] = r * w**5 * numpy.array([-s(w * t), -c(w * t), 0.0])

        p[3] = -numpy.arctan2(s(w*t),c(w*t))
        v[3] = -w
        a[3] = 0.0
        j[3] = 0.0
        sn[3] = 0.0
        cr[3] = 0.0

        return p, v, a, j, sn, cr

def traj_des(t):
#    p = numpy.array([0.6,0.0,0.0]);
#    v = numpy.array([0.0,0.0,0.0]);
#    a = numpy.array([0.0,0.0,0.0]);
#    j = numpy.array([0.0,0.0,0.0]);
#    s = numpy.array([0.0,0.0,0.0]);

    from numpy import cos as c
    from numpy import sin as s

    r = 0.0
    w = 0.0
    
    p = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
    v = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
    a = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
    j = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
    s = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);
    
    return concatenate([p,v,a,j,s])

    def control_block(self):
       

        # calculate the integral of the error in position for each direction
        
        self.x_integral_error.append( self.x_integral_error[-1] + (self.x_des - self.x[-1])*self.h )
        self.y_integral_error.append( self.y_integral_error[-1] + (self.y_des - self.y[-1])*self.h )
        self.z_integral_error.append( self.z_integral_error[-1] + (self.z_des - self.z[-1])*self.h )


        # computte the comm linear accelerations needed to move the system from present location to the desired location
    
        self.xacc_comm.append( self.kdx * (self.xdot_des - self.xdot[-1]) 
                             + self.kpx * ( self.x_des - self.x[-1] ) 
                             + self.kddx * (self.xdd_des - self.xddot[-1] ) 
                             + self.kix * self.x_integral_error[-1]  )


        self.yacc_comm.append( self.kdy * (self.ydot_des - self.ydot[-1]) 
                                  + self.kpy * ( self.y_des - self.y[-1] ) 
                                  + self.kddy * (self.ydd_des - self.yddot[-1] ) 
                                  + self.kiy * self.y_integral_error[-1]  )


        self.zacc_comm.append( self.kdz * (self.zdot_des - self.zdot[-1]) 
                                  + self.kpz * ( self.z_des - self.z[-1] ) 
                                  + self.kddz * (self.zdd_des - self.zddot[-1] )  
                                  + self.kiz * self.z_integral_error[-1] )


        # need to limit the max linear acceleration that is perscribed by the control law

        # as a meaningful place to start, just use the value '10m/s/s' , compare to g = -9.8 ... 

        max_latt_acc = 5

        max_z_acc = 30    

        if abs(self.xacc_comm[-1]) > max_latt_acc: self.xacc_comm[-1] = max_latt_acc * sign(self.xacc_comm[-1])
        if abs(self.yacc_comm[-1]) > max_latt_acc: self.yacc_comm[-1] = max_latt_acc * sign(self.yacc_comm[-1])            
        if abs(self.zacc_comm[-1]) > max_z_acc: self.zacc_comm[-1] = max_z_acc * sign(self.zacc_comm[-1])

        min_z_acc = 12

        if self.zacc_comm[-1] < min_z_acc: self.zacc_comm[-1] = min_z_acc

        # using the comm linear accellerations, calc  theta_c, phi_c and T_c 

        theta_numerator = (self.xacc_comm[-1] * c(self.psi[-1]) + self.yacc_comm[-1] * s(self.psi[-1]) )

        theta_denominator = float( self.zacc_comm[-1] + self.g )

        if theta_denominator <= 0:
            
            theta_denominator = 0.1           # don't divide by zero !!!
    
        self.theta_comm.append(arctan2( theta_numerator ,  theta_denominator ))

        self.phi_comm.append(arcsin( (self.xacc_comm[-1] * s(self.psi[-1]) - self.yacc_comm[-1] * c(self.psi[-1]) ) / float(sqrt( self.xacc_comm[-1]**2 + 
                                                                                                                     self.yacc_comm[-1]**2 +    
                                                                                                                    (self.zacc_comm[-1] + self.g)**2  )) ))

        self.T_comm.append(self.m * ( self.xacc_comm[-1] * ( s(self.theta[-1])*c(self.psi[-1])*c(self.phi[-1]) + s(self.psi[-1])*s(self.phi[-1]) ) +
                                      self.yacc_comm[-1] * (  s(self.theta[-1])*s(self.psi[-1])*c(self.phi[-1]) - c(self.psi[-1])*s(self.phi[-1]) ) +
                                     (self.zacc_comm[-1] + self.g) * ( c(self.theta[-1])*c(self.phi[-1])  ) 
                                    ))

        if self.T_comm[-1] < 1.0:
            self.T_comm = self.T_comm[:-1] 
            self.T_comm.append(1.0)


        # we will need the derivatives of the comanded angles for the torque control laws.
        self.phidot_comm = (self.phi_comm[-1] - self.phi_comm[-2])/self.h

        self.thetadot_comm = (self.theta_comm[-1] - self.theta_comm[-2])/self.h


        # solve for torques based on theta_c, phi_c and T_c , also psi_des , and previous values of theta, phi, and psi


        tao_phi_comm_temp = ( self.kpphi*(self.phi_comm[-1] - self.phi[-1]) + self.kdphi*(self.phidot_comm - self.phidot[-1]) )*self.Ixx
                
        tao_theta_comm_temp = ( self.kptheta*(self.theta_comm[-1] - self.theta[-1]) + self.kdtheta*(self.thetadot_comm - self.thetadot[-1]) )*self.Iyy

        tao_psi_comm_temp = ( self.kppsi*(self.psi_des - self.psi[-1]) + self.kdpsi*( self.psidot_des - self.psidot[-1] ) )*self.Izz

        self.tao_phi_comm.append(tao_phi_comm_temp ) 
        self.tao_theta_comm.append(tao_theta_comm_temp )
        self.tao_psi_comm.append(tao_psi_comm_temp )

        #--------------------------------solve for motor speeds, eq 24

        self.w1_arg.append( (self.T_comm[-1] / (4.0*self.k)) - ( self.tao_theta_comm[-1] / (2.0*self.k*self.L) )  - ( self.tao_psi_comm[-1] / (4.0*self.b) ) ) 
        self.w2_arg.append( (self.T_comm[-1] / (4.0*self.k)) - ( self.tao_phi_comm[-1]   / (2.0*self.k*self.L) )  + ( self.tao_psi_comm[-1] / (4.0*self.b) ) )
        self.w3_arg.append( (self.T_comm[-1] / (4.0*self.k)) + ( self.tao_theta_comm[-1] / (2.0*self.k*self.L) )  - ( self.tao_psi_comm[-1] / (4.0*self.b) ) )
        self.w4_arg.append( (self.T_comm[-1] / (4.0*self.k)) + ( self.tao_phi_comm[-1]   / (2.0*self.k*self.L) )  + ( self.tao_psi_comm[-1] / (4.0*self.b) ) )

        self.w1.append( sqrt( self.w1_arg[-1] )  )
        self.w2.append( sqrt( self.w2_arg[-1] )  )
#.........这里部分代码省略.........

def Rz(tt):
    
    return numpy.array([[c(tt),-s(tt),0.0],[s(tt),c(tt),0.0],[0.0,0.0,1]])

def Ry(tt):
    
    return numpy.array([[c(tt),0.0,s(tt)],[0.0,1,0.0],[-s(tt),0.0,c(tt)]])

def Rx(tt):
    
    return numpy.array([[1.0,0.0,0.0],[0.0,c(tt),-s(tt)],[0.0,s(tt),c(tt)]])

def system_iteration(x_des,
                     y_des,
                     z_des,
                     h,
                     x,y,z,xdot,ydot,zdot,xddot,yddot,zddot,
                     phi,theta,psi,phidot,thetadot,psidot,phiddot,thetaddot,psiddot,
                     w1,w2,w3,w4,
                     tao_phi,tao_theta,tao_psi, 
                     T,
                     max_total_thrust,
                     min_total_thrust,
                     ith_wind_x,
                     ith_wind_y,
                     ith_wind_z,
                     kpx = 10.0,
                     kpy = 10.0,
                     kpz = 8.0,
                     kdx = 5.0,
                     kdy = 5.0,
                     kdz = 5.0,
                     kix = 1, 
                     kiy = 1,
                     kiz = 1,
                     xError,
                     yError,
                     zError
                     ):


    kpphi   = 8.0
    kptheta = 6.0
    kppsi   = 6.0


    kdphi   = 1.75
    kdtheta = 1.75
    kdpsi   = 1.75


    #--------------------------------nonlinear roll/pitch control law gains
    sPh1 = 3
    sPh2 = 3                       
    sPh3 = 2
    sPh4 = .1
    sTh1 = 3
    sTh2 = 3
    sTh3 = 2
    sTh4 = .1



    #--------------------------------control expressions  eq 23


    # the following two expressions for total thrust and torque about z axis are from 'teppo_luukkenon'

    T_k = (g + kdz*( zdot_des - zdot[-1] ) + kpz*( z_des - z[-1] ) + kiz*zError )* m/float( c(phi[-1]) * c(theta[-1]))

    if T_k <= min_total_thrust:
        T.append( min_total_thrust ) 

    elif T_k >= max_total_thrust:
        T.append( max_total_thrust )
        
    elif T_k > 0 and T_k <= max_total_thrust :    
        T.append( T_k )



    tao_psi.append( ( kdpsi*( psidot_des - psidot[-1] ) + kppsi*( psi_des - psi[-1] ) ) * Izz )


    #equation 61 in 'real-time stablization and tracking'
    tao_phi.append( - sPh1 * (phidot[-1] + sPh2 * (phi[-1] + phidot[-1] + 

                        sPh3 * ( 2 * phi[-1] + phidot[-1] + ( ydot[-1]/g ) + 

                        sPh4 * (phidot[-1] + 3 * phi[-1] + 3 * ( ydot[-1]/(g) ) + y[-1]/(g) )))) * Ixx 

                 )


    #equation 66 in 'real-time stablization and tracking'
    tao_theta.append( - sTh1 * ( thetadot[-1] + sTh2 * ( theta[-1] + thetadot[-1] + 

                    sTh3 * ( 2 * theta[-1] + thetadot[-1] - ( xdot[-1]/(g) ) + 

                    sTh4 * ( thetadot[-1] + 3 * theta[-1] - 3 * ( thetadot[-1]/(g) ) - x[-1]/(g) )))) * Iyy
                )


# original pd contol expressions for roll and pitch

#    tao_phi.append( ( kdphi*( phidot_des - phidot[-1] ) + kpphi*( phi_des - phi[-1] ) ) * Ixx )
#    tao_theta.append( ( kdtheta*( thetadot_des - thetadot[-1] ) + kptheta*( theta_des - theta[-1] ) )*Iyy)


    #--------------------------------solve for motor speeds, eq 24

    w1.append( sqrt( (T[-1] / (4.0*k)) - ( tao_theta[-1] / (2.0*k*L) )  - ( tao_psi[-1] / (4.0*b) ) )  )
#.........这里部分代码省略.........

def rot_x(tt):
    """This function returns the rotation matrix corresponding to a rotation
        of tt radians about the x-axis.
        """
    return numpy.array(
        [[1.0, 0.0, 0.0], [0.0, c(tt), -s(tt)], [0.0, s(tt), c(tt)]])

w1 = []
w2 = []
w3 = []
w4 = []

etaddot = []
#----------------------------------

max_iterations = 5000
h = 0.001

for i in range(max_iterations):
    
    #--------------------------------control expressions  eq 23

    T.append( (g + kdz*( zdot_des - zdot[-1] ) + kpz*( z_des - z[-1] ))* m/float( c(phi[-1]) * c(theta[-1])) )

    tao_phi.append( ( kdphi*( phidot_des - phidot[-1] ) + kpphi*( phi_des - phi[-1] ) ) * Ixx )

    tao_theta.append( ( kdtheta*( thetadot_des - thetadot[-1] ) + kptheta*( theta_des - theta[-1] ) )*Iyy)

    tao_psi.append( ( kdpsi*( psidot_des - psidot[-1] ) + kppsi*( psi_des - psi[-1] ) ) * Izz )

    #--------------------------------solve for motor speeds, eq 24

    w1.append( sqrt( (T[-1] / (4.0*k)) - ( tao_theta[-1] / (2.0*k*L) )  - ( tao_psi[-1] / (4.0*b) ) )  )
    w2.append( sqrt( (T[-1] / (4.0*k)) - ( tao_phi[-1]   / (2.0*k*L) )  + ( tao_psi[-1] / (4.0*b) ) )  )
    w3.append( sqrt( (T[-1] / (4.0*k)) + ( tao_theta[-1] / (2.0*k*L) )  - ( tao_psi[-1] / (4.0*b) ) )  )
    w4.append( sqrt( (T[-1] / (4.0*k)) + ( tao_phi[-1]   / (2.0*k*L) )  + ( tao_psi[-1] / (4.0*b) ) )  )

    #---------------------------------calculate new linear accelerations

def rot_y(tt):
    """This function returns the rotation matrix corresponding to a rotation
        of tt radians about the y-axis.
        """
    return np.array(
        [[c(tt), 0.0, s(tt)], [0.0, 1, 0.0], [-s(tt), 0.0, c(tt)]])