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)]])

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

#--------------------------------------------------------------------------#

time = 0.4
# cable length
L = 0.5

# initial LOAD position
pL0 = numpy.array([-1.4,3.4,5.0])
# initial LOAD velocity
vL0 = numpy.array([-4.0,-2.0,3.0])
# initial unit vector
n0 = numpy.array([0.0,s(0.3),c(0.3)])
# initial QUAD position
p0 = pL0 + L*n0
# initial angular velocity
w0 = numpy.array([0.1,0.2,-0.1])
# initial quad velocity
v0 = vL0 + L*dot(skew(w0),n0)


# collecting all initial states
states0  = concatenate([pL0,vL0,p0,v0])
statesd0 = traj_des(time)


Controller = Load_Transport_Controller()
out = Controller.output(states0,statesd0)

def controller(states,states_load,states_d,parameters):
    
    # mass of vehicles (kg)
    m = parameters.m
    ml = .136
    # is there some onboard thrust control based on the vertical acceleration?
    ml = .01

    # acceleration due to gravity (m/s^2)
    g  = parameters.g
    rospy.logwarn('g: '+str(g))

    # Angular velocities multiplication factor
    # Dw  = parameters.Dw
    
    # third canonical basis vector
    e3 = numpy.array([0.0,0.0,1.0])
    
    #--------------------------------------#
    # transported mass: position and velocity
    p  = states[0:3];
    v  = states[3:6];
    # thrust unit vector and its angular velocity
    R  = states[6:15];
    R  = numpy.reshape(R,(3,3))
    r3  = R.dot(e3)

    # load
    pl = states_load[0:3]
    vl = states_load[3:6]

    Lmeas = norm(p-pl)
    rospy.logwarn('rope length: '+str(Lmeas))
    L = 0.66

    #rospy.logwarn('param Throttle_neutral: '+str(parameters.Throttle_neutral))

    rl = (p-pl)/Lmeas
    omegal = skew(rl).dot(v-vl)/Lmeas
    omegal = zeros(3)

    
    #--------------------------------------#
    # desired quad trajectory
    pd = states_d[0:3] - L*e3;
    vd = states_d[3:6];
    ad = states_d[6:9];
    jd = states_d[9:12];
    sd = states_d[12:15];

    # rospy.logwarn(numpy.concatenate((v,vl,vd)))
    
    #--------------------------------------#
    # position error and velocity error
    ep = pl - pd
    ev = vl - vd

    rospy.logwarn('ep: '+str(ep))

    #--------------------------------------#
    gstar = g*e3 + ad
    d_gstar = jd
    dd_gstar = sd

    #rospy.logwarn('rl: '+str(rl))
    #rospy.logwarn('omegal: '+str(omegal))
    #rospy.logwarn('ev: '+str(ev))

# temp override
    #rl = array([0.,0.,1.])
    #omegal = zeros(3)
    #L = 1
    
    #--------------------------------------#

    # rospy.logwarn('ep='+str(ep)+' ev='+str(ev)+' rl='+str(rl)+' omegal='+str(omegal))

    Tl, tau, _, _ = backstep_ctrl(ep, ev, rl, omegal, gstar, d_gstar, dd_gstar)
    rospy.logwarn('g: '+str(g))

    U = (eye(3)-outer(rl,rl)).dot(tau*m*L)+rl*(
        # -m/L*inner(v-vl,v-vl)
        +Tl*(m+ml))
    U = R.T.dot(U)

    n = rl

    # -----------------------------------------------------------------------------#
    # STABILIZE MODE:APM COPTER
    # The throttle sent to the motors is automatically adjusted based on the tilt angle
    # of the vehicle (i.e increased as the vehicle tilts over more) to reduce the 
    # compensation the pilot must fo as the vehicles attitude changes
    # -----------------------------------------------------------------------------#
    # Full_actuation = m*(ad + u + g*e3 + self.d_est)
    Full_actuation = U

    # -----------------------------------------------------------------------------#

    U = numpy.zeros(4)

#.........这里部分代码省略.........

def Grad(w1,w2,w3,w4, th,ph,ps, phd,thd,psd, phdd,thdd,psdd, xdd,ydd,zdd):

    #----------------------------------------------------------------------------------physical constants
    k = 10**-6 #
    m = 2
    l = 0.4    
    g = - 9.81    
    b = 10**-7
   
    beta = (1/12.0)*m*l**2    

    ixx = 0.5*beta
    iyy = 0.5*beta
    izz = beta    

    J = n.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)]    
    ])    
    
    #eta = n.array([ph, th, ps])
    etad = n.array([phd, thd, psd])
    etadd = n.array([phdd,thdd,psdd])
    
    LHS = n.dot( J , etadd ) + n.dot( coriolis_matrix(ph,th,phd,thd,psd ,ixx,iyy,izz) , etad )


    lhs_phi   = LHS[0]
    lhs_theta = LHS[1]
    lhs_psi   = LHS[2] 


    return [
    
    #-----------------------------dw1
    sum([

       2 * (l * k * ( -w2**2 + w4**2 + w1**2 - w3**2 ) - lhs_phi + lhs_theta) * l * k * 2 * w1,

       2 * (b*(w1**2 + w2**2 + w3**2 + w4**2) - lhs_psi) * b * 2 * w1,

       2 * (-k * (w1**2 + w2**2 + w3**2 + w4**2) * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) - m * xdd ) * -k * (c(ps) * s(th) * c(ph) + s(ps) * s(ph))  * 2 * w1,

       2 * (( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * (w1**2 + w2**2 + w3**2 + w4**2)  - m * ( ydd - zdd + g )) * ( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) )  * 2 * w1
    
       ]),
    
    #-----------------------------dw2
    sum([

       2 * (l * k * ( -w2**2 + w4**2 + w1**2 - w3**2 ) - lhs_phi + lhs_theta) * l * k * 2 * -w2,

       2 * (b*(w1**2 + w2**2 + w3**2 + w4**2) - lhs_psi) * b * 2 * w2,

       2 * (-k * (w1**2 + w2**2 + w3**2 + w4**2) * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) - m * xdd ) * -k * (c(ps) * s(th) * c(ph) + s(ps) * s(ph))  * 2 * w2,

	  2 * (( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * (w1**2 + w2**2 + w3**2 + w4**2)  - m * ( ydd - zdd + g )) * ( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) )  * 2 * w2
       
       ] ),
    
    #-----------------------------dw3
    sum( [

       2 * (l * k * ( -w2**2 + w4**2 + w1**2 - w3**2 ) - lhs_phi + lhs_theta) * l * k * 2 * -w3,

       2 * (b*(w1**2 + w2**2 + w3**2 + w4**2) - lhs_psi) * b * 2 * w3,

       2 * (-k * (w1**2 + w2**2 + w3**2 + w4**2) * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) - m * xdd ) * -k * (c(ps) * s(th) * c(ph) + s(ps) * s(ph))  * 2 * w3,

	  2 * (( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * (w1**2 + w2**2 + w3**2 + w4**2)  - m * ( ydd - zdd + g )) * ( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) )  * 2 * w3
       
       ] ),

    #-----------------------------dw4
    sum([

       2 * (l * k * ( -w2**2 + w4**2 + w1**2 - w3**2 ) - lhs_phi + lhs_theta) * l * k * 2 * w4,

       2 * (b*(w1**2 + w2**2 + w3**2 + w4**2) - lhs_psi) * b * 2 * w4,

       2 * (-k * (w1**2 + w2**2 + w3**2 + w4**2) * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) - m * xdd ) * -k * (c(ps) * s(th) * c(ph) + s(ps) * s(ph))  * 2 * w4,

	 2 * (( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * (w1**2 + w2**2 + w3**2 + w4**2)  - m * ( ydd - zdd + g )) * ( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) )  * 2 * w4

       ])

    ]#close return list

def gravity_function(t):
	g0 = numpy.array([ c(t), s(t),9.0])
	g1 = numpy.array([-s(t), c(t),0.0])
	g2 = numpy.array([-c(t),-s(t),0.0])
	return numpy.concatenate([g0,g1,g2])

def coriolis_matrix(ph,th,phd,thd,psd ,ixx,iyy,izz): 
    
    c11 = 0

    c12 = (iyy-izz) * ( thd*c(ph)*s(ph) + psd*c(th)*s(ph)**2 )  + (izz-iyy)*psd*(c(ph)**2)*c(th) - ixx*psd*c(th)

    c13 = (izz-iyy) * psd * c(ph) * s(ph) * c(th)**2

    c21 = (izz-iyy) * ( thd*c(ph)*s(ph) + psd*s(ph)*c(th) ) + (iyy-izz) * psd * (c(ph)**2) * c(th) + ixx * psd * c(th)

    c22 = (izz-iyy)*phd*c(ph)*s(ph)

    c23 = -ixx*psd*s(th)*c(th) + iyy*psd*(s(ph)**2)*s(th)*c(th)

    c31 = (iyy-izz)*phd*(c(th)**2)*s(ph)*c(ph) - ixx*thd*c(th)

    c32 = (izz-iyy)*( thd*c(ph)*s(ph)*s(th) + phd*(s(ph)**2)*c(th) ) + (iyy-izz)*phd*(c(ph)**2)*c(th) + ixx*psd*s(th)*c(th) - iyy*psd*(s(ph)**2)*s(th)*c(th) - izz*psd*(c(ph)**2)*s(th)*c(th)

    c33 = (iyy-izz) * phd *c(ph)*s(ph)*(c(th)**2) - iyy * thd*(s(ph)**2) * c(th)*s(th) - izz*thd*(c(ph)**2)*c(th)*s(th) + ixx*thd*c(th)*s(th)

    return n.array([
                   [c11,c12,c13],
                   [c21,c22,c23],
                   [c31,c32,c33]
    	              ])