def track_points(self, markers): '''Track a set of markers using phasespace. Parameters ---------- markers : int or sequence of int If this is an integer, specifies the number of markers we ought to track. If this is a sequence of integers, it specifies the IDs of the markers to track. Returns ------- PointTracker : A PointTracker instance to use for tracking the markers in question. ''' if isinstance(markers, int): markers = range(markers) marker_ids = sorted(markers) # set up tracker using owl libraries. index = len(self.trackers) OWL.owlTrackeri(index, OWL.OWL_CREATE, OWL.OWL_POINT_TRACKER) for i, marker_id in enumerate(marker_ids): OWL.owlMarkeri(OWL.MARKER(index, i), OWL.OWL_SET_LED, marker_id) OWL.owlTracker(index, OWL.OWL_ENABLE) if OWL.owlGetStatus() == 0: raise OwlError('point tracker (index {})'.format(index)) tracker = PointTracker(index, marker_ids) self.trackers.append(tracker) return tracker
def reset(self): '''Reset this rigid body based on the current locations of its markers. ''' logging.info('resetting rigid body %s', self.marker_ids) # assemble marker positions positions = [] com = [] with self._lock: for m in self.marker_ids: marker = self._raw_markers[self.marker_ids.index(m)] if marker is None or not 0 < marker.cond < 100: logging.error('missing marker %d for reset', m) return positions.append(marker.pos) if m in self.center_marker_ids: com.append(marker.pos) # compute center of mass cx = sum(x for x, _, _ in com) / len(com) cy = sum(y for _, y, _ in com) / len(com) cz = sum(z for _, _, z in com) / len(com) logging.info('body center: (%s, %s, %s)', cx, cy, cz) # reset phasespace marker positions OWL.owlTracker(self._index, OWL.OWL_DISABLE) for i, (x, y, z) in enumerate(positions): OWL.owlMarkerfv(OWL.MARKER(self._index, i), OWL.OWL_SET_POSITION, (x - cx, y - cy, z - cz)) OWL.owlTracker(self._index, OWL.OWL_ENABLE)
def __init__(self, ip_address, num_points, framerate=None): super(PhasespaceMocapSource, self).__init__() self._num_points = num_points self._shutdown_flag = False # Run the read loop at 2000Hz regardless of actual framerate to control # jitter self._timer = RateTimer(2000) # Connect to the server OWL.owlInit(ip_address, 0) OWL.owlSetInteger(OWL.OWL_FRAME_BUFFER_SIZE, 0) tracker = 0 OWL.owlTrackeri(tracker, OWL.OWL_CREATE, OWL.OWL_POINT_TRACKER) # add the points we want to track to the tracker for i in range(num_points): OWL.owlMarkeri(OWL.MARKER(tracker, i), OWL.OWL_SET_LED, i) # Activate tracker OWL.owlTracker(0, OWL.OWL_ENABLE) # Set frequency if framerate is None: OWL.owlSetFloat(OWL.OWL_FREQUENCY, OWL.OWL_MAX_FREQUENCY) # self._timer = RateTimer(1500) else: OWL.owlSetFloat(OWL.OWL_FREQUENCY, framerate) # self._timer = RateTimer(framerate*3) # Start streaming OWL.owlSetInteger(OWL.OWL_STREAMING, OWL.OWL_ENABLE) # Check for errors if OWL.owlGetError() != OWL.OWL_NO_ERROR: print('%s', (OWL.owlGetError())) raise RuntimeError( 'An error occurred while connecting to the mocap server') # Start the reader thread self._reader = Thread(target=self._reader_thread) self._reader.daemon = True self._start_time = time.time() self._reader.start()
def reset(self): '''Reset this rigid body based on the current locations of its markers. ''' #logging.info('resetting rigid body %s', self.marker_ids) print('Tryingn to reset rigid body %s', self.filename) localPositions = self._getLocalPositions() if (localPositions == -1): return OWL.owlTracker(self._index, OWL.OWL_DISABLE) for i, (x, y, z) in enumerate(localPositions): print('Resetting marker: ' + str(i)) OWL.owlMarkerfv(OWL.MARKER(self._index, i), OWL.OWL_SET_POSITION, [x, y, z]) OWL.owlTracker(self._index, OWL.OWL_ENABLE) print('Done. resetting rigid body %s', self.filename)
def reset(self): '''Reset this rigid body based on the current locations of its markers. ''' logging.info('resetting rigid body %s', self.marker_ids) localPositionsPS_mIdx_xyz = self._getLocalPositions(psFormat=True) # localPositionsPS_mIdx_xyz = [] # for viz_XYZ in localPositionsViz_mIdx_xyz: # ps_XYZ = (-1000*viz_XYZ[2],1000*viz_XYZ[1],-1000*viz_XYZ[0]) # localPositionsPS_mIdx_xyz.append(ps_XYZ) if (localPositionsPS_mIdx_xyz == -1): return OWL.owlTracker(self._index, OWL.OWL_DISABLE) for i, (x, y, z) in enumerate(localPositionsPS_mIdx_xyz): OWL.owlMarkerfv(OWL.MARKER(self._index, i), OWL.OWL_SET_POSITION, [x, y, z]) OWL.owlTracker(self._index, OWL.OWL_ENABLE)
def track_rigid(self, markers_orFilename=None, center_markers=None, localOffest_xyz=[0, 0, 0]): '''Add a rigid-body tracker to the phasespace workload. Parameters ---------- markers_orFilename : varied This parameter describes the markers that make up the rigid body. It can take several forms: - An integer, n. This will result in tracking markers 0 through n-1. - A sequence of integers, (n1, n2, ...). This will use markers n1, n2, ... to track the rigid body. - A dictionary, {n1: (x, y, z), n2: ...}. This will use markers n1, n2, ... to track the rigid body, using relative marker offsets specified by the values in the dictionary. - A string. The rigid body configuration will be loaded from the file named in the string. If markers_orFilename is an integer or sequence of integers, then random marker offsets will be assigned initially; call the .reset() method on the rigid tracker to reassign the marker offsets. center_markers : sequence of int This parameter specifies which marker ids to use for computing the center of the rigid body. If this is None, all markers will be used. Returns ------- RigidTracker : A RigidTracker instance to use for tracking this rigid body. ''' def random_offsets(): return [random.random() - 0.5 for _ in range(3)] marker_map = {} # If markers_orFilename is a dictionary if isinstance(markers_orFilename, dict): marker_map = markers_orFilename # If markers_orFilename is an int if isinstance(markers_orFilename, int): for i in range(markers_orFilename): marker_map[i] = random_offsets() # If markers_orFilename is a tuple if isinstance(markers_orFilename, (tuple, list, set)): for i in markers_orFilename: marker_map[i] = random_offsets() # If markers_orFilename is a string if isinstance(markers_orFilename, str): fileLocation = self.phaseSpaceFilePath + markers_orFilename print "Initializing rigid body: " + fileLocation import os #import os.path #f = open(os.path.dirname(__file__) + '/../data.yml') # load rigid body configuration from a file. with open(fileLocation) as handle: for line in handle: id, x, y, z = line.replace(',', '').strip().split() marker_map[int(id)] = float(x), float(y), float(z) marker_map = sorted(marker_map.iteritems()) marker_ids = tuple(i for i, _ in marker_map) # set up tracker using owl libraries. # Marker locations have been read in from file index = len(self.trackers) OWL.owlTrackeri(index, OWL.OWL_CREATE, OWL.OWL_RIGID_TRACKER) for i, (marker_id, pos) in enumerate(marker_map): OWL.owlMarkeri(OWL.MARKER(index, i), OWL.OWL_SET_LED, marker_id) OWL.owlMarkerfv(OWL.MARKER(index, i), OWL.OWL_SET_POSITION, pos) OWL.owlTracker(index, OWL.OWL_ENABLE) if OWL.owlGetStatus() == 0: raise OwlError('rigid tracker (index {})'.format(index)) tracker = RigidTracker(index, marker_ids, center_markers or marker_ids, localOffest_xyz) if isinstance(markers_orFilename, str): tracker.filepath = self.phaseSpaceFilePath tracker.filename = markers_orFilename self.trackers.append(tracker) return tracker