Пример #1
0
        y = unpackd[1]  # y+ -> pitch+
        z = unpackd[2]  # z+ -> thrust+
        #x = 1000*unpackd[0]  # x+ -> roll-
        #y = 1000*unpackd[1]  # y+ -> pitch+
        #z = 1000*unpackd[2]  # z+ -> thrust+
        """ Rotate Optitrack Axis"""
        #y, z = z, y

        """
        Orientation Feedback: quaternion given as (qx, qy, qz, qw)
        """
        qx, qy, qz, qw = unpackd[3], unpackd[4], unpackd[5], unpackd[6]
        q = np.array([qx, qy, qz, qw])
        q = np.linalg.norm(q)
        #print("X:{}, Y:{}, Z:{}".format(x, y, z))
        orientation = euler_from_quaternion([unpackd[3], unpackd[4], unpackd[5], unpackd[6]], axes='sxzy')
        #orientation = [elem*(180/math.pi) for elem in orientation]


        roll = orientation[1]
        pitch = orientation[0]
        yaw = orientation[2]


        """
        Check if vehicle is being tracked by cameras
        """
        detected = unpackd[-1]
        if detected:
            last_detect_ts = time.time()
            delta = unpackd[-2]
Пример #2
0
        # z+ -> thrust+

        x = unpackd[0]
        y = unpackd[1]
        z = unpackd[2]

        # Swap Z and Y axes since Y axis is 'up' w/ Optitrack
        y, z = -z, y

        # Orientation Feedback: quaternion given as (qx, qy, qz, qw)
        qx, qy, qz, qw = unpackd[3], unpackd[4], unpackd[5], unpackd[6]
        q = np.array([qx, qy, qz, qw])
        np.linalg.norm(q)
        #print(q)
        #print("X:{}, Y:{}, Z:{}".format(x, y, z))
        orientation = euler_from_quaternion(q, axes='syxz')
        orientation = [elem*(180/math.pi) for elem in orientation]

        yaw = orientation[0]
        roll = orientation[1]
        pitch = orientation[2]


        """
        Check if body is being tracked by cameras
        """
        detected = unpackd[-1]
        if detected:
            detect_ts = int(round(time.time() * 1000))
            delta = unpackd[-2]
        else:
Пример #3
0
from feedback.transformations import euler_from_quaternion
import time

context = zmq.Context()
optitrack_conn = context.socket(zmq.REP)
optitrack_conn.bind("tcp://204.102.224.3:5000")

while 1:
    packet = optitrack_conn.recv()
    filtered = msgpack.unpackb(packet)
    #print "Got", filtered[3], filtered[4], filtered[5], filtered[6]
    optitrack_conn.send(b'Ack', zmq.NOBLOCK)

    EulerAngles = namedtuple('EulerAngles', 'pitch roll yaw')

    res = euler_from_quaternion([filtered[3], filtered[4], filtered[5], filtered[6]], axes='syxz')
    res = [elem*(180/math.pi) for elem in res]
    print filtered[0], filtered[1], filtered[2], res
    #time.sleep(.5)

    #print res, filtered[-1], filtered[-2]

    """
    Rbn = np.matrix([[q[1]**2 + q[2]**2 - q[3]**2 - q[4]**2,  2*(q[2] * q[3] - q[1] * q[4]), 2*(q[1] * q[3] + q[2] * q[4])],
       [2*(q[2] * q[3] + q[1] * q[4]), q[1]**2 - q[2]**2 + q[3]**2 - q[4]**2,  2*(q[3] * q[4] - q[1] * q[2])],
       [2*(q[2] * q[4] - q[1] * q[3]), 2*(q[1] * q[2] + q[3] * q[4]), q[1]**2 - q[2]**2 - q[3]**2 + q[4]**2]])

    R = np.transpose(Rbn)
    phi   =  math.atan2(R[1, 2], R[2, 2])*(180/math.pi)
    theta = -math.asin(R[0, 2])*(180/math.pi)
    psi   =  -math.atan2(R[0, 1], R[0, 0])*(180/math.pi)