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
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)
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)
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()
def get_time_in_seconds(): return ovr.getTimeInSeconds()
##########################################
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()
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)
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')
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)
def get_time_in_seconds():
return ovr.getTimeInSeconds()