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