def get_pos_command(self):
        ts  = ovr.getTrackingState(self.session, ovr.getTimeInSeconds(), True)
        if ts.StatusFlags & (ovr.Status_OrientationTracked | ovr.Status_PositionTracked):
            pose = ts.HeadPose
        
        # Get queternions
        q0 = pose.ThePose.Orientation.w;
        q1 = pose.ThePose.Orientation.x;
        q2 = pose.ThePose.Orientation.y;
        q3 = pose.ThePose.Orientation.z;

        # Calculate Euler angles
        x = int(1500 + 637*math.asin(2*(q0*q1 - q3*q2)))    #pitch
        y = int(1450 - 637*math.atan2(2*(q0*q2 - q1*q3),1-2*(q2*q2+q1*q1))) #yaw
        z = int(1500 + 637*math.asin(2*(q0*q3 + q1*q2)))    #roll
        
        pos_command = ""
        # Anti-shaking
        if abs(self.xtemp-x)>1 or abs(self.ytemp-y)>1 or abs(self.ztemp-z)>1:
            pos_command = (str(x) + "," + str(y) + ","+str(z)+'\n')
            sys.stdout.flush()
            self.xtemp = x
            self.ytemp = y
            self.ztemp = z
        return pos_command
Exemple #2
0
def stream_loop(session, tx, dryrun=False):
    '''
    Collect head orientation data from the Oculus Rift and stream it to the
    microcontroller, forever.
    
    Parameters
    ----------
    session : pointer to HmdDesc
        Interface for the head-moounted device
    tx : tx.Transmitter
        Interface for serial port
    dryrun : bool
        If True, runs the test mode of the loop where there is no MCU
        communication
    '''
    
    print_after = 10
    count = 0
    print("Starting stream loop")
    yvel = 0
    xvel = 0
    while True:
        # Query the HMD for the current tracking state.
        ts  = ovr.getTrackingState(session, ovr.getTimeInSeconds(), True)
        if ts.StatusFlags & (ovr.Status_OrientationTracked | ovr.Status_PositionTracked):
            pose = ts.HeadPose
            angles = np.array(pose.ThePose.Orientation.getEulerAngles())* 180 / np.pi
            pitch = np.round(angles[0] * -1 + 90)
            yaw = np.round(angles[1] + 90)
            
            # Don't allow values outside the range of the servos
            pitch = clamp(pitch, 0, 180)
            yaw = clamp(yaw, 0, 180)
            
            # Get data from gamepad thread
            if lock.acquire(True):
                yvel = y_data * 127 / 32768.0 # Scale to int8_t
                xvel = x_data * 127 / 32768.0 # Scale to int8_t
                lock.release()
            
            if count % print_after == 0:
                print("Pitch: {0}| Yaw: {1}| Yvel: {2} | Xvel: {3}".format(pitch, yaw, yvel, xvel))
            
            if not dryrun:
                tx.transmit(pitch=pitch, yaw=yaw, yvel=yvel, xvel=xvel)
            
            sys.stdout.flush()
            count = count + 1
        time.sleep(0.010)
Exemple #3
0
def getPYRDataFromRift(session):
    """
    Method to obtain the current pitch, yaw and roll data of the Oculus Rift.

    :return: Current Pitch, yaw and roll data of the Oculus Rift
    """
    ts = ovr.getTrackingState(session, ovr.getTimeInSeconds(), True)
    if ts.StatusFlags & (ovr.Status_OrientationTracked | ovr.Status_PositionTracked):
        pose = ts.HeadPose.ThePose
        # axisY=1,axisX=0, axisZ=2
        yaw_rad, pitch_rad, roll_rad = pose.Orientation.getEulerAngles(axis1=1, axis2=0, axis3=2,
                                                                       rotate_direction=1, handedness=1)
        # positive rotation is counter-clockwise
        pitch = float(np.rad2deg(pitch_rad))  # Pitch = rotation around X axis
        yaw = float(np.rad2deg(yaw_rad))  # Yaw = rotation around Y axis
        roll = float(np.rad2deg(roll_rad))  # Roll = rotation around Z axis
        yaw_orig = yaw
        yaw = yaw_orig
        if yaw >= 180:
            yaw = -360 + yaw_orig
    return (pitch * -1, yaw * -1, roll * -1)
Exemple #4
0
def on_open(ws):
    print "### opened ###"
    ws.send("main_client")

    # Used for anti-shaking
    xtemp=0
    ytemp=0
    ztemp=0

    # Main loop
    while True:
        ts  = ovr.getTrackingState(session, ovr.getTimeInSeconds(), \
            True)
        if ts.StatusFlags & (ovr.Status_OrientationTracked | \
            ovr.Status_PositionTracked):
            pose = ts.HeadPose
        
        # Get queternions
        q0 = pose.ThePose.Orientation.w;
        q1 = pose.ThePose.Orientation.x;
        q2 = pose.ThePose.Orientation.y;
        q3 = pose.ThePose.Orientation.z;

        # Calculate Euler angles
        x = int(1500 + 637*math.asin(2*(q0*q1 - q3*q2)))    #pitch
        y = int(1450 - 637*math.atan2(2*(q0*q2 - q1*q3),1 \
        - 2*(q2*q2 + q1*q1))) #yaw
        z = int(1500 + 637*math.asin(2*(q0*q3 + q1*q2)))    #roll

        time.sleep(0.004)
		
		# Anti-shaking
        if abs(xtemp-x)>1 or abs(ytemp-y)>1 or abs(ztemp-z)>1:
                        ws.send(str(x) + "," + str(y) + "," \
                            + str(z) + '\n')
                        sys.stdout.flush()
                        xtemp = x
                        ytemp = y
                        ztemp = z
    ws.close()
Exemple #5
0
 def get_time_in_seconds():
   return ovr.getTimeInSeconds()
Exemple #6
0
##########################################

dateToday = str(datetime.date.today())
Ts = 0.01
reset = False
start = True
latched = False
elapsed = 0.0
experimentStart = 0.0
previousTime = unix.time()
scenario = 0

try:
    while True:
        # Oculus IMU Stuff ############
        ts = ovr.getTrackingState(hmd, ovr.getTimeInSeconds(), True)
        if ts.StatusFlags & (ovr.Status_OrientationTracked
                             | ovr.Status_PositionTracked):
            angularAcc = ts.HeadPose.AngularAcceleration
            linearAcc = ts.HeadPose.LinearAcceleration
            timeSec = ts.HeadPose.TimeInSeconds
            sys.stdout.flush()
        ################################

        # GSR Stuff ####################
        while numbytes < framesize:
            ddata += ser.read(framesize)
            numbytes = len(ddata)

        data = ddata[0:framesize]
        ddata = ddata[framesize:]
'''
Created on Oct 13, 2018

@author: maddo
'''

import sys
import time
import ovr

ovr.initialize(None)
session, luid = ovr.create()
hmdDesc = ovr.getHmdDesc(session)
print(hmdDesc.ProductName)
ts = ovr.getTrackingState(session, ovr.getTimeInSeconds(), True)
if ts.StatusFlags & (ovr.Status_OrientationTracked
                     | ovr.Status_PositionTracked):
    pose = ts.HeadPose
for t in range(100):
    # Query the HMD for the current tracking state.
    ts = ovr.getTrackingState(session, ovr.getTimeInSeconds(), True)
    if ts.StatusFlags & (ovr.Status_OrientationTracked
                         | ovr.Status_PositionTracked):
        pose = ts.HeadPose
        #print(pose.ThePose.Orientation.x) #ovr.Quatf = Orientation, ovr.Vector3f = Position
        #print(pose)
        nextupdown = pose.ThePose.Orientation.z
        nextleftright = pose.ThePose.Orientation.y
        print(nextupdown)
        print(nextleftright)
        sys.stdout.flush()
Exemple #8
0
dateToday = str(datetime.date.today())
Ts = 0.01
reset = False
start = False
latched = True
elapsed = 0.0
experimentStart = 0.0
previousTime = unix.time()
scenario = 0
iterations = num_iterations

try:
    while True:
        # Oculus IMU Stuff ############
        ts  = ovr.getTrackingState(hmd, ovr.getTimeInSeconds(), True)
        if ts.StatusFlags & (ovr.Status_OrientationTracked | ovr.Status_PositionTracked):
            angularAcc = ts.HeadPose.AngularAcceleration
            linearAcc = ts.HeadPose.LinearAcceleration
            timeSec = ts.HeadPose.TimeInSeconds
            sys.stdout.flush()
        ################################
            
        # GSR Stuff ####################
        while numbytes < framesize:
            ddata += ser.read(framesize)
            numbytes = len(ddata)
         
        data = ddata[0:framesize]
        ddata = ddata[framesize:]
        numbytes = len(ddata)
Exemple #9
0
def start_tracker_app():

    oscIdentifier = '/pyBinSim'
    ip = '127.0.0.1'
    port = 10000
    nSources = 1
    minAngleDifference = 5

    if (len(sys.argv)) > 1:
        for i in range(len(sys.argv)):

            if (sys.argv[i] == '-port'):
                port = int(sys.argv[i + 1])

            if (sys.argv[i] == '-ip'):
                ip = sys.argv[i + 1]

            if (sys.argv[i] == '-nSources'):
                nSources = int(sys.argv[i + 1])

            if (sys.argv[i] == '-angleStep'):
                minAngleDifference = int(sys.argv[i + 1])

    # Internal settings:
    positionVector = np.arange(360)
    positionVectorSubSampled = range(0, 360, minAngleDifference)

    # Create OSC client
    parser = argparse.ArgumentParser()
    parser.add_argument("--ip", default=ip, help="The ip of the OSC server")
    parser.add_argument("--port",
                        type=int,
                        default=port,
                        help="The port the OSC server is listening on")
    args = parser.parse_args()

    client = udp_client.SimpleUDPClient(args.ip, args.port)

    # Create OVR session
    ovr.initialize(None)
    session, luid = ovr.create()
    hmdDesc = ovr.getHmdDesc(session)
    print(hmdDesc.ProductName)

    try:
        while 1:
            # Query the HMD for the current tracking state.
            ts = ovr.getTrackingState(session, ovr.getTimeInSeconds(), True)

            if ts.StatusFlags & (ovr.Status_OrientationTracked
                                 | ovr.Status_PositionTracked):
                pose = ts.HeadPose.ThePose

                yawRad, pitchRad, rollRad = pose.Orientation.getEulerAngles(
                    axis1=1,
                    axis2=0,
                    axis3=2,
                    rotate_direction=1,
                    handedness=1)

                yaw = int(round(np.rad2deg(yawRad)))
                pitch = int(round(np.rad2deg(pitchRad)))
                roll = int(round(np.rad2deg(rollRad)))

                # Adjustment to desired global origin
                yaw = 360 - yaw

                if yaw > 360:
                    yaw = yaw - 360

                print(['YAW: ', yaw, ' PITCH: ', pitch, 'ROLL: ', roll])

                for n in range(0, nSources):
                    # round yaw to angle stepsize
                    yaw = min(positionVectorSubSampled,
                              key=lambda x: abs(x - yaw))
                    #channel valueOne valueTwo ValueThree valueFour valueFive ValueSix
                    binSimParameters = [n, yaw, n, 0, 0, 0, 0]
                    print([' Yaw: ', yaw])
                    client.send_message(oscIdentifier, binSimParameters)

                #print pose
                sys.stdout.flush()

            time.sleep(0.020)

    except KeyboardInterrupt:  # Break if ctrl+c is pressed

        ovr.destroy(session)
        ovr.shutdown()

        print('Done')
Exemple #10
0
s.listen(5)
s2.listen(5)
print "Listening for connections..."
q,addr = s.accept()

q2,addr2 = s2.accept()


pos=0
posH = 0


time.sleep(5)


ts  = ovr.getTrackingState(session, ovr.getTimeInSeconds(), True)
if ts.StatusFlags & (ovr.Status_OrientationTracked | ovr.Status_PositionTracked):
    time.sleep(0.200)
        
    pos =ts.HeadPose.ThePose.Orientation.x
    posH =ts.HeadPose.ThePose.Orientation.y


    degaux = convert_to_degrees(pos)
    degauxH = convert_to_degrees(posH)
    degaux = degaux+90
    degaux = round(degaux)
    deg = degaux

    degauxH = degauxH+90
    degauxH = round(degauxH)
Exemple #11
0
 def get_time_in_seconds():
     return ovr.getTimeInSeconds()