def PolarisCoord(trck_init, trck_id, ref_mode):
trck = trck_init[0]
trck.Run()
probe = trck.probe.decode(const.FS_ENCODE).split(',')
angles_probe = np.degrees(tr.euler_from_quaternion(probe[2:6],
axes='rzyx'))
trans_probe = np.array(probe[6:9]).astype(float)
coord1 = np.hstack((trans_probe, angles_probe))
ref = trck.ref.decode(const.FS_ENCODE).split(',')
angles_ref = np.degrees(tr.euler_from_quaternion(ref[2:6], axes='rzyx'))
trans_ref = np.array(ref[6:9]).astype(float)
coord2 = np.hstack((trans_ref, angles_ref))
obj = trck.obj.decode(const.FS_ENCODE).split(',')
angles_obj = np.degrees(tr.euler_from_quaternion(obj[2:6], axes='rzyx'))
trans_obj = np.array(obj[6:9]).astype(float)
coord3 = np.hstack((trans_obj, angles_obj))
coord = np.vstack([coord1, coord2, coord3])
Publisher.sendMessage('Sensors ID',
probe_id=trck.probeID,
ref_id=trck.refID,
obj_id=trck.objID)
return coord
def PolarisP4Coord(trck_init, trck_id, ref_mode):
trck = trck_init[0]
trck.Run()
probe = trck.probe.decode(const.FS_ENCODE)
ref = trck.ref.decode(const.FS_ENCODE)
obj = trck.obj.decode(const.FS_ENCODE)
probe = probe[2:]
ref = ref[2:]
obj = obj[2:]
if probe[:7] == "MISSING":
coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
else:
q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
trans_probe = np.array(t).astype(float)
coord1 = np.hstack((trans_probe, angles_probe))
if ref[:7] == "MISSING":
coord2 = np.hstack(([0, 0, 0], [0, 0, 0]))
else:
q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
trans_ref = np.array(t).astype(float)
coord2 = np.hstack((trans_ref, angles_ref))
if obj[:7] == "MISSING":
coord3 = np.hstack(([0, 0, 0], [0, 0, 0]))
else:
q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
trans_obj = np.array(t).astype(float)
coord3 = np.hstack((trans_obj, angles_obj))
coord = np.vstack([coord1, coord2, coord3])
return coord, [trck.probeID, trck.refID, trck.objID]
def OptitrackCoord(trck_init, trck_id, ref_mode):
"""
Obtains coordinates and angles of tracking rigid bodies (Measurement Probe, Coil, Head). Converts orientations from quaternion
rotations to Euler angles. This function uses Optitrack wrapper from Motive API 2.2.
Parameters
----------
:trck_init: tracker initialization instance from OptitrackTracker function at trackers.py
:trck_id: not used
:ref_mode: not used
Returns
-------
coord: position of tracking rigid bodies
"""
trck = trck_init[0]
trck.Run()
scale = 1000 * np.array([1.0, 1.0, 1.0
]) # coordinates are in millimeters in Motive API
angles_probe = np.degrees(
tr.euler_from_quaternion([
float(trck.qwToolTip),
float(trck.qzToolTip),
float(trck.qxToolTip),
float(trck.qyToolTip)
],
axes='rzyx'))
coord1 = np.array([
float(trck.PositionToolTipZ1) * scale[0],
float(trck.PositionToolTipX1) * scale[1],
float(trck.PositionToolTipY1) * scale[2]
])
coord1 = np.hstack((coord1, angles_probe))
angles_head = np.degrees(
tr.euler_from_quaternion([
float(trck.qwHead),
float(trck.qzHead),
float(trck.qxHead),
float(trck.qyHead)
],
axes='rzyx'))
coord2 = np.array([
float(trck.PositionHeadZ1) * scale[0],
float(trck.PositionHeadX1) * scale[1],
float(trck.PositionHeadY1) * scale[2]
])
coord2 = np.hstack((coord2, angles_head))
angles_coil = np.degrees(
tr.euler_from_quaternion([
float(trck.qwCoil),
float(trck.qzCoil),
float(trck.qxCoil),
float(trck.qyCoil)
],
axes='rzyx'))
coord3 = np.array([
float(trck.PositionCoilZ1) * scale[0],
float(trck.PositionCoilX1) * scale[1],
float(trck.PositionCoilY1) * scale[2]
])
coord3 = np.hstack((coord3, angles_coil))
coord = np.vstack([coord1, coord2, coord3])
return coord, [trck.probeID, trck.HeadID, trck.coilID]