def UpdateView(self):
yaw, pitch, roll = self.target.getEuler()
dm = viz.getFrameElapsed() * _KC_MOVE_SPEED
dr = viz.getFrameElapsed() * _KC_ROTATE_SPEED
if viz.key.isDown("w"):
self.target.setPosition([0, 0, dm], viz.REL_LOCAL)
if viz.key.isDown("s"):
self.target.setPosition([0, 0, -dm], viz.REL_LOCAL)
if viz.key.isDown("a"):
self.target.setPosition([-dm, 0, 0], viz.REL_LOCAL)
if viz.key.isDown("d"):
self.target.setPosition([dm, 0, 0], viz.REL_LOCAL)
if viz.key.isDown(_KEYS["forward"]):
pitch -= dr
if viz.key.isDown(_KEYS["back"]):
pitch += dr
if viz.key.isDown(_KEYS["left"]):
yaw -= dr
if viz.key.isDown(_KEYS["right"]):
yaw += dr
if viz.key.isDown("e"):
self.target.setPosition([0, dm, 0], viz.REL_LOCAL)
if viz.key.isDown("q"):
self.target.setPosition([0, -dm, 0], viz.REL_LOCAL)
self.target.setEuler([yaw, pitch, roll], mode=viz.ABS_PARENT)
def UpdateView(self):
yaw,pitch,roll = self.target.getEuler()
dm = viz.getFrameElapsed() * _KC_MOVE_SPEED
dr = viz.getFrameElapsed() * _KC_ROTATE_SPEED
if viz.key.isDown('w'):
self.target.setPosition([0,0,dm], viz.REL_LOCAL)
if viz.key.isDown('s'):
self.target.setPosition([0,0,-dm], viz.REL_LOCAL)
if viz.key.isDown('a'):
self.target.setPosition([-dm,0,0], viz.REL_LOCAL)
if viz.key.isDown('d'):
self.target.setPosition([dm,0,0], viz.REL_LOCAL)
if viz.key.isDown(_KEYS['forward']):
pitch-=dr
if viz.key.isDown(_KEYS['back']):
pitch+=dr
if viz.key.isDown(_KEYS['left']):
yaw-=dr
if viz.key.isDown(_KEYS['right']):
yaw+=dr
if viz.key.isDown('e'):
self.target.setPosition([0,dm,0], viz.REL_LOCAL)
if viz.key.isDown('q'):
self.target.setPosition([0,-dm,0], viz.REL_LOCAL)
self.target.setEuler([yaw,pitch,roll], mode=viz.ABS_PARENT)
def UpdateView():
yaw, pitch, roll = viewLink.getEuler()
m = viz.Matrix.euler(yaw, 0, 0)
dm = viz.getFrameElapsed() * MOVE_SPEED
high = viz.getFrameElapsed() * MOVE_SPEED
if viz.key.isDown(KEYS['forward']):
m.preTrans([0, 0, dm])
if viz.key.isDown(KEYS['back']):
m.preTrans([0, 0, -dm])
if viz.key.isDown(KEYS['left']):
m.preTrans([-dm, 0, 0])
if viz.key.isDown(KEYS['right']):
m.preTrans([dm, 0, 0])
navigationNode.setPosition(m.getPosition(), viz.REL_PARENT)
def masterLoop(num): global time, timeID, DATA_COLLECT, MODEL_DIR, OUTPUT_DIR, dataBuffer, frame, OVRTracker, virtual, \ ISTracker, init, fusionInit, ovrEulerOld # record time time += viz.getFrameElapsed() frame += 1 # first frame of tracker data if init and not fusionInit: virtual.setEuler(ISTracker.getEuler()) ovrEulerOld = OVRTracker.getEuler() fusionInit = True # after first frame of tracker data if fusionInit: # read trackers updataes ovrEuler = OVRTracker.getEuler() ISQuat = ISTracker.getQuat() # quaternion # compute the orientation (as quaternion) if only use the update of oculus virtualQuatByOvr = vizmat.EulerToQuat([a + b - c for a, b, c in zip(virtual.getEuler(), ovrEuler, ovrEulerOld)]) # use intersense position virtual.setPosition(ISTracker.getPosition()) # set camera orientation as the slerp from the oculus-updated orientation to intersense orientation virtual.setQuat(vizmat.slerp(virtualQuatByOvr, ISQuat, viz.getFrameElapsed())) # save the oculus orientation for next frame ovrEulerOld = ovrEuler if DATA_COLLECT: # write data to buffer data = OVRTracker.getEuler() + ISTracker.getEuler() + virtual.getEuler() strData = [str(round(t,4)) for t in data+[time]] strData = ','.join(strData)+'\n' dataBuffer = dataBuffer + strData # write buffer to file if frame % 450 == 0: fileName = OUTPUT_DIR + 'sensorfusion' + timeID + '.csv' file = open(fileName, 'a') file.write(dataBuffer) dataBuffer = '' file.close() print(data)
def update_joystick_movement(joystick, view_node, move_speed, turn_speed, lsl_stream):
"""
Updates the viewpoint depending on joystick movements.
Sends 6dof of viz.MainView via lsl.
Args:
joystick: joystick loaded with either vizconnect or directly as in 'use_control' function above
view_node: Viewpoint node
move_speed: move speed translated from joystick movement to viewpoint updating. Default value is 5.
turn_speed: turn speed elicited by joystick twist in VR. Default value is 90.
lsl_stream: stream to push 6dof sample
"""
elapsed = viz.elapsed()
# Get the joystick position
x, y, z = joystick.getPosition()
# Move the viewpoint based on xy-axis value
view_node.setPosition([0, 0, y * move_speed * viz.getFrameElapsed()], viz.REL_LOCAL)
# Turn the viewpoint left/right based on twist value
view_node.setEuler([x * turn_speed * elapsed, 0, 0], viz.REL_LOCAL)
# stream the HMD 6DOF
pos = viz.MainView.getPosition()
ori = viz.MainView.getEuler()
lsl_stream.push_sample([pos[0], pos[1], pos[2], ori[0], ori[1], ori[2]])
def await_launch(self):
"""
Count timer up to 3s if participant stands at start location and faces ball location. Launch ball after
successful 3s. Reset trial if participant fails.
:return: None
"""
subject_pos = viz.MainView.getPosition()
view_dir = viz.MainView.getMatrix().getForward()
if is_near(pos=subject_pos, target=self.start_pos, margin=0.5):
if self.parent.message_panel:
self.parent.message_panel.visible(False)
self.start_pole.visible(viz.OFF)
if faces(pos=subject_pos, direction=view_dir, target=self.launch_pole.getPosition(), margin=5):
self.countdown_sound.play()
self.start_timer += viz.getFrameElapsed()
self.launch_pole.setScale([1.1, self.start_timer / 3.0, 1.1])
if self.start_timer >= 3.0:
self.ball_pole.visible(viz.OFF)
self.launch_pole.visible(viz.OFF)
self.await_action.setEnabled(False)
self.launch_ball()
else:
self.reset()
else:
self.reset()
def _updatePositionDrag(self): pos = self._tracker.getPosition(self._trackerFlag) self.head.setPosition(pos) self.body_root.setPosition(pos) SPEED_DAMPING = 2.0 ANGLE_DAMPING = 0.4 MAX_DIST = 0.5 # Increment drag position towards actual position t = viz.clamp(viz.getFrameElapsed() * SPEED_DAMPING,0.0,1.0) - 1.0 self._bodyDragPos = vizmat.Interpolate(self._bodyDragPos,pos,-(t*t*t*t-1.0)) #self._bodyDragPos = vizmat.Interpolate(pos,self._bodyDragPos,SPEED_DAMPING) self._bodyDragPos[1] = pos[1] # Clamp drag distance to max value lookPos = viz.Vector(self._bodyDragPos) - pos if lookPos.length() > MAX_DIST: lookPos.setLength(MAX_DIST) lookPos[1] -= ANGLE_DAMPING # Have body root look at drag position m = viz.Matrix() m.makeVecRotVec([0,-1,0],lookPos) self.body_root.setQuat(m.getQuat())
def UpdateJoystick():
if current_scene == 0:
if joy.isButtonDown(1):
accionbuttonNewGame()
elif joy.isButtonDown(2):
Daniel()
elif joy.isButtonDown(3):
Controls()
elif current_scene == 1:
if joy.isButtonDown(2):
MainMenu()
elif current_scene == 2:
if joy.isButtonDown(1):
Conant()
elif joy.isButtonDown(2):
MainMenu()
elif current_scene == 3:
if joy.isButtonDown(1):
Pau()
elif joy.isButtonDown(2):
Daniel()
elif current_scene == 4:
if joy.isButtonDown(1):
Diego()
elif joy.isButtonDown(2):
Conant()
elif current_scene == 5:
if joy.isButtonDown(1):
MainMenu()
elif joy.isButtonDown(2):
Pau()
elif current_scene == 6:
if joy.isButtonDown(2):
returntoMenu()
yaw, pitch, roll = viewLink.getEuler()
m = viz.Matrix.euler(yaw, 0, 0)
dm = viz.getFrameElapsed() * MOVE_SPEED
rm = viz.getFrameElapsed() * ROTATION_SPEED
#get move of Joystick
x, y, z = joy.getPosition()
m.preTrans([0, 0, -y * dm])
navigationNode.setPosition(m.getPosition(), viz.REL_PARENT)
#Get the twist of the joystick
# twist = joy.getTwist()
m.setEuler(x * rm)
navigationNode.setEuler(m.getEuler(), viz.REL_LOCAL)
def UpdateMovement():
elapsed = viz.getFrameElapsed()
trans = device.getRawTranslation()
rx, ry, rz = device.getRawRotation()
viz.MainView.setAxisAngle(
[0, 1, 0, ry * elapsed * ROTATE_SCALE], viz.HEAD_ORI,
viz.REL_LOCAL)
viz.MainView.move(viz.Vector(trans) * elapsed * MOVE_SCALE)
def _incrementYaw(): data.elapsed += viz.getFrameElapsed() if data.elapsed >= data.duration: self.body.setQuat(data.end) return True self.body.setQuat(data.interpolate(data.begin,data.end,data.elapsed/data.duration)) return False
def update(self):
# Get tracker position
tracker_pos = self._tracker.getPosition()
# Get current view position
view_pos = self._offset + tracker_pos
view_pos[1] = self._offset[1] + self.STEP_HEIGHT
# Perform intersection to determine height of view above ground
line_end = view_pos - [0, 500, 0]
isections = [self._intersect(view_pos, line_end)]
# Check points around position to allow buffer around edges
if self.FALL_EDGE_BUFFER > 0.0:
buf = self.FALL_EDGE_BUFFER
isections.append(
self._intersect(view_pos + [buf, 0, 0],
line_end + [buf, 0, 0]))
isections.append(
self._intersect(view_pos + [-buf, 0, 0],
line_end + [-buf, 0, 0]))
isections.append(
self._intersect(view_pos + [0, 0, buf],
line_end + [0, 0, buf]))
isections.append(
self._intersect(view_pos + [0, 0, -buf],
line_end + [0, 0, -buf]))
# Get intersection with largest height
try:
info = max((info for info in isections if info.valid),
key=lambda info: info.point[1])
except ValueError:
info = isections[0]
if info.valid:
# Get height above ground
ground_height = info.point[1]
# If current offset is greater than ground height, then apply gravity
if self._offset[1] > ground_height:
dt = viz.getFrameElapsed()
self._velocity = min(self._velocity + (self.GRAVITY * dt),
self.TERMINAL_VELOCITY)
self._offset[1] -= (self._velocity * dt)
# Clamp to ground level if fallen below threshold
if self._offset[1] - self.GROUND_CLAMP_THRESHOLD < ground_height:
self._offset[1] = ground_height
self._clearVelocity()
# Update position/orientation
self.setPosition(self._offset + tracker_pos)
self.setQuat(self._tracker.getQuat())
def stepPhysics(self):
self.emptyCollisionBuffer()
numCycles = 20
timeStep = viz.getFrameElapsed(
) / numCycles #self.frameRate / numCycles
for idx in range(numCycles):
self.space.collide(self, self.detectCollisions)
self.world.step(timeStep)
def UpdateView(): yaw,pitch,roll = self.viewLink.getEuler() m = viz.Matrix.euler(yaw,0,0) dm = viz.getFrameElapsed() * MOVE_SPEED if viz.key.isDown(KEYS['forward']): m.preTrans([0,0,dm]) if viz.key.isDown(KEYS['back']): m.preTrans([0,0,-dm]) if viz.key.isDown(KEYS['left']): m.preTrans([-dm,0,0]) if viz.key.isDown(KEYS['right']): m.preTrans([dm,0,0]) self.navigationNode.setPosition(m.getPosition(), viz.REL_PARENT)
def UpdateView(): yaw,pitch,roll = hmd.getSensor().getEuler() m = viz.Matrix.euler(yaw,0,0) dm = viz.getFrameElapsed() * MOVE_SPEED if viz.key.isDown(viz.KEY_UP): m.preTrans([0,0,dm]) if viz.key.isDown(viz.KEY_DOWN): m.preTrans([0,0,-dm]) if viz.key.isDown(viz.KEY_LEFT): m.preTrans([-dm,0,0]) if viz.key.isDown(viz.KEY_RIGHT): m.preTrans([dm,0,0]) navigationNode.setPosition(m.getPosition(), viz.REL_PARENT)
def UpdateView():
yaw, pitch, roll = viewLink.getEuler()
m = viz.Matrix.euler(yaw, 0, 0)
dm = viz.getFrameElapsed() * MOVE_SPEED
if viz.key.isDown(viz.KEY_UP):
m.preTrans([0, 0, dm])
if viz.key.isDown(viz.KEY_DOWN):
m.preTrans([0, 0, -dm])
if viz.key.isDown(viz.KEY_LEFT):
m.preTrans([-dm, 0, 0])
if viz.key.isDown(viz.KEY_RIGHT):
m.preTrans([dm, 0, 0])
navigationNode.setPosition(m.getPosition(), viz.REL_PARENT)
def move():
if globals_oa.headTrackingActive:
headForwardDir = headTracker.getLineForward().getDir()
forwardDir = Vec3ToVizardFloatList(getProjectionOfVectorOnPlane(vizardFloatListToVec3(headForwardDir), UP_VECTOR))
backDir = [-forwardDir[0], -forwardDir[1], -forwardDir[2]]
rightDir = Vec3ToVizardFloatList(vizardFloatListToVec3(forwardDir).Cross(UP_VECTOR))
leftDir = [-rightDir[0], -rightDir[1], -rightDir[2]]
upDir = Vec3ToVizardFloatList(UP_VECTOR)
downDir = [-upDir[0], -upDir[1], -upDir[2]]
currentPos = navigationNode.getPosition(viz.ABS_GLOBAL)
newPos = [currentPos[0], currentPos[1], currentPos[2]]
yaw,pitch,roll = navigationNode.getEuler()
m = viz.Matrix.euler(yaw,0,0)
dm = viz.getFrameElapsed() * MOVE_SPEED
deltaYaw = 0
if viz.key.isDown(KEYS['forward']) or viz.key.isDown('w'):
m.preTrans([0,0,dm])
if globals_oa.headTrackingActive:
newPos = [currentPos[0] + forwardDir[0] * dm, currentPos[1] + forwardDir[1] * dm, currentPos[2] + forwardDir[2] * dm]
if viz.key.isDown(KEYS['back']) or viz.key.isDown('s'):
m.preTrans([0,0,-dm])
if globals_oa.headTrackingActive:
newPos = [currentPos[0] + backDir[0] * dm, currentPos[1] + backDir[1] * dm, currentPos[2] + backDir[2] * dm]
if viz.key.isDown(KEYS['left']) or viz.key.isDown('a'):
m.preTrans([-dm,0,0])
if globals_oa.headTrackingActive:
newPos = [currentPos[0] + leftDir[0] * dm, currentPos[1] + leftDir[1] * dm, currentPos[2] + leftDir[2] * dm]
if viz.key.isDown(KEYS['right']) or viz.key.isDown('d'):
m.preTrans([dm,0,0])
if globals_oa.headTrackingActive:
newPos = [currentPos[0] + rightDir[0] * dm, currentPos[1] + rightDir[1] * dm, currentPos[2] + rightDir[2] * dm]
if viz.key.isDown(' '):
m.preTrans([0,dm,0])
if globals_oa.headTrackingActive:
newPos = [currentPos[0] + upDir[0] * dm, currentPos[1] + upDir[1] * dm, currentPos[2] + upDir[2] * dm]
if viz.key.isDown(viz.KEY_CONTROL_L):
m.preTrans([0,-dm,0])
if globals_oa.headTrackingActive:
newPos = [currentPos[0] + downDir[0] * dm, currentPos[1] + downDir[1] * dm, currentPos[2] + downDir[2] * dm]
if viz.key.isDown('e'):
yaw += 1
deltaYaw = 1
if viz.key.isDown('q'):
yaw -= 1
deltaYaw = -1
if not globals_oa.headTrackingActive:
newPos = [currentPos[0] + m.getPosition()[0], currentPos[1] + m.getPosition()[1], currentPos[2] + m.getPosition()[2]]
navigationNode.setEuler(yaw,pitch,roll)
navigationNode.setPosition(newPos, viz.ABS_GLOBAL)
def UpdateView(): yaw,pitch,roll = hmd.getSensor().getEuler() m = viz.Matrix.euler(yaw,0,0) m.setPosition(viz.MainView.getPosition()) dm = viz.getFrameElapsed() * MOVE_SPEED if viz.key.isDown(viz.KEY_UP): m.preTrans([0,0,dm]) if viz.key.isDown(viz.KEY_DOWN): m.preTrans([0,0,-dm]) if viz.key.isDown(viz.KEY_LEFT): m.preTrans([-dm,0,0]) if viz.key.isDown(viz.KEY_RIGHT): m.preTrans([dm,0,0]) m.setEuler([yaw,pitch,roll]) viz.MainView.setMatrix(m)
def update(self): # Get tracker position tracker_pos = self._tracker.getPosition() # Get current view position view_pos = self._offset + tracker_pos view_pos[1] = self._offset[1] + self.STEP_HEIGHT # Perform intersection to determine height of view above ground line_end = view_pos - [0,500,0] isections = [self._intersect(view_pos,line_end)] # Check points around position to allow buffer around edges if self.FALL_EDGE_BUFFER > 0.0: buf = self.FALL_EDGE_BUFFER isections.append(self._intersect(view_pos+[buf,0,0],line_end+[buf,0,0])) isections.append(self._intersect(view_pos+[-buf,0,0],line_end+[-buf,0,0])) isections.append(self._intersect(view_pos+[0,0,buf],line_end+[0,0,buf])) isections.append(self._intersect(view_pos+[0,0,-buf],line_end+[0,0,-buf])) # Get intersection with largest height try: info = max((info for info in isections if info.valid),key=lambda info:info.point[1]) except ValueError: info = isections[0] if info.valid: # Get height above ground ground_height = info.point[1] # If current offset is greater than ground height, then apply gravity if self._offset[1] > ground_height: dt = viz.getFrameElapsed() self._velocity = min(self._velocity + (self.GRAVITY * dt),self.TERMINAL_VELOCITY) self._offset[1] -= (self._velocity * dt) # Clamp to ground level if fallen below threshold if self._offset[1] - self.GROUND_CLAMP_THRESHOLD < ground_height: self._offset[1] = ground_height self._clearVelocity() # Update position/orientation self.setPosition(self._offset+tracker_pos) self.setQuat(self._tracker.getQuat())
def updateView(self): yaw,pitch,roll = self.VIEW_LINK.getEuler() m = viz.Matrix.euler(yaw,0,0) dm = viz.getFrameElapsed() * self.MOVE_SPEED if viz.key.isDown(self.KEYS['forward']) and self.CAN_STRAFE: m.preTrans([0,0,dm]) if viz.key.isDown(self.KEYS['back']) and self.CAN_STRAFE: m.preTrans([0,0,-dm]) if viz.key.isDown(self.KEYS['left']) and self.CAN_STRAFE: m.preTrans([-dm * self.STRAFE_SPEED,0,0]) if viz.key.isDown(self.KEYS['right']) and self.CAN_STRAFE: m.preTrans([dm * self.STRAFE_SPEED,0,0]) if viz.key.isDown(self.KEYS['up']) and self.CAN_ELEVATE: m.preTrans([0,dm,0]) if viz.key.isDown(self.KEYS['down']) and self.CAN_ELEVATE: m.preTrans([0,-dm,0]) self.NODE.setPosition(m.getPosition(), viz.REL_PARENT)
def log_data(self):
"""
Gather data from viewpoint, ball, glove, eye tracker and write to fail
:return: None
"""
# update timer
self.timer += viz.getFrameElapsed()
#####################################
# collect data ######################
#####################################
# time
data = [self.timer]
# viewpos xyz
data += viz.MainView.getPosition()
# view direction xyz
data += viz.MainView.getMatrix().getForward()
# gaze direction xyz
if hardware.eye_tracker:
gaze_vec = hardware.eye_tracker.getGazeDirection()
else:
gaze_vec = [np.nan, np.nan, np.nan]
data += gaze_vec
# ball position xyz
data += self.ball.vis_node.getPosition()
# ball color
data.append(self.ball.getColor()[0])
# glove position and orientation (if any)
if self.glove:
data += self.glove.vis_node.getPosition()
data += self.glove.ori_tracker.getMatrix().getForward()
else:
data += [np.nan, np.nan, np.nan]
data += [np.nan, np.nan, np.nan]
######################################
######################################
data_string = ','.join([str(x) for x in data])
self.data_file.write(data_string + "\n")
def run_trial(i_trial, angle, speed, dsize, ipd, recorded):
global trial_time, alpha, print_flag, ii, timer_on, obst_v, data_buffer, recording, \
IPD, eye_height
time_elapsed = viz.getFrameElapsed()
trial_time += time_elapsed
# Current position and roation of the participant
cur_pos = viz.get(viz.HEAD_POS)
cur_rot = viz.get(viz.HEAD_ORI)
# Pops up the Emergency Walls when participant is close to physical room edge.
popWalls(cur_pos)
# print(viz.MainWindow.getIPD())
# print(eye_height, models['obstPole'].getPosition()[1] + models['obstPole'].getScale()[1] * 3)
# Record data to buffer
if recorded and recording:
obst = models['obstPole'].getPosition()
goal = models['goalPole'].getPosition()
obstScale = models['obstPole'].getScale()
data = [
trial_time, cur_pos[0], cur_pos[1], cur_pos[2], cur_rot[0],
cur_rot[1], cur_rot[2], obst[0], obst[1], obst[2], obstScale[0],
obstScale[1], obstScale[2], goal[0], goal[1], goal[2]
]
data_buffer += ','.join([str(round(dat, 4)) for dat in data]) + '\n'
if stage == 'ready':
# Print start of trial, trial # and condition, initialize trial
if print_flag:
print_flag = False
print('>>> Start Trial ' + str(i_trial) + ': ' + str(angle) +
', ' + str(speed) + ', ' + str(dsize) + ', ' + str(ipd))
# Update IPD for this the trial
IPD = ipd
# Compute obstacle position and velocity for later use
ang, dist = angle / 360 * 2 * math.pi, (DIAGONAL - 4) / 2
obst_vi = rotate(DIAGONAL_UNIT[i_trial % 2], ang) # unit vector
obst_p = [-i * dist for i in obst_vi]
obst_v = [vi * speed for vi in obst_vi]
models['obstPole'].setPosition(obst_p)
models['obstPole'].setScale([0.5, 0.6, 0.5])
models['homePole'].setPosition(HOME_POLE[i_trial % 2])
models['orientPole'].setPosition(ORI_POLE[i_trial % 2])
models['homePole'].visible(viz.ON)
models['orientPole'].visible(viz.ON)
if recorded:
data_buffer += '\n' + 'trial,' + str(i_trial).zfill(3) + ',angle,' + str(angle) + ',speed,' + str(speed) +\
',dsize,' + str(dsize) + ',ipd,' + str(ipd) + '\n'
data_buffer += 'timeStamp,subjX,subjY,subjZ,subjYaw,subjPitch,subjRow,obstX,obstY,obstZ,obstScaleX,obstScaleY,obstScaleZ,goalX,goalY,goalZ\n'
if alpha < 1.0:
models['homePole'].alpha(alpha)
models['orientPole'].alpha(alpha)
alpha += time_elapsed
if inRadiusAndFacing():
if timer(ORIENT_TIME):
goToStage('go')
else:
timer_on = False
elif stage == 'go':
# Turn off home and orient poles, turn on goal pole
if models['homePole'].getVisible():
# Read eye height
eye_height = cur_pos[1]
models['homePole'].visible(viz.OFF)
models['orientPole'].visible(viz.OFF)
models['goalPole'].setPosition(ORI_POLE[i_trial % 2])
models['goalPole'].visible(viz.ON)
sounds['begin'].play()
trial_time = 0
recording = True
# Is stage ended?
if overTheLine(cur_pos, RAMP_DIST, i_trial):
goToStage('avoid')
else:
# Check subject lateral deviation
if offCourse(cur_pos):
sounds['startover'].play()
reset_trial()
# Obstacle appears
elif stage == 'avoid':
if speed > 0:
if not models['obstPole'].getVisible():
models['obstPole'].visible(viz.ON)
# Move obstPole
moveTarget(models['obstPole'], obst_v, time_elapsed)
# Change the size of obstPole when it's on the opposite side of homePole
# if models['obstPole'].getPosition()[0] * models['homePole'].getPosition()[0] < 0:
changeSize(models['obstPole'], eye_height, cur_pos, dsize,
time_elapsed)
# Trial end
if overTheLine(cur_pos, -END_DIST, i_trial):
# Write data to disk
if recorded:
filename = os.path.abspath(
os.path.join(OUTPUT_DIR, 'exp_a_Subj' + subject + '.csv'))
with open(filename, 'a') as file:
file.write(data_buffer)
reset_trial()
if i_trial == TOTAL_TRIALS:
stage == 'NULL'
models['homePole'].setPosition(HOME_POLE[(i_trial + 1) % 2])
models['homePole'].visible(viz.ON)
models['orientPole'].visible(viz.OFF)
models['goalPole'].visible(viz.OFF)
sounds['end'].play()
else:
ii += 1
def UpdateView(self, YR_input=None):
#elapsedTime = viz.elapsed()
elapsedTime = viz.getFrameElapsed()
if elapsedTime > .02:
print("viz.tick: ", viz.tick())
print("frame number: ", viz.getFrameNumber())
print("elapsedTime:", elapsedTime)
yawrate = 0.0
turnangle = 0.0
distance = 0.0
dt = elapsedTime
#dt = 1.0/60.0 #not sure why but it's perceptually smoother with a constant. This shouldn't be the case.
#Get steering wheel and gas position
data = joy.getPosition()
SteeringWheelValue = data[0] # on scale from -1 to 1.
gas = data[1]
#keep heading up to date.
ori = self.__view.getEuler()
self.__heading = ori[0]
if viz.key.isDown(viz.KEY_UP):
gas = -5
elif viz.key.isDown(viz.KEY_DOWN):
gas = 5
if viz.key.isDown(viz.KEY_LEFT
): #rudimentary control with the left/right arrows.
data[0] = -1
elif viz.key.isDown(viz.KEY_RIGHT):
data[0] = 1
# #Compute drag
# drag = self.__speed / 300.0
self.__dir = 1
if (YR_input is not None) and (
self.__automation == True
): #provides the opportunity to pass a yaw rate to the driver.
yawrate = YR_input
Wheel_yawrate = self.__dir * SteeringWheelValue * 35.0 #max wheel lock is 35degrees per s yawrate
self.__Wheel_yawrate_adjustment = yawrate - Wheel_yawrate #correction for any offset between virtual yawrate and yawrate as specified by the steering wheel angle.
else:
yawrate = self.__dir * SteeringWheelValue * 35.0 #max wheel lock is 35degrees per s yawrate
#add adjustment for smooth take-over.
yawrate += self.__Wheel_yawrate_adjustment
turnangle = yawrate * dt
self.__heading += turnangle
self.__pause = self.__pause + 1
#Update the viewpoint
if self.__pause > 0:
distance = self.__speed * dt
#posnew = (0,0,self.__speed)
posnew = (0, 0, distance)
eulernew = (self.__heading, 0, 0)
self.__view.setPosition(posnew, viz.REL_LOCAL)
self.__view.setEuler(eulernew)
else:
self.__heading = 0.0
self.__dir = 1.0
turnangle = 0.0
#return the values used in position update
UpdateValues = []
UpdateValues.append(yawrate)
UpdateValues.append(turnangle)
UpdateValues.append(distance)
UpdateValues.append(dt)
UpdateValues.append(SteeringWheelValue)
UpdateValues.append(self.__Wheel_yawrate_adjustment)
return (UpdateValues)
def UpdateMovement(): elapsed = viz.getFrameElapsed() trans = device.getRawTranslation() rx,ry,rz = device.getRawRotation() viz.MainView.setAxisAngle([0,1,0,ry*elapsed*ROTATE_SCALE], viz.HEAD_ORI, viz.REL_LOCAL) viz.MainView.move(viz.Vector(trans)*elapsed*MOVE_SCALE)
def calculate_segment_CoM(self):
x_TBCoM = {}
y_TBCoM = {}
z_TBCoM = {}
self.test_segment_list = []
for segment in self.body_segments:
if segment[0] is 'Pelvis':
# for body segment pelvis distal end (hip midpoint) equals the midpoint between r and l hip
# get tracker of r and l hip
lhip, rhip = trackers[16], trackers[20]
# use list comprehension to calculate midpoint (using getPosition for coordinates)
self.HIP_MIDPOINT = [
((a + b / 2))
for a, b in zip(lhip.getPosition(), rhip.getPosition())
]
#print(self.HIP_MIDPOINT)
# apply hip midpoint coordinates to sphere
self.s.setPosition(self.HIP_MIDPOINT)
# get position of prox / dist segment ends
prox_pos = self.HIP_CENTER
prox_pos = prox_pos.getPosition()
dist_pos = self.HIP_MIDPOINT
else:
# link green/red spheres to prox/dist segment
prox = segment[1]
dist = segment[2]
# get position of prox / dist segment ends
prox_pos = prox.getPosition()
dist_pos = dist.getPosition()
#print(segment, prox_pos, dist_pos)
seg = [
segment[0] + '_prox', prox_pos[0], prox_pos[1], prox_pos[2],
segment[0] + '_dist', dist_pos[0], dist_pos[1], dist_pos[2]
]
#data = map(lambda x: str(x).translate(None, ['[',']']), seg)
self.test_segment_list.append(seg) #[segment, dist_pos, prox_pos])
x_prox, y_prox, x_dist, y_dist = prox_pos[0], prox_pos[
1], dist_pos[0], dist_pos[1]
z_prox, z_dist = prox_pos[2], prox_pos[2]
segment = segment[0]
l_prox, l_dist = self.perc_of_segm_length[segment][
0], self.perc_of_segm_length[segment][1]
#
x_COM = x_prox * l_dist + x_dist * l_prox
y_COM = y_prox * l_dist + y_dist * l_prox
z_COM = z_prox * l_dist + z_dist * l_prox
# apply body segment COM to according sphere
self.com_spheres[segment].setPosition(x_COM, y_COM, z_COM)
self.com_spheres[segment].visible(viz.OFF)
segment_fraction = self.perc_of_segm_length[segment][2]
x_TBCoM[segment] = (x_COM * segment_fraction)
y_TBCoM[segment] = (y_COM * segment_fraction)
z_TBCoM[segment] = (z_COM * segment_fraction)
X = sum(x_TBCoM.values())
Y = sum(y_TBCoM.values())
Z = sum(z_TBCoM.values())
# apply TBCoM coordinates to TBCoM sphere
self.TBCoM_sphere.setPosition(X, Y, Z)
self.TBCoM_sphere.visible(viz.OFF)
#print(self.TBCoM_sphere.getPosition()[0])
#print(self.TBCoM_sphere.getEuler())
# print out data in console
#print(self.test_segment_list)
#print(self.exp.TRIAL_NO, self.exp.current_stimulus_name, self.exp.current_stimulus_yaw, self.exp.current_stimulus_roll,self.exp.STATE,
# viz.tick(), (viz.tick() - self.exp.TRIAL_START_TIME), viz.getFrameNumber(), viz.getFrameElapsed(), viz.getFrameTime(), [X, Y, Z], tracker_coordinates)#self.segment_coordinates)
# temporarily save framewise data into empty list
self.TEMP_RV_DATA.append([
self.exp.STATE, self.exp.response, self.exp.keyPressTime,
viz.tick(),
viz.getFrameNumber(),
viz.getFrameElapsed(),
viz.getFrameTime()
])
self.TEMP_COM_DATA.append(
self.test_segment_list) # self.segment_coordinates
self.TEMP_TCBOM.append(['TCBOM', X, Y, Z])
def masterLoop(num):
# global variables within masterLoop
global DATA_COLLECT, DO_PRACTICE, is_practice, is_freewalk, data_collect, trial_stage, trial_num, \
freewalk_session, time, time_stamp, cur_pos, conditions, practice_conditions, condition, K, B, _alpha, \
reset_countDown, controlType,instruction, screenshot, data_batch, leaderSpd, leader, avatarID,\
time_elapsed, HZ, flag
# Time elapsed since the last run of masterLoop and then added to the global time
time_elapsed = viz.getFrameElapsed()
time += time_elapsed
if os.path.isfile(OUTPUT_DIR + 'image'+ str(screenshot) +'.bmp') == True:
screenshot += 1
vizact.onkeydown('p', viz.window.screenCapture, OUTPUT_DIR + 'image'+ str(screenshot) +'.bmp')
# Current position and roation of the participant
cur_pos = viz.get(viz.HEAD_POS)
cur_rot = viz.get(viz.HEAD_ORI)
#>> Will only work in VENLab <<
emergencyWalls.popWalls(cur_pos) # Pops up the Emergency Walls when participant is close to physical room edge.
##################
# Begin Freewalk #
##################
if is_freewalk == True:
# Writes Position and Rotation, but only when DATA_COLLECT set to True
# and trial_stage has set data_collect to True
if DATA_COLLECT and data_collect:
# Position: Target_x, Target_y, Target_z, Participant_x, Participant_y, Participant_z, Yaw, Pitch, Row, time stamp
data = [cur_pos[0], cur_pos[1], cur_pos[2], cur_rot[0], cur_rot[1], cur_rot[2]]
strData = [str(round(t,4)) for t in data+[time]]
strData = ','.join(strData)+'\n'
data_batch = data_batch + strData
#########
# 00 01 Freewalk Pretrial: sets up practice trial, establishes pole locations
if trial_stage == 'pretrial_00_01':
if flag:
print '> Start Free Walking Session ' + str(freewalk_session) + ' Trial ' + str(trial_num) + ' ----------------------------------'
flag = False
# Set position of home pole (where participant stands to start trial)
if models['homePole'].getVisible() == False:
models['homePole'].setPosition(HOME_POLE[trial_num%2])
models['homePole'].alpha(1.0)
models['homePole'].visible(viz.ON)
# Set position of orientation pole (where participant faces to start trial)
if models['orientPole'].getVisible() == False:
models['orientPole'].setPosition(HOME_POLE[(trial_num+1)%2])
models['orientPole'].visible(viz.ON)
if trial_num == 1 and freewalk_session == 1 and instruction:
# placeholder for instructions
instruction = False
if not(trial_num == 1 and freewalk_session == 1) or countDown(5):
# Move to next stage
goToStage('orient_00_02')
instruction = True
reset_countDown = True
#########
# 00 02 Orienting to Pole: Give time for participant to orient to the pole
elif trial_stage == 'orient_00_02':
flag = True
if inRadiusAndFacing():
if trial_num == 1 and instruction:
if freewalk_session == 2:
pass # placeholder for instructions
instruction = False
if not(trial_num == 1) or (freewalk_session == 1) or (freewalk_session == 2 and countDown(5)):
# Move to stage 3
goToStage('orient_00_02_wait')
instruction = True
reset_countDown = True
#########
# wait for orientation
elif (trial_stage == 'orient_00_02_wait'):
if countDown(ORIENT_TIME):
goToStage('inposition_00_03')
reset_countDown = True
if not inRadiusAndFacing():
reset_countDown = True
#########
# 00 03 Freewalk In Position: proceeds once participant is standing on home and facing orient for three seconds
elif (trial_stage == 'inposition_00_03'):
print 'Free walk start'
# Turn off home pole
models['homePole'].visible(viz.OFF)
models['orientPole'].visible(viz.OFF)
models['targetPole'].setPosition(HOME_POLE[(trial_num+1)%2])
models['targetPole'].visible(viz.ON)
sounds['Begin'].play()
# Start to collect data
data_collect = True
# initialize batch data output
data_batch = ''
time = 0
# Move to Stage 4
goToStage('target_00_04')
#########
# 00 04 Freewalk: Participants Moves
elif (trial_stage == 'target_00_04'):
# Detects participant location, moves to Stage 5 (Ends Trial) when participant reache the end line
if (trial_num%2 == 1) and (cur_pos[2] > endLine(cur_pos[0])) or \
(trial_num%2 == 0)and (cur_pos[2] < endLine(cur_pos[0])):
goToStage('endtrial_00_05')
#########
# 00 05 End Freewalk Trial: Close out the trial and reset values for next practice trial or start Experiment
elif trial_stage == 'endtrial_00_05':
# Clears the target pole
models['targetPole'].visible(viz.OFF)
# save the data of this trial
fileName = OUTPUT_DIR + NICKNAME + '_freewalk' + '_subj' + subject + '_s' + str(freewalk_session) + '_trial' + str(trial_num).zfill(3) + '.csv'
with open(fileName, 'a') as file:
file.write(data_batch)
print 'End Freewalk Trial ' + str(trial_num)
data_collect = False
# End Check: When trial_num is greater than FREEWALK_TRIALS, end practice and start experiment block
if trial_num == FREEWALK_TRIALS:
print '>> End Freewalk Session<<'
if freewalk_session == 2:
print '>>> End Experiment <<<'
goToStage('NULL')
if instruction:
sounds['End'].play()
instruction = False
elif freewalk_session == 1:
goToStage('pretrial_01_01')
is_freewalk = False
trial_num = 1
freewalk_session += 1
# Returns to Stage 1, resets clock
else:
trial_num += 1
goToStage('pretrial_00_01')
instruction = True
##################
# Begin practice #
##################
elif is_practice == True:
#########
# 01 01 Practice Pretrial: sets up practice trial, establishes pole locations
if trial_stage == 'pretrial_01_01':
if flag:
print '> Start Practice Trial ' + str(trial_num) + ' ----------------------------------'
flag = False
# load input
if practice_conditions[trial_num][3] == 'avatar':
avatarID = random.choice(range(len(avatars)))
leader = avatars[avatarID]
leader.state(5)
else:
leader = models['leaderPole']
leaderSpd = 0
# play instruction
if trial_num == 1 and instruction:
# placeholder for instructions
instruction = False
# Move to Stage 2
if trial_num != 1 or countDown(5):
goToStage('orient_01_02')
#########
# 01 02 Orienting to Pole: Give time for participant to orient to the pole
elif (trial_stage == 'orient_01_02'):
flag = True
# Set position of home pole (where participant stands to start trial)
if models['homePole'].getVisible() == False:
models['homePole'].setPosition(HOME_POLE[trial_num%2])
if _alpha < 1.0:
models['homePole'].alpha(_alpha)
models['homePole'].visible(viz.ON)
_alpha += 1.0/HZ
# Set position of orientation pole (where participant faces to start trial)
if models['orientPole'].getVisible() == False:
models['orientPole'].setPosition(HOME_POLE[(trial_num+1)%2])
models['orientPole'].visible(viz.ON)
if inRadiusAndFacing():
reset_countDown = True
# Move to stage 3
goToStage('orient_01_02_wait')
instruction = True
_alpha = 0.0
# Current time
time_stamp = time
#########
# wait for orientation
elif (trial_stage == 'orient_01_02_wait'):
if countDown(ORIENT_TIME):
goToStage('inposition_01_03')
reset_countDown = True
if not inRadiusAndFacing():
reset_countDown = True
#########
# 01 03 Practice In Position: proceeds once participant is standing on home and facing orient for three seconds
elif trial_stage == 'inposition_01_03':
print 'Practice Target Appears'
# Turn off home pole and orientation pole
models['homePole'].visible(viz.OFF)
models['orientPole'].visible(viz.OFF)
models['targetPole'].setPosition(HOME_POLE[(trial_num+1)%2])
models['targetPole'].visible(viz.ON)
sounds['Begin'].play()
# Move to Stage 4
goToStage('target_01_04')
time = 0
#########
# 01 04 Moving leader: Leader Moves
elif (trial_stage == 'target_01_04'):
leaderOnset = float(practice_conditions[trial_num][4])
d0 = float(practice_conditions[trial_num][1])
v0 = float(practice_conditions[trial_num][2])
if time > leaderOnset:
if not leader.getVisible():
models['targetPole'].visible(viz.OFF)
# leader appear d0 meter in front of participant
leader.setPosition(HOME_POLE[trial_num%2])
leader.lookAt(HOME_POLE[(trial_num+1)%2], mode=viz.ABS_GLOBAL)
home = models['homePole'].getPosition()
d = d0 + projection([x-y for x, y in zip(cur_pos, home)], [-x for x in home])
moveTarget(leader, d/time_elapsed, conditions[trial_num][3] == 'avatar') # move the leader by d0 from homepole to leader
leader.visible(viz.ON)
# leader moves
moveTarget(leader, v0, practice_conditions[trial_num][3] == 'avatar')
# Detects participant location, moves to Stage 6 (Ends Trial) when participant reache the end line
if (trial_num%2 == 1) and (cur_pos[2] > endLine(cur_pos[0])) or \
(trial_num%2 == 0)and (cur_pos[2] < endLine(cur_pos[0])):
goToStage('endtrial_01_05')
#########
# 01 05 End Practice Trial: Close out the trial and reset values for next practice trial or start Experiment
elif trial_stage == 'endtrial_01_05':
# Clears the target pole
leader.visible(viz.OFF)
print 'End Practice Trial ' + str(trial_num)
# End Check: When trial_num is greater than PRACTICE_TRIALS, end practice and start experiment block
if trial_num >= PRACTICE_TRIALS:
print '>> End Practice <<'
goToStage('pretrial_02_01')
is_practice = False
trial_num = 1
# Returns to Stage 1, resets clock
else:
trial_num += 1
goToStage('pretrial_01_01')
####################
# Begin Experiment #
####################
elif is_practice == False:
# Writes Position and Rotation, but only when DATA_COLLECT set to True
# and trial_stage has set data_collect to True
if DATA_COLLECT and data_collect:
# Location of Target Pole
leader_loc = leader.getPosition()
leader_loc = leader.getPosition()
# Position: Target_x, Target_y, Target_z, Participant_x, Participant_y, Participant_z, Yaw, Pitch, Row, time stamp
data = [leader_loc[0], leader_loc[1], leader_loc[2], cur_pos[0], cur_pos[1], cur_pos[2], cur_rot[0], cur_rot[1], cur_rot[2]]
strData = [str(round(t,4)) for t in data+[time]] + [str(avatarID)]
strData = ','.join(strData)+'\n'
data_batch = data_batch + strData
# log IPD
if trial_num == 1:
file = open(OUTPUT_DIR + NICKNAME + '_subj' + subject + \
'_IPD_' + str(IPD) + '.txt', 'a')
file.close()
#########
# 02 01 Experiment Pretrial: sets up trial, establishes pole locations
if trial_stage == 'pretrial_02_01':
condition = ', '.join(conditions[trial_num][1:4])
# Print start of trial, trial #, and type of trial [pos][speed][turn]
if flag:
print '> Start Trial ' + str(trial_num) + ': ' + condition + ' ----------------------------------'
flag = False
# load input
if conditions[trial_num][3] == 'avatar':
avatarID = random.choice(range(len(avatars)))
leader = avatars[avatarID]
leader.state(5)
elif conditions[trial_num][3] == 'pole':
leader = models['leaderPole']
leaderSpd = 0
# play instruction
if trial_num == 1 and instruction:
instruction = False
# Move to Stage 2
if trial_num != 1 or countDown(5):
goToStage('orient_02_02')
#########
# 02 02 Orienting to Pole: Give time for participant to orient to the pole
elif (trial_stage == 'orient_02_02'):
flag = True
## Set position of home pole (where participant stands to start trial)
if models['homePole'].getVisible() == False:
models['homePole'].setPosition(HOME_POLE[trial_num%2])
if _alpha < 1.0:
models['homePole'].alpha(_alpha)
models['homePole'].visible(viz.ON)
_alpha += 1.0/HZ
# Set position of orient pole (where participant faces to start trial)
if models['orientPole'].getVisible() == False:
models['orientPole'].setPosition(HOME_POLE[(trial_num+1)%2])
models['orientPole'].visible(viz.ON)
if inRadiusAndFacing():
# Move to stage 3
goToStage('orient_02_02_wait')
_alpha = 0.0
instruction = True
reset_countDown = True
#########
# wait for orientation
elif (trial_stage == 'orient_02_02_wait'):
if countDown(ORIENT_TIME):
goToStage('inposition_02_03')
reset_countDown = True
if not inRadiusAndFacing():
reset_countDown = True
#########
# 02 03 In Position: proceeds once participant is standing on home and facing orient
elif trial_stage == 'inposition_02_03':
# Turn off home and orient poles
models['homePole'].visible(viz.OFF)
models['orientPole'].visible(viz.OFF)
models['targetPole'].setPosition(HOME_POLE[(trial_num+1)%2])
models['targetPole'].visible(viz.ON)
sounds['Begin'].play()
# Turn on data collection for this trial
data_collect = True
# Move to Stage 5
goToStage('target_02_04')
# initialize batch data output
data_batch = ''
time = 0
#########
# 02 04 Moving Leader: Leader Moves
elif (trial_stage == 'target_02_04'):
# read data
leaderOnset = float(conditions[trial_num][4])
d0 = float(conditions[trial_num][1])
v0 = float(conditions[trial_num][2])
if time > leaderOnset:
if not leader.getVisible():
models['targetPole'].visible(viz.OFF)
# leader appear d0 meter in front of participant
leader.setPosition(HOME_POLE[trial_num%2])
leader.lookAt(HOME_POLE[(trial_num+1)%2], mode=viz.ABS_GLOBAL)
home = models['homePole'].getPosition()
d = d0 + projection([x-y for x, y in zip(cur_pos, home)], [-x for x in home])
moveTarget(leader, d/time_elapsed, conditions[trial_num][3] == 'avatar') # move the leader by d0 from homepole to leader
leader.visible(viz.ON)
# leader moves
moveTarget(leader, v0, conditions[trial_num][3] == 'avatar')
# Detects participant location, moves to Stage 6 (Ends Trial) when participant reache the end line
if (trial_num%2 == 1) and (cur_pos[2] > endLine(cur_pos[0])) or \
(trial_num%2 == 0)and (cur_pos[2] < endLine(cur_pos[0])):
goToStage('endtrial_02_05')
#########
# 02 05 End Trial: Close out the trial and reset values for next trial
elif trial_stage == 'endtrial_02_05':
# Clears the target pole
leader.visible(viz.OFF)
# End data collection for this trial
data_collect = False
# save the data of this trial
fileName = OUTPUT_DIR + NICKNAME + '_subj' + subject + '_trial' + str(trial_num).zfill(3) + '_' + condition + '.csv'
with open(fileName, 'a') as file:
file.write(data_batch)
print 'End Trial ' + str(trial_num)
# When trial_num is greater than TOTAL_TRIALS, end experiment
if trial_num == TOTAL_TRIALS:
is_freewalk = True
goToStage('pretrial_00_01')
trial_num = 1
# Returns to Stage 1, resets clock
else:
trial_num += 1
goToStage('pretrial_02_01')
def controlSchemaForFiveArms(): global headControlledAppendage global fourthArmTrackerLastPos, fifthArmTrackerLastPos, sixthArmTrackerLastPos #schema for the headControlledAppendage if pptextension is not None: if pptextension.headPPT is not None: yaw,pitch,roll = pptextension.headPPT.getEuler() taYaw = yaw + 180#pitch/RANGE_DIVISOR taPitch = 0 taRoll = -pitch#roll/RANGE_DIVISOR + 90 # right headControlledAppendage.setEuler(taYaw, taPitch, taRoll) # if some_global_variables.USING_WIIMOTE_TO_ACTIVATE_ARMS and (not some_global_variables.FOURTH_FIFTH_APPENDAGE_ACTIVE): # return #schema for the some_global_variables.appendage4 yaw,pitch,roll = some_global_variables.isCube1.getEuler() taYaw = yaw taPitch = 0 taRoll = pitch # some_global_variables.appendage4.setEuler(taYaw, taPitch, taRoll) deltaYaw = taYaw - some_global_variables.lhControlledAppendagePreviousEuler[0] deltaPitch = 0 deltaRoll = taRoll - some_global_variables.lhControlledAppendagePreviousEuler[1] some_global_variables.lhControlledAppendagePreviousEuler = [yaw, pitch, roll] # print "euler changes = " + str([deltaYaw, deltaPitch, deltaRoll]) # print "some_global_variables.lhControlledAppendagePreviousEuler = " + str(some_global_variables.lhControlledAppendagePreviousEuler) if some_global_variables.ABSOLUTE_MAPPING: some_global_variables.appendage4.setEuler(taYaw, taPitch, taRoll, mode = viz.ABS_PARENT) else: some_global_variables.appendage4.setEuler(deltaYaw, deltaPitch, deltaRoll, mode = viz.REL_LOCAL) #schema for the some_global_variables.appendage5 yaw,pitch,roll = some_global_variables.isCube2.getEuler() taYaw = yaw taPitch = 0 taRoll = pitch # some_global_variables.appendage5.setEuler(taYaw, taPitch, taRoll) deltaYaw = taYaw - some_global_variables.lhControlledAppendagePreviousEuler[0] deltaPitch = 0 deltaRoll = taRoll - some_global_variables.lhControlledAppendagePreviousEuler[1] some_global_variables.lhControlledAppendagePreviousEuler = [yaw, pitch, roll] if some_global_variables.ABSOLUTE_MAPPING: some_global_variables.appendage5.setEuler(taYaw, taPitch, taRoll, mode = viz.ABS_PARENT) #derivatives for leftFingersPPT, fifthArmPPT, sixthArmPPT # print "viz.getFrameNumber() = " + str(viz.getFrameNumber()) # print "viz.getFrameElapsed() = " + str(viz.getFrameElapsed()) if fourthArmTrackerLastPos is None: fourthArmTrackerLastPos = pptextension.leftFingersPPT.getPosition(viz.ABS_GLOBAL) if fifthArmTrackerLastPos is None: fifthArmTrackerLastPos = pptextension.fifthArmPPT.getPosition(viz.ABS_GLOBAL) if sixthArmTrackerLastPos is None: sixthArmTrackerLastPos = pptextension.sixthArmPPT.getPosition(viz.ABS_GLOBAL) elapsedTimeSinceTheLastFrame = viz.getFrameElapsed() global dsMax, dsMin, dsdtMax, dsdtMin, d2sdt2Max, d2sdt2Min if dsMax == None: dsMax = 0 if dsMin == None: dsMin = 10000000 if dsdtMax == None: dsdtMax = 0 if dsdtMin == None: dsdtMin = 10000000 if d2sdt2Max == None: d2sdt2Max = 0 if d2sdt2Min == None: d2sdt2Min = 10000000 #derivatives of fourth arm tracker currentPosOfFourthArmTracker = pptextension.leftFingersPPT.getPosition(viz.ABS_GLOBAL) dsFourth = (Vector3(currentPosOfFourthArmTracker[0], currentPosOfFourthArmTracker[1], currentPosOfFourthArmTracker[2]).__sub__(Vector3(fourthArmTrackerLastPos[0], fourthArmTrackerLastPos[1], fourthArmTrackerLastPos[2]))).magnitude() dsdtFourth = dsFourth/elapsedTimeSinceTheLastFrame d2sdt2Fourth = dsdtFourth/elapsedTimeSinceTheLastFrame if dsFourth > dsMax: dsMax = dsFourth if dsFourth < dsMin: dsMin = dsFourth if dsdtFourth > dsdtMax: dsdtMax = dsdtFourth if dsdtFourth < dsdtMin: dsdtMin = dsdtFourth if d2sdt2Fourth > d2sdt2Max: d2sdt2Max = d2sdt2Fourth if d2sdt2Fourth < d2sdt2Min: d2sdt2Min = d2sdt2Fourth #derivatives of fifth arm tracker currentPosOfFifthArmTracker = pptextension.fifthArmPPT.getPosition(viz.ABS_GLOBAL) dsFifth = (Vector3(currentPosOfFifthArmTracker[0], currentPosOfFifthArmTracker[1], currentPosOfFifthArmTracker[2]).__sub__(Vector3(fifthArmTrackerLastPos[0], fifthArmTrackerLastPos[1], fifthArmTrackerLastPos[2]))).magnitude() dsdtFifth = dsFifth/elapsedTimeSinceTheLastFrame d2sdt2Fifth = dsdtFifth/elapsedTimeSinceTheLastFrame #derivatives of sixth arm tracker currentPosOfSixthArmTracker = pptextension.sixthArmPPT.getPosition(viz.ABS_GLOBAL) dsSixth = (Vector3(currentPosOfSixthArmTracker[0], currentPosOfSixthArmTracker[1], currentPosOfSixthArmTracker[2]).__sub__(Vector3(sixthArmTrackerLastPos[0], sixthArmTrackerLastPos[1], sixthArmTrackerLastPos[2]))).magnitude() dsdtSixth = dsSixth/elapsedTimeSinceTheLastFrame d2sdt2Sixth = dsdtSixth/elapsedTimeSinceTheLastFrame
def updateContrast(self, mode, min=0.01):
"""
Update the contrast in the specified mode (called by setContrastMode)
:param mode: lin_dist, inv_dist, exp_dist, cos2angle, cos2gaze, bearing_vel, elevation_acc
:param min: minimum contrast
:return: None
"""
pos = np.array(self.vis_node.getPosition())
view_pos = np.array(viz.MainView.getPosition())
view_ori = np.array(viz.MainView.getMatrix().getForward())
if hardware.eye_tracker:
gaze_dir = hardware.eye_tracker.getGazeDirection()
else:
gaze_dir = view_ori
dist = np.sqrt(np.sum((pos - view_pos) ** 2))
rel_dist = abs(self.initial_dist - dist) / self.initial_dist
ball_vec = (pos - view_pos)
# ball_vel = np.array(self.getVelocity())
dt = viz.getFrameElapsed()
# self.vis_velocity = np.mean(np.diff(self.vis_angle_array)) / dt
# print vis_velocity
# bearing angle
bearing = np.rad2deg(np.arctan2(ball_vec[0], ball_vec[2]))
# print self.bearing_velocity
c = .3
previous_bearing = self.bearing
self.bearing = c * bearing + (1 - c) * previous_bearing
# elevation angle
# elevation = np.tan(np.arcsin(ball_vec.dot(np.array([0, 1, 0])) / np.linalg.norm(ball_vec)))
if mode == 'lin_dist':
light_o = min + (1 - min) * rel_dist
elif mode == 'inv_dist':
light_o = 1 - (1 - min) * rel_dist
elif mode == 'exp_dist':
light_o = min + ((1 - min) - (1 - min) * np.exp(-rel_dist))
elif mode == 'cos2angle':
dir_angle = cos_between(view_ori, (pos - view_pos))
light_o = min + (1 - min) * (1 - dir_angle ** 2)
elif mode == 'cos2gaze':
dir_angle = cos_between(gaze_dir, (pos - view_pos))
light_o = min + (1 - min) * (1 - dir_angle ** 2)
elif mode == 'bearing_vel':
# light_o = min + max * (1 - 1.0 / (1 + np.abs(bearing_vel)))
# light_o = min + (1 - min) * (1 - np.exp(- 2 * np.abs(self.bearing_velocity)))
#bearing_velocity = \
#savgol_filter(np.array(self.bearing_array), window_length=9, polyorder=2, deriv=1, delta=dt)[4]
bearing_velocity = (self.bearing - previous_bearing) / dt
light_o = (1 / (1 + np.exp(- 2 * (np.abs(bearing_velocity) - 6))))
# print light_o
elif mode == 'bearing_dist':
#bearing_velocity = \
#savgol_filter(np.array(self.bearing_array), window_length=9, polyorder=2, deriv=1, delta=dt)[4]
#bearing_velocity = (np.array(self.bearing_array[-5:]).mean() - np.array(self.bearing_array[-6:-1]).mean()) / dt
bearing_velocity = (self.bearing - previous_bearing) / dt
light_o = (1 / (1 + np.exp(- 1.5 * (np.abs(bearing_velocity) - 7)))) * (min + (1 - min) / self.initial_dist * dist)
elif mode == 'tan_pos':
x = x_path
if self.initial_pos[0] > 0:
x = -x_path
posx = view_pos[0]
posz = view_pos[2]
dist = np.sqrt(np.min((posz-z)**2 + (posx-x)**2))
light_o = 1 - (1 / (1 + np.exp(- 15 * (dist - 0.4))))
elif mode == 'constant_dark':
light_o = 0.2
# constant or invalid mode
else:
light_o = 1.0
# print 'light_o: '+str(light_o)
# self.visNode.alpha(light_o)
# TODO: do we really want 0.8? make this variable via config
self.setColor([0.8 * light_o] * 3)
def LogPosition (self): timeStamp = round(viz.tick()-sgetGameLoadingTime(False),2) #record player position every 1 second if viz.tick() % 1 <= viz.getFrameElapsed(): self._pLog.append((timeStamp, self._view.getPosition()[0], self._view.getPosition()[2]))
def run_trial(i_trial, s0, d0, angle, recorded):
global trial_time, alpha, print_flag, ii, timer_on, v0, p_goal, data_buffer, recording
time_elapsed = viz.getFrameElapsed()
trial_time += time_elapsed
# Current position and roation of the participant
cur_pos = viz.get(viz.HEAD_POS)
cur_rot = viz.get(viz.HEAD_ORI)
# Pops up the Emergency Walls when participant is close to physical room edge.
popWalls(cur_pos)
# Record data to buffer
if recorded and recording:
goal = models['goalPole'].getPosition()
data = [
trial_time, cur_pos[0], cur_pos[1], cur_pos[2], cur_rot[0],
cur_rot[1], cur_rot[2], goal[0], goal[2],
models['goalPole'].getVisible()
]
data_buffer += ','.join([str(dat) for dat in data]) + '\n'
if stage == 'ready':
# Print start of trial, trial # and condition, initialize trial
if print_flag:
print_flag = False
print('>>> Start Trial ' + str(i_trial) + ': ' +
', '.join([str(s0), str(d0), str(angle)]))
# Compute goal pole position and pacePole velocity for later use
ang = angle / 360 * 2 * math.pi
subj_goal_unit = rotate(
DIAGONAL_UNIT[i_trial % 2],
ang) # unit vector pointing from subj to goal
p_goal = [
h + d * RAMP_DIST + i * d0
for i, h, d in zip(subj_goal_unit, HOME_POLE[i_trial % 2],
DIAGONAL_UNIT[i_trial % 2])
]
p_pace = [
h + d * PACE_DIST
for h, d in zip(HOME_POLE[i_trial % 2], DIAGONAL_UNIT[i_trial %
2])
]
v0 = [i * s0 for i in DIAGONAL_UNIT[i_trial % 2]]
models['goalPole'].setPosition(p_goal)
models['pacePole'].setPosition(p_pace)
models['homePole'].setPosition(HOME_POLE[i_trial % 2])
models['orientPole'].setPosition(ORI_POLE[i_trial % 2])
models['pacePole'].setPosition(p_pace)
models['homePole'].visible(viz.ON)
models['orientPole'].visible(viz.ON)
if recorded:
data_buffer += '\n' + ','.join([
'trial',
str(i_trial).zfill(3), 's0',
str(s0), 'd0',
str(d0), 'angle',
str(angle)
]) + '\n'
data_buffer += 'timeStamp,subjX,subjY,subjZ,subjYaw,subjPitch,subjRow,' + 'goalX,goalZ,goalVisible\n'
if alpha < 1.0:
models['homePole'].alpha(alpha)
models['orientPole'].alpha(alpha)
alpha += time_elapsed
if inRadiusAndFacing():
if timer(ORIENT_TIME):
goToStage('pace')
else:
timer_on = False
elif stage == 'pace':
# Turn off home and orient poles, turn on pacePole
if models['homePole'].getVisible():
models['homePole'].visible(viz.OFF)
models['orientPole'].visible(viz.OFF)
if s0:
models['pacePole'].visible(viz.ON)
else:
# For freewalk trial
models['goalPole'].setPosition(ORI_POLE[i_trial % 2])
models['goalPole'].visible(viz.ON)
sounds['begin'].play()
trial_time = 0
recording = True
# Move pacePole
if s0:
moveTarget(models['pacePole'], v0, time_elapsed)
# Is stage ended?
if overTheLine(cur_pos, RAMP_DIST, i_trial):
goToStage('approach')
else:
# Check subject lateral deviation
if offCourse(cur_pos):
sounds['startover'].play()
reset_trial()
# Obstacle appears
elif stage == 'approach':
if s0:
if not models['goalPole'].getVisible():
models['goalPole'].visible(viz.ON)
models['pacePole'].visible(viz.OFF)
# Trial end
end_dist = END_DIST
if not s0:
p_goal = HOME_POLE[(i_trial + 1) % 2]
end_dist = 2
if distance(cur_pos[0], cur_pos[2], p_goal[0], p_goal[2]) < end_dist:
# Write data to disk
if recorded:
filename = os.path.abspath(
os.path.join(OUTPUT_DIR,
'Approach_subj' + subject + '.csv'))
with open(filename, 'a') as file:
file.write(data_buffer)
reset_trial()
if i_trial == TOTAL_TRIALS:
stage == 'NULL'
sounds['end'].play()
else:
ii += 1