def greatCircleDistance(coord1, coord2, r):
    Φ1 = np.deg2rad(coord1[0])
    Φ2 = np.deg2rad(coord2[0])
    λ1 = np.deg2rad(coord1[1])
    λ2 = np.deg2rad(coord2[1])
    Δσ = np.arccos(s(Φ1) * s(Φ2) + c(Φ1) * c(Φ2) * c(abs(λ2 - λ1)))
    return r * Δσ
Exemple #2
0
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 = bound(sin_theta, 1, -1)
    theta = numpy.arcsin(sin_theta)
    EULER[1] = theta

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

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

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

    return EULER
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
Exemple #4
0
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)]    
    ])    
Exemple #5
0
def projected_distance(dist,ra_center,de_center,ra,de,dtype="rad"):
    '''
    args:
        ra_center,de_center,ra,de: radian unit
    note:
        calculate the projected distance from the center position:
        dist = dist*sin(theta),
        cos_theta = c(de_center)*c(de)*(c(ra-ra_center))+s(de_center)*s(de)
    '''
    t = np.deg2rad if dtype == "deg" else lambda x:x
    cos_theta = c(t(de_center))*c(t(de))*(c(t(ra-ra_center)))+s(t(de_center))*s(t(de))
    return dist*np.sqrt(1-cos_theta*cos_theta)
Exemple #6
0
def projected_angle(ra_center,de_center,ra,de,dtype="rad"):
    '''
    args:
        ra_center,de_center,ra,de: radian unit
    note:
        calculate the projected distance from the center position:
        dist = dist*sin(theta),
        cos_theta = c(de_center)*c(de)*(c(ra-ra_center))+s(de_center)*s(de)
    '''
    t,t_inv = (np.deg2rad,np.rad2deg) if dtype == "deg" else (lambda x:x,lambda x:x)
    cos_theta = c(t(de_center))*c(t(de))*(c(t(ra-ra_center)))+s(t(de_center))*s(t(de))
    return t_inv(np.arccos(cos_theta))
def rot2(nu_az, nu_zen, gen_az, gen_zen):
    new_x = c(nu_az)*c(gen_az)*s(gen_zen) - s(nu_az)*s(gen_az)*s(gen_zen)
    new_y = s(nu_az)*c(gen_az)*s(gen_zen) + c(nu_az)*s(gen_az)*s(gen_zen)
    new_z = c(gen_zen)
    new_az  = np.zeros(len(new_x))
    for i in range(len(new_x)):
        if new_x[i] > 0:
            new_az[i] = np.arctan(new_y[i] / new_x[i]) % (2*np.pi)
        else:
            new_az[i] = np.arctan(new_y[i] / new_x[i]) + np.pi
    new_zen = np.arccos(new_z)
    return new_az, new_zen
Exemple #8
0
    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]
        	         ])
Exemple #9
0
    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]])
Exemple #10
0
def jet_engine(engine, cg, altitude, throttle):
    """returns jet engine forces and moments."""
    rho = Atmosphere(altitude).air_density()
    rho_sl = Atmosphere(0).air_density()
    t_slo = engine['thrust'] * throttle
    phi = deg2rad(engine['thrust_angle'])
    psi = deg2rad(engine['toe_angle'])
    r = array([-engine['station'], engine['buttline'], -engine['waterline']
               ]) + array(cg)
    t = t_slo * rho / rho_sl  # [lbs]
    t = t * array([c(phi) * c(psi), s(psi), -s(phi)])
    m = cross(r, t)
    c_f_m = array([t[0], t[1], t[2], m[0], m[1], m[2]])
    return c_f_m
Exemple #11
0
    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])
Exemple #12
0
def Euler2Quat(euler):
    p = euler[0] / 2.
    t = euler[1] / 2.
    g = euler[2] / 2.

    cp = c(p)
    ct = c(t)
    cg = c(g)
    sp = s(p)
    st = s(t)
    sg = s(g)
    return [
        cp * ct * cg + sp * st * sg, sp * ct * cg - cp * st * sg,
        cp * st * cg + sp * ct * sg, cp * ct * sg - sp * st * cg
    ]
    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])
Exemple #14
0
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 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)
Exemple #16
0
    def feed(self, X):
        X = array(X)
        if len(X) != self._inputs_num:
            raise Exception(
                "vector 'X' must be list of length equal to inputs_num=%d" %
                self._inputs_num)

        return self._feed(c(([1], X)) if self._shift else X)
Exemple #17
0
def main(argv):
    '''Take in 3 joint angles and calculate the end effector position
       for the modified DH parameters through transformation.
       Also find the Jacobian matrix and its determinant for the 6th
       transformation to calculate the angular torque using input forces in XYZ directions.
       Using the torque then find the actual end effector force
    '''
    # Case 1 : DH-Parameters
    dhp = np.mat([[0, 0, 18.5, 0.1381482112], [0, 22.3, 0, 1.54],
                  [-np.pi / 2, 25.3, 0, -0.1841976149],
                  [np.pi / 2, 6.5, -1.5, 0.08186560661], [0, 6, -15.5, 0]])

    # Copy joint angles that were provided as command line arguments
    # into the table of DH parameters and forces into a 1x3 Matrix.
    z0 = np.empty((dhp.shape[0], 4, 4))  # Z-axis matrices for each joint
    (fig, ax, ep) = visualizeArm(np.empty((1, 1, 1)))
    plt.show()
    # for j in range(10):
    #     t0 = np.empty((dhp.shape[0], 4,4))
    #     a1, a2, a3, a4 = inverse_jacobian(dhp[0, -1], dhp[1, -1], dhp[2, -1], dhp[3, -1], 0, -1, 0, 0)
    #     dhp[:, 3] = dhp[:, 3] + (np.matrix([a1, a2, a3, a4, 0])).T
    #     # Compute the forward kinematics of the arm, finding the orientation
    #     # and position of each joint's frame in the base frame (X0, Y0, Z0).
    #     for i in range(dhp.shape[0]):
    #         # Create joint transformation from DH Parameters
    #
    #         t0[i] = np.mat([(c(dhp[i,3]), -s(dhp[i,3]), 0, dhp[i,1]),
    #                         (s(dhp[i,3]) * c(dhp[i,0]), c(dhp[i,3]) * c(dhp[i,0]), -s(dhp[i,0]),-dhp[i,2] * s(dhp[i, 0])),
    #                         (s(dhp[i,3]) * s(dhp[i,0]), c(dhp[i,3]) * s(dhp[i,0]), c(dhp[i,0]), dhp[i,2] * c(dhp[i, 0])),
    #                         (0, 0, 0, 1)])
    #
    #         # Put each transformation in the frame 0
    #         if i != 0: t0[i] = np.matmul(t0[i-1],t0[i])
    #
    #         # Compute Z axis in frame 0 for each joint
    #         z0[i] = np.matmul(t0[i],np.matrix([[1, 0, 0, 0],
    #                            [0, 1, 0, 0],
    #                            [0, 0, 1, 15],
    #                            [0, 0, 0, 1]]))
    #     np.savetxt(sys.stdout.buffer, np.mat(t0[-1,:-1,-1]), fmt="%.5f")
    #     print(a1, a2, a3, a4)
    #     (fig, ax) = visualizeArm(t0, ax=ax, fig=fig)
    #     # print (jacobian(dhp[0,-1],dhp[1,-1],dhp[2,-1],dhp[3,-1],0,0,0,0))
    t0 = np.empty((dhp.shape[0], 4, 4))

    for i in range(dhp.shape[0]):
        # Create joint transformation from DH Parameters

        t0[i] = np.mat([
            (c(dhp[i, 3]), -s(dhp[i, 3]), 0, dhp[i, 1]),
            (s(dhp[i, 3]) * c(dhp[i, 0]), c(dhp[i, 3]) * c(dhp[i, 0]),
             -s(dhp[i, 0]), -dhp[i, 2] * s(dhp[i, 0])),
            (s(dhp[i, 3]) * s(dhp[i, 0]), c(dhp[i, 3]) * s(dhp[i, 0]),
             c(dhp[i, 0]), dhp[i, 2] * c(dhp[i, 0])), (0, 0, 0, 1)
        ])
    step_x(10, dhp, z0, ax, fig, ep)
    print()
    step_y(-2, dhp, z0, ax, fig, ep)
    step_x(-10, dhp, z0, ax, fig, ep)
    step_y(2, dhp, z0, ax, fig, ep)
    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
Exemple #19
0
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 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])
Exemple #21
0
def eci_to_ned(mu, lam):
    """Earth Centered Inertial to North East Down direct cosine matrix."""
    b = array([[c(mu), -s(mu) * s(lam), s(mu) * c(lam)], [0, c(lam),
                                                          s(lam)],
               [-s(mu), -c(mu) * s(lam),
                c(mu) * c(lam)]])
    return b
Exemple #22
0
    def train1(self, X, T, train_rate=None, prints=False):
        X_ = c(([1], X))
        T = array(T)
        n = self.n
        p = self.p

        def print_(*args, **kwargs):
            print(*args, **kwargs) if prints else None

        #print_("Weights BEFORE train:\n")
        #self.print_weights() if prints else None

        result = self.process_input(X, full=True)
        Z_in = result['Z_in']
        Z = result['Z']
        Z_ = c(([1], Z))
        Y_in = result['Y_in']
        Y = result['Y']

        sigma_Y = (T-Y)*sigmoid_deriv(Y_in)
        delta_w = array([sigma_Y * Z_[i] for i in range(0, 1+p)]) *\
                  ((1/Y) if train_rate is None else train_rate)
        print_("\nDelta W:\n", abs(delta_w))

        sigma_in = array([sum(sigma_Y * self.W[i] for i in range(1, p+1))]).T[0]
        sigma_Z = sigma_in * sigmoid_deriv(Z_in)
        delta_v = array([sigma_Z * X_[i] for i in range(0, 1+n)]) *\
                ((1/Z) if train_rate is None else train_rate)

        #print_("Sigma Z:", sigma_Z)
        print_("Delta V:\n", abs(delta_v), "\n")

        self.W = self.W + delta_w
        self.V = self.V + delta_v

        #print_("Weights AFTER train:\n")
        #self.print_weights() if prints else None

        return delta_w
Exemple #23
0
def matTrans(thetArr):
    t1 = thetArr[0]
    t2 = thetArr[1]
    t3 = thetArr[2]
    matRes = np.array([[0, s(t3), c(t3)], [0, c(t3) * c(t2), -s(t3) * c(t2)],
                       [c(t2), s(t2) * s(t3),
                        c(t3) * s(t2)]])
    return matRes
Exemple #24
0
def propeller(engine, cg, v, throttle):
    """returns propeller system forces and moments."""
    # thrust coefficient table
    c_t = array([[0.14, 0.08, 0, -0.07, -0.15, -0.21],
                 [0.16, 0.15, 0.1, 0.02, -0.052, -0.1],
                 [0.18, 0.172, 0.16, 0.12, 0.04, -0.025]])
    pitch_c_t = [15, 25, 35]
    j_c_t = [0, 0.4, 0.8, 1.2, 1.6, 2.0]

    rpm = engine['rpm_max'] * throttle / 60
    j = array([v / (engine['diameter'] * rpm)])
    f = RectBivariateSpline(pitch_c_t, j_c_t, c_t, kx=1)
    c_t_i = f(engine['pitch'], j)
    rho = Atmosphere(0).air_density()
    t = float(rho * (rpm**2) * (engine['diameter']**4) * c_t_i)
    phi = deg2rad(engine['thrust_angle'])
    psi = deg2rad(engine['toe_angle'])
    r = array([-engine['station'], engine['buttline'], -engine['waterline']
               ]) + array(cg)
    t = t * array([c(phi) * c(psi), s(psi), -s(phi)])
    m = cross(r, t)
    c_f_m = array([t[0], t[1], t[2], m[0], m[1], m[2]])
    return c_f_m
Exemple #25
0
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)
                  ]])
Exemple #26
0
    def rotation_from_angle_axix(self, theta, v):
        """
        theta
        v = [vx, vy, vz]

        """
        vx = v.item(0)
        vy = v.item(1)
        vz = v.item(2)

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

        R = np.matrix([[(vx**2)*(1-c(theta))+c(theta), vx*vy*(1-c(theta))-vz*s(theta), vx*vz*(1-c(theta))+vy*s(theta)],
                       [vx*vy*(1-c(theta))+vz*s(theta), (vy**2)*(1-c(theta))+c(theta), vy*vz*(1-c(theta))-vx*s(theta)],
                       [vx*vz*(1-c(theta))-vy*s(theta), vy*vz*(1-c(theta))+vx*s(theta), (vz**2)*(1-c(theta))+c(theta)]])

        return R
Exemple #27
0
	def train1(self, X, d, eps=1e-1, train_rate=None, maxiters=1e4, prints=False):
		if eps <= Perzeptron.train_accuracy_threshold:
			raise Exception("Accuracy parameter eps must be greater then %d" %
							Perzeptron.train_accuracy_threshold)

		def print_(*args, **kwargs):
			if prints:
				print(*args, **kwargs)

		print_("Training parameters:\n",
				"accuracy is ", eps, '\n',
				"training rate is ", train_rate, '\n',
				"Weights before train:\n",
				self._weights,
				sep='')
		print_("\nNow training...\n")
		
		X = c(([1], array(X))) if self._shift else array(X)

		for i in range(0, int(maxiters)):
			y = self._process_input(X)
			delta = (d - y) * (1/y if train_rate is None else train_rate)

			print_("Iteration", i)
			print_("Y_i:")
			print_(y)
			print_("Delta:")
			print_(abs(delta))
			print_("\n")

			if abs(delta) < eps:
				print_("Train stoped after %d iteration" % i)
				print_("Weights after train:\n", self._weights, sep='')
				return delta
			else:
				self._weights = self._weights + X * delta


		print_("Training process reached maxiters=%d number of iterations. Training stoped" % maxiters)
		print_("Weights after train:\n", self._weights, sep='')
		return abs(delta)
Exemple #28
0
    def set_thetas(self, position=(0, 0, 0, 0), wait=False):
        a1, a2, a3, a4 = position
        # print (position)
        self.dhp[:, 3] = (np.mat([a1, a2, a3, a4, 0])).T
        for i in range(self.dhp.shape[0]):
            # Create joint transformation from DH Parameters

            self.t0[i] = np.mat([
                (c(self.dhp[i, 3]), -s(self.dhp[i, 3]), 0, self.dhp[i, 1]),
                (s(self.dhp[i, 3]) * c(self.dhp[i, 0]),
                 c(self.dhp[i, 3]) * c(self.dhp[i, 0]), -s(self.dhp[i, 0]),
                 -self.dhp[i, 2] * s(self.dhp[i, 0])),
                (s(self.dhp[i, 3]) * s(self.dhp[i, 0]),
                 c(self.dhp[i, 3]) * s(self.dhp[i, 0]), c(self.dhp[i, 0]),
                 self.dhp[i, 2] * c(self.dhp[i, 0])), (0, 0, 0, 1)
            ])
            # Put each transformation in the frame 0
            if i != 0: self.t0[i] = np.matmul(self.t0[i - 1], self.t0[i])
        self.visualizeArm()
Exemple #29
0
def step_x(dist, dhp, z0, ax, fig, ep):
    steps = 100
    a1, a2, a3, a4 = dhp[:4, 3]
    step_x = dist / (steps)
    x = 24.509846106016546
    for j in range(steps):
        # a2 +=  -0.65366736003 * np.sin(0.02077772917 * step_x)
        x += step_x
        a2 -= 2 * 0.0006346 * x * step_x
        # a2 += -np.sin(0.00661375661 * np.pi * step_x)
        a1 += (-0.0273428 * step_x) + 2 * 0.0006346 * x * step_x
        # print (a1, 0.0012692 * step_x)
        dhp[:, 3] = (np.mat([a1, a2, a3, a4, 0])).T

        # print(a1,a2,a3,a4,x)
        # print(*get_transformation(a1, a2, a3, a4)[:-1, -1].T)

        t0 = np.empty((dhp.shape[0], 4, 4))

        for i in range(dhp.shape[0]):
            # Create joint transformation from DH Parameters

            t0[i] = np.mat([
                (c(dhp[i, 3]), -s(dhp[i, 3]), 0, dhp[i, 1]),
                (s(dhp[i, 3]) * c(dhp[i, 0]), c(dhp[i, 3]) * c(dhp[i, 0]),
                 -s(dhp[i, 0]), -dhp[i, 2] * s(dhp[i, 0])),
                (s(dhp[i, 3]) * s(dhp[i, 0]), c(dhp[i, 3]) * s(dhp[i, 0]),
                 c(dhp[i, 0]), dhp[i, 2] * c(dhp[i, 0])), (0, 0, 0, 1)
            ])

            # Put each transformation in the frame 0
            if i != 0: t0[i] = np.matmul(t0[i - 1], t0[i])

            # Compute Z axis in frame 0 for each joint
            z0[i] = np.matmul(
                t0[i],
                np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 15],
                           [0, 0, 0, 1]]))

        (fig, ax, ep) = visualizeArm(t0, ax=ax, fig=fig, ep=ep)
Exemple #30
0
def Eul2DCM(ang, axis, deg):
    if deg:
        angs = r(ang)
    else:
        angs = ang
    MatRes = np.eye(3)
    for n, th in enumerate(angs):
        if axis[n] == 1:
            MatInt = np.array([[1, 0, 0], [0, c(th), s(th)],
                               [0, -s(th), c(th)]])
        elif axis[n] == 2:
            MatInt = np.array([[c(th), 0, -s(th)], [0, 1, 0],
                               [s(th), 0, c(th)]])
        else:
            MatInt = np.array([[c(th), s(th), 0], [-s(th), c(th), 0],
                               [0, 0, 1]])
        MatRes = np.dot(MatInt, MatRes)
    return MatRes
Exemple #31
0
def step_y(dist, dhp, z0, ax, fig, ep):
    steps = 100
    a1, a2, a3, a4 = dhp[:4, 3]
    step_x = dist / (steps)

    for j in range(steps):
        a1 += 0.046884 * step_x
        a2 += -1.009377 * 0.046884 * step_x - 0.5627 * 0.046884 * step_x * a1
        # a2 += - 2.854 * np.sin(0.0071 * np.pi * step_x)

        dhp[:, 3] = (np.mat([a1, a2, a3, a4, 0])).T

        # print(a1,a2,a3,a4,x)
        # print(*get_transformation(a1, a2, a3, a4)[:-1, -1].T)

        t0 = np.empty((dhp.shape[0], 4, 4))

        for i in range(dhp.shape[0]):
            # Create joint transformation from DH Parameters

            t0[i] = np.mat([
                (c(dhp[i, 3]), -s(dhp[i, 3]), 0, dhp[i, 1]),
                (s(dhp[i, 3]) * c(dhp[i, 0]), c(dhp[i, 3]) * c(dhp[i, 0]),
                 -s(dhp[i, 0]), -dhp[i, 2] * s(dhp[i, 0])),
                (s(dhp[i, 3]) * s(dhp[i, 0]), c(dhp[i, 3]) * s(dhp[i, 0]),
                 c(dhp[i, 0]), dhp[i, 2] * c(dhp[i, 0])), (0, 0, 0, 1)
            ])

            # Put each transformation in the frame 0
            if i != 0: t0[i] = np.matmul(t0[i - 1], t0[i])

            # Compute Z axis in frame 0 for each joint
            z0[i] = np.matmul(
                t0[i],
                np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 15],
                           [0, 0, 0, 1]]))

        (fig, ax, ep) = visualizeArm(t0, ax=ax, fig=fig, ep=ep)
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
Exemple #33
0
    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  )
Exemple #34
0
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) ) )  )
    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

    #note the "wind" is introduced as an acceleration which imparts exogenous forces in the position control law expressions 



    xddot.append(    (T[-1]/m)  * ( c(psi[-1]) * s(theta[-1]) * c(phi[-1]) + s(psi[-1]) * s(phi[-1]) ) 
                    - (Ax * xdot[-1]/m )  
                    + (-kdx * xdot[-1] - kpx * ( x[-1] - x_des ) + kix * xError )
                    + ith_wind_x * Ax / 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 )  
                    + (-kdy * ydot[-1] - kpy * ( y[-1] - y_des ) + kiy * yError )
                    + ith_wind_y * Ay / m
                )
#    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 ) + ith_wind_z * Az /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) ,  
                               etadot
                              )         
                  )

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

    thetaddot.append(etaddot[1])

    psiddot.append(etaddot[2])

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

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

    phidot.append(  phidot[-1] + phiddot[-1] * h  )
    thetadot.append(  thetadot[-1] + thetaddot[-1] * h  )
    psidot.append(  psidot[-1] + psiddot[-1] * h  )

   
    #------------------------------ integrate new velocity values to obtain position / angle values

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

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

    return [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
           ]
Exemple #35
0
def traj_des_circle(t):

    r = 0.0
    w = 2 * 3.14 / 20.0

    pp = r * w**0 * numpy.array([c(w * t), s(w * t), 0.0])
    vv = r * w**1 * numpy.array([-s(w * t), c(w * t), 0.0])
    aa = r * w**2 * numpy.array([-c(w * t), -s(w * t), 0.0])
    jj = r * w**3 * numpy.array([s(w * t), -c(w * t), 0.0])
    ss = r * w**4 * numpy.array([c(w * t), s(w * t), 0.0])
    uu = r * w**5 * numpy.array([-s(w * t), c(w * t), 0.0])

    pp = pp + numpy.array([0.0, 0.0, 0.01])

    return numpy.concatenate([pp, vv, aa, jj, ss, uu])


# # Desired trajectory for LOAD
# def traj_des(t):

#     if t <= 0.0:

#         r = 1.0
#         w = 2*3.14/20.0

#         pp = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
#         vv = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
#         aa = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
#         jj = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
#         ss = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);

#         pp = pp + numpy.array([0.0,0.0,0.01])

#     else:

#         pp = numpy.array([0.0,0.0,0.01]);
#         vv = numpy.array([0.0,0.0,0.0 ]);
#         aa = numpy.array([0.0,0.0,0.0 ]);
#         jj = numpy.array([0.0,0.0,0.0 ]);
#         ss = numpy.array([0.0,0.0,0.0 ]);

#     return numpy.concatenate([pp,vv,aa,jj,ss])

# # Desired trajectory for LOAD
# def traj_des(t):

#     T = 20.0

#     if t <= T/2:

#         Delta = 5.0
#         w     = 2*3.14/T

#         pp = numpy.array([ -0.5*Delta*(w**0)*(c(w*t) - 1.0),0.0,0.0]);
#         vv = numpy.array([  0.5*Delta*(w**1)*s(w*t)     ,0.0,0.0]);
#         aa = numpy.array([  0.5*Delta*(w**2)*c(w*t)     ,0.0,0.0]);
#         jj = numpy.array([ -0.5*Delta*(w**3)*s(w*t)     ,0.0,0.0]);
#         ss = numpy.array([ -0.5*Delta*(w**4)*c(w*t)     ,0.0,0.0]);

#         pp = pp + numpy.array([-Delta,0.0,0.01])
#         pp = pp + numpy.array([0.0,0.0,0.01])

#     else:

#         pp = numpy.array([0.0,0.0,0.0 ]);
#         vv = numpy.array([0.0,0.0,0.0 ]);
#         aa = numpy.array([0.0,0.0,0.0 ]);
#         jj = numpy.array([0.0,0.0,0.0 ]);
#         ss = numpy.array([0.0,0.0,0.0 ]);

#     return numpy.concatenate([pp,vv,aa,jj,ss])

# # Desired trajectory for LOAD
# def traj_des2(t,pp_real,vv_real,aa_real,jj_real,ss_real):

#     pp_final,vv_final,aa_final,jj_final,ss_final
#     if t <= 0.0:

#         r = 1.0
#         w = 2*3.14/10.0

#         pp = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
#         vv = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
#         aa = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
#         jj = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
#         ss = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);

#         pp = pp + numpy.array([0.0,0.0,0.01])

#     else:

#         pp = numpy.array([0.0,0.0,0.01]);
#         vv = numpy.array([0.0,0.0,0.0 ]);
#         aa = numpy.array([0.0,0.0,0.0 ]);
#         jj = numpy.array([0.0,0.0,0.0 ]);
#         ss = numpy.array([0.0,0.0,0.0 ]);

#     return numpy.concatenate([pp,vv,aa,jj,ss])
def rot_y(tt):
    return np.array([[c(tt),0.0,s(tt)],[0.0,1,0.0],[-s(tt),0.0,c(tt)]])
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)]])
def Ry(tt):
    return numpy.array([[c(tt), 0.0, s(tt)], [0.0, 1, 0.0],
                        [-s(tt), 0.0, c(tt)]])
Exemple #39
0
    def coriolis_matrix(self): 

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

        phd = self.phidot[-1]
        thd = self.thetadot[-1]
        psd = self.psidot[-1]

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

        c11 = 0

        # break up the large elements in to bite size chunks and then add each term ...

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

        c12_term2and3 = (izz-iyy)*psd*(c(ph)**2)*c(th) - ixx*psd*c(th)

        c12 = c12_term1  + c12_term2and3



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



        c21_term1 = (izz-iyy) * ( thd*c(ph)*s(ph) + psd*s(ph)*c(th) )

        c21_term2and3 = (iyy-izz) * psd * (c(ph)**2) * c(th) + ixx * psd * c(th)

        c21 = c21_term1 + c21_term2and3



        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_term1     = (izz-iyy)*( thd*c(ph)*s(ph)*s(th) + phd*(s(ph)**2)*c(th) )

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

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

        c32 = c32_term1 + c32_term2and3 + c32_term4 + c32_term5



        c33_term1 = (iyy-izz) * phd *c(ph)*s(ph)*(c(th)**2)

        c33_term2 = - iyy * thd*(s(ph)**2) * c(th)*s(th)

        c33_term3and4 = - izz*thd*(c(ph)**2)*c(th)*s(th) + ixx*thd*c(th)*s(th)

        c33 = c33_term1 + c33_term2 + c33_term3and4


        return array([
                       [c11,c12,c13],
                       [c21,c22,c23],
                       [c31,c32,c33]
        	              ])
Exemple #40
0
    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] )  )
        self.w3.append( sqrt( self.w3_arg[-1] )  )
        self.w4.append( sqrt( self.w4_arg[-1] )  )
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 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)

    Throttle = numpy.dot(Full_actuation,r3)
    # this decreases the throtle, which will be increased
    Throttle = Throttle*numpy.dot(n,r3)
    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) = r3_des

    # desired roll and pitch angles
    r3_des     = Full_actuation/numpy.linalg.norm(Full_actuation)
    r3_des_rot = Rz(-psi).dot(r3_des)


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

    sin_theta = r3_des_rot[0]/c(phi)
    sin_theta = bound(sin_theta,1,-1)
    cos_theta = r3_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
Exemple #43
0
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
Exemple #44
0
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]
    	              ])
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 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 Rx(tt):
    
    return numpy.array([[1.0,0.0,0.0],[0.0,c(tt),-s(tt)],[0.0,s(tt),c(tt)]])
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 rot_x(tt):
    return np.array([[1.0,0.0,0.0],[0.0,c(tt),-s(tt)],[0.0,s(tt),c(tt)]])
def Rz(tt):
    
    return numpy.array([[c(tt),-s(tt),0.0],[s(tt),c(tt),0.0],[0.0,0.0,1]])
def rot_z(tt):
    return np.array([[c(tt),-s(tt),0.0],[s(tt),c(tt),0.0],[0.0,0.0,1]])
Exemple #54
0
    th = .1
    ps = .1
    phd = 1
    thd = 1
    psd = 1
    m = 1
 
    l = 0.4
    ixx = (1/12.)*0.5 * m * l**2
    iyy = (1/12.)*0.5 * m * l**2
    izz = (1/12.)* m * l**2

    matr = coriolis_matrix(ph,th,phd,thd,psd,ixx,iyy,izz)
    print 'coriolis_matrix = ',matr
    
    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([1,1,1])
    
    LHS = n.dot( J , etadd ) + n.dot( coriolis_matrix(ph,th,phd,thd,psd ,ixx,iyy,izz) , etad )


    print 'LHS = ',LHS

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 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 traj_des_circle(t):


    r = 0.0
    w = 2*3.14/20.0

    pp = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
    vv = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
    aa = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
    jj = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
    ss = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);
    uu = r*w**5*numpy.array([-s(w*t), c(w*t),0.0]);

    pp = pp + numpy.array([0.0,0.0,0.01])


    return numpy.concatenate([pp,vv,aa,jj,ss,uu])


# # Desired trajectory for LOAD
# def traj_des(t):

#     if t <= 0.0:

#         r = 1.0
#         w = 2*3.14/20.0

#         pp = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
#         vv = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
#         aa = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
#         jj = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
#         ss = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);

#         pp = pp + numpy.array([0.0,0.0,0.01])

#     else:

#         pp = numpy.array([0.0,0.0,0.01]);
#         vv = numpy.array([0.0,0.0,0.0 ]);
#         aa = numpy.array([0.0,0.0,0.0 ]);
#         jj = numpy.array([0.0,0.0,0.0 ]);
#         ss = numpy.array([0.0,0.0,0.0 ]);

#     return numpy.concatenate([pp,vv,aa,jj,ss])

# # Desired trajectory for LOAD
# def traj_des(t):

#     T = 20.0
    
#     if t <= T/2:

#         Delta = 5.0
#         w     = 2*3.14/T

#         pp = numpy.array([ -0.5*Delta*(w**0)*(c(w*t) - 1.0),0.0,0.0]);
#         vv = numpy.array([  0.5*Delta*(w**1)*s(w*t)     ,0.0,0.0]);
#         aa = numpy.array([  0.5*Delta*(w**2)*c(w*t)     ,0.0,0.0]);
#         jj = numpy.array([ -0.5*Delta*(w**3)*s(w*t)     ,0.0,0.0]);
#         ss = numpy.array([ -0.5*Delta*(w**4)*c(w*t)     ,0.0,0.0]);

#         pp = pp + numpy.array([-Delta,0.0,0.01])
#         pp = pp + numpy.array([0.0,0.0,0.01])

#     else:

#         pp = numpy.array([0.0,0.0,0.0 ]);
#         vv = numpy.array([0.0,0.0,0.0 ]);
#         aa = numpy.array([0.0,0.0,0.0 ]);
#         jj = numpy.array([0.0,0.0,0.0 ]);
#         ss = numpy.array([0.0,0.0,0.0 ]);

#     return numpy.concatenate([pp,vv,aa,jj,ss])


# # Desired trajectory for LOAD
# def traj_des2(t,pp_real,vv_real,aa_real,jj_real,ss_real):


#     pp_final,vv_final,aa_final,jj_final,ss_final
#     if t <= 0.0:

#         r = 1.0
#         w = 2*3.14/10.0

#         pp = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
#         vv = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
#         aa = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
#         jj = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
#         ss = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);

#         pp = pp + numpy.array([0.0,0.0,0.01])

#     else:

#         pp = numpy.array([0.0,0.0,0.01]);
#         vv = numpy.array([0.0,0.0,0.0 ]);
#         aa = numpy.array([0.0,0.0,0.0 ]);
#         jj = numpy.array([0.0,0.0,0.0 ]);
#         ss = numpy.array([0.0,0.0,0.0 ]);

#     return numpy.concatenate([pp,vv,aa,jj,ss])
    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)