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 _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 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(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 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 Rz(tt):
    
    return numpy.array([[c(tt),-s(tt),0.0],[s(tt),c(tt),0.0],[0.0,0.0,1]])

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 Ry(tt):
    
    return numpy.array([[c(tt),0.0,s(tt)],[0.0,1,0.0],[-s(tt),0.0,c(tt)]])

def controller(states,states_d,parameters):
    
    # mass of vehicles (kg)
    m = parameters.m
    
    # acceleration due to gravity (m/s^2)
    g  = parameters.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
    x  = states[0:3];
    v  = states[3:6];
    # thrust unit vector and its angular velocity
    R  = states[6:15];
    R  = numpy.reshape(R,(3,3))
    n  = R.dot(e3)

    # print(GetEulerAngles(R)*180/3.14)

    
    #--------------------------------------#
    # desired quad trajectory
    xd = states_d[0:3];
    vd = states_d[3:6];
    ad = states_d[6:9];
    
    #--------------------------------------#
    # position error and velocity error
    ep = xd - x
    ev = vd - v
    
    
    u = cmd_di_3D(ep,ev,parameters)

    # -----------------------------------------------------------------------------#
    # vector of commnads to be sent by the controller
    U = numpy.zeros(4)

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

    Throttle = numpy.dot(Full_actuation,n)
    # this decreases the throtle, which will be increased
    Throttle = Throttle*numpy.dot(n,e3)
    U[0]     = Throttle

    #--------------------------------------#
    # current  euler angles
    euler  = GetEulerAngles(R)
    # current phi and theta
    phi   = euler[0];
    theta = euler[1];
    # current psi
    psi   = euler[2];


    #--------------------------------------#
    # Rz(psi)*Ry(theta_des)*Rx(phi_des) = n_des

    # desired roll and pitch angles
    n_des     = Full_actuation/numpy.linalg.norm(Full_actuation)
    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)
    U[1]      = 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)
    U[2]      = pitch

    #--------------------------------------#
    # yaw control: gain
    k_yaw    = parameters.k_yaw; 
    # desired yaw: psi_star
    psi_star = parameters.psi_star; 

    psi_star_dot = 0;
    psi_dot      = psi_star_dot - k_yaw*s(psi - psi_star);
    U[3]         = 1/c(phi)*(c(theta)*psi_dot - s(phi)*U[2]);

    U = Cmd_Converter(U,parameters)

    return U

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

time = 0.0
# cable length
L = 0.5

#--------------------------------------------------------------------------#
print('Goal to the front')

# initial LOAD position
pL0 = numpy.array([-1.0,0.0,0.0])
# initial LOAD velocity
vL0 = numpy.array([0.0,0.0,0.0])
# initial unit vector
n0 = numpy.array([0.0,s(0.0),c(0.0)])
# initial QUAD position
p0 = pL0 + L*n0
# initial angular velocity
w0 = numpy.array([0.0,0.0,0.0])
# 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 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 controller(states,states_d,parameters):
    
    # mass of vehicles (kg)
    m = parameters.m
    
    # acceleration due to gravity (m/s^2)
    g  = parameters.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
    x  = states[0:3];
    v  = states[3:6];
    # thrust unit vector and its angular velocity
    R  = states[6:15];
    R  = numpy.reshape(R,(3,3))
    n  = R.dot(e3)

    # print(GetEulerAngles(R)*180/3.14)

    
    #--------------------------------------#
    # desired quad trajectory
    xd = states_d[0:3];
    vd = states_d[3:6];
    ad = states_d[6:9];
    jd = states_d[9:12];
    sd = states_d[12:15];
    
    #--------------------------------------#
    # position error and velocity error
    ep = xd - x
    ev = vd - v
    
    
    #--------------------------------------#
    G     = g*e3 + ad
    Gdot  = jd
    G2dot = sd
    
    G_all = numpy.concatenate([G,Gdot,G2dot])
    
    
    #--------------------------------------#

    TT,wd,nTd = UniThurstControlComplete(ep,ev,n,G_all,parameters)

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

    U[0]   = TT*m
    RT     = numpy.transpose(R)
    U[1:4] = RT.dot(wd)


    #--------------------------------------#
    # yaw control: gain
    k_yaw    = parameters.k_yaw; 
    # desired yaw: psi_star
    psi_star = parameters.psi_star; 
    # current euler angles
    euler = euler_rad_from_rot(R);
    phi   = euler[0];
    theta = euler[1];
    psi   = euler[2];

    psi_star_dot = 0;
    psi_dot  = psi_star_dot - k_yaw*s(psi - psi_star);
    U[3]    = 1/c(phi)*(c(theta)*psi_dot - s(phi)*U[2]);

    U = Cmd_Converter(U,parameters)

    return U

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

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

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

    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

    xddot.append(   ( T[-1] / m ) * ( c(psi[-1]) * s(theta[-1]) * c(phi[-1]) + s(psi[-1]) * s(phi[-1]) ) - Ax * xdot[-1] / m   )

    yddot.append(   ( T[-1] / m ) * ( s(psi[-1]) * s(theta[-1]) * c(phi[-1]) - c(psi[-1]) * s(phi[-1]) ) - Ay * ydot[-1] / m   )

    zddot.append(   -g + ( T[-1] / m ) * ( c(theta[-1]) * c(phi[-1]) ) - Az * zdot[-1] / m   )
 
    #-------------------------------- calculate new angular accelerations

    tao = array( [tao_phi[-1], tao_theta[-1], tao_psi[-1] ] )  # must build vectors of kth timestep quantities for the matrix math evaluations

    etadot = array( [phidot[-1], thetadot[-1], psidot[-1] ] )

    etaddot =  dot(  
                   inv( J(phi[-1],theta[-1])  ),  
                   tao  -  dot(  
                               coriolis_matrix(phi[-1],theta[-1],phidot[-1],thetadot[-1],psidot[-1] ,Ixx,Iyy,Izz) ,  

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 system_model_block(self):

        # BELOW ARE THE EQUATIONS THAT MODEL THE SYSTEM,
        # FOR THE PURPOSE OF SIMULATION, GIVEN THE MOTOR SPEEDS WE CAN CALCULATE THE STATES OF THE SYSTEM

        self.tao_qr_frame.append( array([ 
                                self.L*self.k*( -self.w2[-1]**2 + self.w4[-1]**2 ) , 
                                self.L*self.k*( -self.w1[-1]**2 + self.w3[-1]**2 ) ,
                                self.b* ( -self.w1[-1]**2 + self.w2[-1]**2 - self.w3[-1]**2 + self.w4[-1]**2 )
                             ]) )

        self.tao_phi.append(self.tao_qr_frame[-1][0])
        self.tao_theta.append(self.tao_qr_frame[-1][1])    
        self.tao_psi.append(self.tao_qr_frame[-1][2])

        self.T.append(self.k*( self.w1[-1]**2 + self.w2[-1]**2 + self.w3[-1]**2 + self.w4[-1]**2 ) )

        # use the previous known angles and the known thrust to calculate the new resulting linear accelerations
        # remember this would be measured ...
        # for the purpose of modeling the measurement error and testing a kalman filter, inject noise here...
        # perhaps every 1000ms substitute an artificial gps measurement (and associated uncertianty) for the double integrated imu value

        self.xddot.append(  (self.T[-1]/self.m)*( c(self.psi[-1])*s(self.theta[-1])*c(self.phi[-1]) 
                            + s(self.psi[-1])*s(self.phi[-1]) ) 
                            - self.Ax * self.xdot[-1] / self.m  )
        
        self.yddot.append(  (self.T[-1]/self.m)*( s(self.psi[-1])*s(self.theta[-1])*c(self.phi[-1]) 
                            - c(self.psi[-1])*s(self.phi[-1]) ) 
                            - self.Ay * self.ydot[-1] / self.m  )        

        self.zddot.append(  self.g + (self.T[-1]/self.m)*( c(self.theta[-1])*c(self.phi[-1]) ) - self.Az * self.zdot[-1] / self.m  )

        # calculate the new angular accelerations based on the known values
        self.etadot.append( array( [self.phidot[-1], self.thetadot[-1], self.psidot[-1] ] ) )

        self.etaddot.append( dot(inv( self.J() ), self.tao_qr_frame[-1]  -  dot(self.coriolis_matrix() , self.etadot[-1]) ) )

        # parse the etaddot vector of the new accelerations into the appropriate time series'
        self.phiddot.append(self.etaddot[-1][0])       

        self.thetaddot.append(self.etaddot[-1][1])

        self.psiddot.append(self.etaddot[-1][2])

        #------------------------------ integrate new acceleration values to obtain velocity values

        self.xdot.append(  self.xdot[-1] + self.xddot[-1] * self.h  )
        self.ydot.append(  self.ydot[-1] + self.yddot[-1] * self.h  )
        self.zdot.append(  self.zdot[-1] + self.zddot[-1] * self.h  )

        self.phidot.append(    self.phidot[-1]   + self.phiddot[-1] * self.h  )
        self.thetadot.append(  self.thetadot[-1] + self.thetaddot[-1] * self.h  )
        self.psidot.append(    self.psidot[-1]   + self.psiddot[-1] * self.h  )
    
        #------------------------------ integrate new velocity values to obtain position / angle values

        self.x.append(  self.x[-1] + self.xdot[-1] * self.h  )
        self.y.append(  self.y[-1] + self.ydot[-1] * self.h  )
        self.z.append(  self.z[-1] + self.zdot[-1] * self.h  )

        self.phi.append(  self.phi[-1] + self.phidot[-1] * self.h  )
        self.theta.append(  self.theta[-1] + self.thetadot[-1] * self.h  )
        self.psi.append( self.psi[-1] + self.psidot[-1] * self.h  )

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)