class ContinuousRotationTrajectory(RotationTrajectory): """ A trajectory with constant rotational velocity. """ def __init__(self, rotationalVelocity, initialRotation=None): self.initialRotation = Quaternion() \ if initialRotation is None else initialRotation self._rotationalVelocity = rotationalVelocity w = rotationalVelocity W = np.asmatrix([[ 0, -w[2,0], w[1,0]], [ w[2,0], 0, -w[0,0]], [-w[1,0], w[0,0], 0 ]]) self.qW = Quaternion() self.qW.setFromMatrix(expm(W)) def rotation(self, t): if np.ndim(t) == 0: return self.qW ** t else: return QuaternionArray([self.qW ** ti for ti in t]) def rotationalVelocity(self, t): omega = np.empty((3, len(np.atleast_1d(t)))) omega[:] = self._rotationalVelocity return omega def rotationalAcceleration(self, t): return np.zeros((3, len(np.atleast_1d(t))))
class ContinuousRotationTrajectory(RotationTrajectory): """ A trajectory with constant rotational velocity. """ def __init__(self, rotationalVelocity, initialRotation=None): self.initialRotation = Quaternion() \ if initialRotation is None else initialRotation self._rotationalVelocity = rotationalVelocity w = rotationalVelocity W = np.asmatrix([[ 0, -w[2,0], w[1,0]], [ w[2,0], 0, -w[0,0]], [-w[1,0], w[0,0], 0 ]]) self.qW = Quaternion() self.qW.setFromMatrix(expm(W)) def rotation(self, t): if np.ndim(t) == 0: return self.qW ** t else: return QuaternionArray([self.qW ** ti for ti in t]) def rotationalVelocity(self, t): omega = np.empty((3, len(np.atleast_1d(t)))) omega[:] = self._rotationalVelocity return omega def rotationalAcceleration(self, t): return np.zeros((3, len(np.atleast_1d(t))))
def loadQualisysTSVFile(filename): """ Load 3DOF or 6DOF marker data from a Qualisys Track Manager TSV file. @param filename: Name of TSV file to load. @return: A L{MarkerCapture} instance. """ # Open file to read header. datafile = open(filename, 'r') # Count of header lines in file. headerLines = 0 capture = MarkerCapture() markerIndices = dict() while True: # Read and count each file header line. line = datafile.readline().strip('\r\n') headerLines = headerLines + 1 # Key and values are tab-separated. items = line.split('\t') key = items[0] values = items[1:] # Handle relevant fields. if key == 'FREQUENCY': # Frame rate. capture.frameRate = int(values[0]) capture.framePeriod = 1/capture.frameRate elif key == 'DATA_INCLUDED': # 3D or 6D data. type = values[0] if (type == '3D'): # 3D data fields are implicitly XYZ co-ordinates. fieldNames = ['X', 'Y', 'Z'] elif key == 'BODY_NAMES' or key == 'MARKER_NAMES': # List of markers or 6DOF body names. for i in range(len(values)): name = values[i] if type == '6D': markerClass = Marker6DOF else: markerClass = Marker3DOF markerIndices[markerClass(capture, name)] = i if (type == '6D'): # Field names are on a line after the body names, each # beginning with a space. fieldNames = [x.strip(' ') for x in datafile.readline().strip('\r\n').split('\t')] headerLines = headerLines + 1 # The above keys are always on the last line in the header. datafile.close() break # Load values from file, skipping header. data = np.loadtxt(filename, skiprows = headerLines) capture.frameCount = len(data) capture.frameTimes = np.arange(0, capture.frameCount / capture.frameRate, capture.framePeriod) # Find index in data array for a given marker and marker field name. def index(marker, name): offset = markerIndices[marker] * len(fieldNames) return offset + fieldNames.index(name) # Return data for a given marker and marker field names. def fields(marker, names): indices = [index(marker, name) for name in names] return data[:,indices[0]:indices[-1]+1] # Iterate through markers to fill in their data. for marker in capture.markers: # Get position data. positions = fields(marker, ['X', 'Y', 'Z']) / 1000 if type == '6D': # Get rotation matrices. matrix_coeffs = fields(marker, ['Rot[%d]' % i for i in range(9)]) # Mark invalid data (residual = -1) with NaNs. invalid = fields(marker, ['Residual'])[:,0] == -1.0 matrix_coeffs[invalid] = np.nan positions[invalid] = np.nan # Convert to quaternions and unflip. quats = QuaternionArray(np.empty((capture.frameCount,4))) for i in range(len(quats)): q = Quaternion() q.setFromMatrix(matrix_coeffs[i].reshape((3,3)).T) quats[i] = q rotations = quats.unflipped() # Add rotation data to marker. for time, rotation in zip(capture.frameTimes, rotations): marker.rotationKeyFrames.add(time, rotation) # Add position data to marker. for time, position in zip(capture.frameTimes, positions): marker.positionKeyFrames.add(time, position.reshape(3,1)) return capture
def loadQualisysTSVFile(filename): """ Load 3DOF or 6DOF marker data from a Qualisys Track Manager TSV file. @param filename: Name of TSV file to load. @return: A L{MarkerCapture} instance. """ # Open file to read header. datafile = open(filename, 'r') # Count of header lines in file. headerLines = 0 capture = MarkerCapture() markerIndices = dict() while True: # Read and count each file header line. line = datafile.readline().strip('\r\n') headerLines = headerLines + 1 # Key and values are tab-separated. items = line.split('\t') key = items[0] values = items[1:] # Handle relevant fields. if key == 'FREQUENCY': # Frame rate. capture.frameRate = int(values[0]) capture.framePeriod = 1 / capture.frameRate elif key == 'DATA_INCLUDED': # 3D or 6D data. type = values[0] if (type == '3D'): # 3D data fields are implicitly XYZ co-ordinates. fieldNames = ['X', 'Y', 'Z'] elif key == 'BODY_NAMES' or key == 'MARKER_NAMES': # List of markers or 6DOF body names. for i in range(len(values)): name = values[i] if type == '6D': markerClass = Marker6DOF else: markerClass = Marker3DOF markerIndices[markerClass(capture, name)] = i if (type == '6D'): # Field names are on a line after the body names, each # beginning with a space. fieldNames = [ x.strip(' ') for x in datafile.readline().strip('\r\n').split('\t') ] headerLines = headerLines + 1 # The above keys are always on the last line in the header. datafile.close() break # Load values from file, skipping header. data = np.loadtxt(filename, skiprows=headerLines) capture.frameCount = len(data) capture.frameTimes = np.arange(0, capture.frameCount / capture.frameRate, capture.framePeriod) # Find index in data array for a given marker and marker field name. def index(marker, name): offset = markerIndices[marker] * len(fieldNames) return offset + fieldNames.index(name) # Return data for a given marker and marker field names. def fields(marker, names): indices = [index(marker, name) for name in names] return data[:, indices[0]:indices[-1] + 1] # Iterate through markers to fill in their data. for marker in capture.markers: # Get position data. positions = fields(marker, ['X', 'Y', 'Z']) / 1000 if type == '6D': # Get rotation matrices. matrix_coeffs = fields(marker, ['Rot[%d]' % i for i in range(9)]) # Mark invalid data (residual = -1) with NaNs. invalid = fields(marker, ['Residual'])[:, 0] == -1.0 matrix_coeffs[invalid] = np.nan positions[invalid] = np.nan # Convert to quaternions and unflip. quats = QuaternionArray(np.empty((capture.frameCount, 4))) for i in range(len(quats)): q = Quaternion() q.setFromMatrix(matrix_coeffs[i].reshape((3, 3)).T) quats[i] = q rotations = quats.unflipped() # Add rotation data to marker. for time, rotation in zip(capture.frameTimes, rotations): marker.rotationKeyFrames.add(time, rotation) # Add position data to marker. for time, position in zip(capture.frameTimes, positions): marker.positionKeyFrames.add(time, position.reshape(3, 1)) return capture