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