Files for 6-axis force torque sensor

2019-01-30 20:34:12_arucotagData.csv

2019-01-30 20:34:12_optoforceData.csv

generateMultiaxialFitPlots.py

"""
Make graphs for uniaxial (F=kx, where k.shape is (1,1))

date: 11 Feb 2019
author: nouyang

"""

from scipy.stats.stats import pearsonr
from sklearn import metrics
from sklearn import linear_model
from scipy.stats import linregress
from scipy.interpolate import interp1d
import copy
import math
import sys
import glob
import os
import seaborn as sns
import matplotlib.pyplot as plt
import pandas as pd
import numpy as np


sns.set(rc={"figure.figsize": (16, 10)},
        font_scale=1.8)
plt.style.use('bmh')
# plt.style.use('fivethirtyeight')
# sns.set() #use seaborn style for matplotlib
# ===============================================
#### DECLARE FILES ####
# ===============================================
path = "data"

DATA_FOLDER = os.getcwd()

Fx_file = '2019-01-31 02:25:05'
# Mx_file = '2019-01-31 02:25:05'
Fy_file = '2019-01-31 02:48:31'
Fz_file = '2019-01-31 02:26:49'

# Mx_file = '2019-01-31 02:51:12'
# Mx_file = '2019-01-31 02:50:37'
Mx_file = '2019-01-30 20:05:53'  # +- thetaA #known good

# My_file = '2019-01-31 02:52:32'
# My_file = '2019-01-31 02:51:58'
My_file = '2019-01-30 20:09:20'  # -phi #known good

# Mz_file = '2019-01-31 02:49:34'
# Mz_file = '2019-01-31 02:50:00'
Mz_file = '2019-01-30 20:07:57'  # +- gamma #known good


datafilename = '2019-02-01 06:23:32'
datafilename = '2019-02-01 06:28:10'  # soft motion # looks ok minux z
# datafilename = '2019-02-01 06:32:40'
# Manually everywhere, before zero fix. but
datafilename = '2019-01-30 20:34:12'
# but what does zerofix mean? #TODO #best #delay 13
# datafilename = '2019-02-01 06:26:41' # rest
# datafilename = '2019-02-01 06:28:32' # impulses
# datafilename = Mx_file

# datafilename = '2019-02-01 06:29:22'  # delay 12 samples  / 0.04

# datafilename = '2019-01-30 20:34:12' # tag 2 looks... ok...


def extractDF(filename):
    opto_files = [
        file
        for file in glob.glob(
            os.path.join(DATA_FOLDER, path, filename + "_optoforceData.csv")
        )
    ]
    tag_files = [
        file
        for file in glob.glob(
            os.path.join(DATA_FOLDER, path, filename + "_arucotagData.csv")
        )
    ]
    print("\n--------------------------\n")
    print(filename)
    # print(opto_files)
    # print(tag_files)
    opto_fname = opto_files[0]
    tag_fname = tag_files[0]

    # ===============================================
    ####  Import Data ####
    # ===============================================
    opto_cols = ["Description", "Timestamp",
                 "x", "y", "z", "theta", "phi", "gamma"]

    # following are zero and averaged (in the original collecting)
    tag_cols = ["Description", "Timestamp", "x1", "y1", "z1", "x2", "y2", "z2",
                "theta1", "phi1", "gamma1", "theta2", "phi2", "gamma2",
                "_x", "_y", "_z", "_theta", "_phi", "_gamma", ]

    # tag_cols = ["Description", "Timestamp", "x1", "y1", "z1", "x2", "y2", "z2",
    # "theta1", "phi1", "gamma1", "theta2", "phi2", "gamma2",
    # "_x", "_y", "_z", "_theta", "_phi", "_gamma", ]

    opto_df = pd.read_csv(
        opto_fname,
        header=None,
        sep=";",
        names=opto_cols,
        skip_blank_lines=True,
        usecols=range(8),
    )

    opto_df = opto_df.drop(["Description"], 1)

    tag_df = pd.read_csv(
        tag_fname,
        header=None,
        sep=";",
        names=tag_cols,
        skip_blank_lines=True,
        usecols=range(20),
    )

    tag_df = tag_df.drop(["Description"], 1)

    # ===============================================
    ####  Conversion to real units ####
    # ===============================================

    # --- Tag
    # -- MULTIPLY - convert tag from meters to mm
    for var in ["x1", "y1", "z1"] + ["_x", "_y", "_z"] + ["x2", "y2", "z2"]:
        tag_df[var] *= 1000

    # convert optoforce counts to newtons (provided by optoforce datasheet)
    opto_df["x"] /= 96.11
    opto_df["y"] /= 95.64
    opto_df["z"] /= 20.92

    # --- Optoforce
    # -- convert counts to newton-meters (provided by optoforce datasheet)
    opto_df["theta"] /= 5425.36
    opto_df["phi"] /= 5524.60
    opto_df["gamma"] /= 8103.25

    # -- MULTIPLY - convert optoforce, | N to mN | N*m to mN*m |
    # print('before zero', opto_df.head(2))
    # avg_opto_zeroing = np.average(opto_df.head(20), axis=0)
    # print('zero', avg_opto_zeroing)
    # opto_df -= avg_opto_zeroing
    # print('after zero', opto_df.head(2))

    for a in ["x", "y", "z", "theta", "phi", "gamma"]:
        opto_df[a] *= 1000

    # -- print range of forces measured
    print("\n--------- Force ranges measured -----------------\n")
    for a in ["x", "y", "z", "theta", "phi", "gamma"]:
        print("%s axis force. Min: %0.3f, Max: %0.3f mN or mN*m" %
              (a, opto_df[a].min(), opto_df[a].max()))

    # print(opto_df.head())
    # print(opto_df.describe())

    # ===============================================
    ####  FIX THE DATA (swap axis etc.) ####
    # ===============================================
    # NOTE: truncate beginning, which is behaving strangely for some reason
    # TODO: diagnose this
    tag = tag_df.copy()[20:]
    opto = opto_df.copy()
    # [20:] tag = tag_df.copy()

    # Swap axis as needed to align tag axes with opto axis
    new_cols = ["Timestamp", "y1", "x1", "z1", "y2", "x2", "z2",
                "theta1", "phi1", "gamma1", "theta2", "phi2", "gamma2",
                # following are zero and averaged
                "_y", "_x", "_z", "_theta", "_phi", "_gamma", ]
    tag.columns = new_cols

    # Negate axes as needed
    negateList = ["x1", "y1", "z1", "x2", "y2", "z2",
                  "theta1", "theta2", "gamma1", "gamma2",
                  "_x", "_y", "_z", "_theta", "_gamma"]

    # new_cols = [ "Timestamp", "y1", "x1", "z1", "y2", "x2", "z2",
    # "phi1", "theta1", "gamma1", "phi2", "theta2", "gamma2",
    # # following are zero and averaged
    # "_y", "_x", "_z", "_phi", "_theta", "_gamma", ]

    for n in negateList:
        tag[n] *= -1

    return tag, opto


# ===============================================
####  Linear regress to match apriltag to optoforce & plot ####
# ===============================================


# tagset = str(1)
tagset = "_"


def gen_interp(tag_df, opto_df):
    list_axis = ['x', 'y', 'z', 'theta', 'phi', 'gamma']
    interp_tag_data = []
    interp_opto_data = []
    for AXIZ in list_axis:
        print("\n>--- delay AXIZ: ", AXIZ)
        # a_x = tag_df[AXIZ + tagset]  # single tag
        a_x = tag_df[tagset + AXIZ]  # single tag
        a_t = tag_df.Timestamp

        o_x = opto_df[AXIZ]
        o_t = opto_df.Timestamp

        # interp_tagfxn = interp1d(a_t - lag, a_x, kind='linear', fill_value='extrapolate')
        num_samples = len(o_t) + len(a_t)
        begin = min(o_t.min(), a_t.min())
        end = max(o_t.max(), a_t.max())
        t_regular = np.linspace(begin, end, num_samples * 2)

        # a_x = a_x[20:]
        f_o = interp1d(o_t, o_x, kind="linear", fill_value="extrapolate")
        f_a = interp1d(
            a_t, a_x, kind="linear", fill_value="extrapolate"
        )  # definitely not cubic!
        interp_o = f_o(t_regular)
        interp_a = f_a(t_regular)

        # normalize for cross-correlation
        norm_interp_a = interp_a - interp_a.mean()
        norm_interp_a /= norm_interp_a.std()
        norm_interp_o = interp_o - interp_o.mean()
        norm_interp_o /= norm_interp_o.std()

        corr = np.correlate(norm_interp_o, norm_interp_a, mode="full")
        # source https://dsp.stackexchange.com/questions/9058/measuring-time-delay-of-audio-signals
        delay = int(len(corr) / 2) - np.argmax(corr)
        delay = 11  # NOTE hardcod3

        print("\n>--- delay # datapoints: ", delay, "; in real time: ",
              delay * (t_regular[1] - t_regular[0]))
        timefix_a_x = np.roll(interp_a, shift=-delay)

        gunk = 250
        gunky = -250
        # remove some gunkiness, I think it's introduced by the "extrapolate" option
        # on the interpolation
        t_regular = t_regular[gunk:gunky]
        y = interp_o.reshape(-1, 1)[gunk:gunky]
        X = np.array(timefix_a_x).reshape(-1, 1)[gunk:gunky]
        # interp_opto_data[AXIZ] = y
        # interp_tag_data[AXIZ] = X
        # print('xamples of y', y[100:110])
        # print('samples of X', X[100:110])
        interp_opto_data.append(y)
        interp_tag_data.append(X)

        # print('interp o', interp_opto_data.shape)
        # interp_opto_data = np.vstack((interp_opto_data, y))
        # print('interp o', interp_opto_data.shape)
        # print('y', y.shape)
        # interp_tag_data = np.vstack((interp_tag_data, X))
    interp_opto_data = np.array(interp_opto_data)[:, :, 0]
    interp_tag_data = np.array(interp_tag_data)[:, :, 0]

    return t_regular, interp_tag_data, interp_opto_data


delay = '12=0.045'  # NOTE HARDCODE
# delay = 'normal'

afile = datafilename
tag_df, opto_df = extractDF(afile)  # all 6 axis
t_regular, tag_data, opto_data = gen_interp(tag_df, opto_df)

print(tag_data.shape)
print(opto_data.shape)
# for i, key in enumerate(tag_data):
# print('i', i, 'key', key)
# print('shape', tag_data[key].shape)
tag_data = tag_data.T
opto_data = opto_data.T
regr = linear_model.LinearRegression().fit(tag_data, opto_data)
regr = linear_model.Ridge().fit(tag_data, opto_data)
y_est = regr.predict(tag_data)

print(y_est.shape)
print(y_est.shape[0])

print(regr.coef_)
print('!-- shape!--', regr.coef_.shape)
# print(regr.intercept_)
# print(regr.score(y_est, opto_data))
# print(y_est - opto_data)
print(np.sum(y_est - opto_data))

# ------------ PLOT overlaid
f, axess = plt.subplots(nrows=2, ncols=3, sharex=False)
axess = np.array(axess).flatten()
ax1, ax2, ax3, ax4, ax5, ax6, = axess

axes_reordered = [ax1, ax4, ax2, ax5, ax3, ax6, ]

# -- Plot row of x,y,z tag, then row of x,y,z, opto, 12 plots
for i in range(y_est.shape[1]):
    print(y_est.shape)
    print(opto_data.shape)
    figA = axess[i]
    a_x = y_est[:, i]
    o_x = opto_data[:, i]
    figA.plot(t_regular, o_x, 'k.', ms=1.5,)
    # label=('Measured opto data: ' + str(i)))

    figA.plot(t_regular, a_x, 'r.', ms=1,)
    # label=('Measured opto data: ' + str(i)))

    # figA.plot(t_regular, tag_data[:, i], 'r.', ms=1.5,)
    # label=('Measured tag data: ' + str(i)))
    # figA.legend.

# ax1 = fig_axs[0]
# ax4 = fig_axs[3]
# ax2 = fig_axs[1]
# ax5 = fig_axs[4]
ax1.set_title('X Force')
ax2.set_title('Y Force')
ax3.set_title('Z Force')

ax4.set_title('X Moment')
ax5.set_title('Y Moment')
ax6.set_title('Z Moment')


ax1.set_ylabel("Force (mN)")
ax4.set_ylabel("Torque (mN*m)")
ax5.set_xlabel("Time (secs)")

plt.tight_layout()

# plt.suptitle("Timesteps data, y_est vs y")
plt.savefig('./plots/ridge+lessgunk-' + datafilename + '-' + 'delay-' + str(delay) +
            '-axis-' + tagset + '.png')
# plt.show()

# ------------ YES: PLOT LINEAR FIT ---------------------------
f, fig_axs = plt.subplots(nrows=2, ncols=3, sharex=False)
fig_axs = np.array(fig_axs).T.flatten(order='F')

# Plot of Uniaxial, 6 single axis
for i in range(y_est.shape[1]):
    fig_ax = fig_axs[i]
    a_x = y_est[:, i]
    o_x = opto_data[:, i]
    # print('shapes', a_x.shape, o_x.shape)
    fig_ax.plot(o_x, o_x, 'k-')  # label='Optoforce data '+key)
    print(a_x.shape)
    print(o_x.shape)
    # regr.score(a_x.reshape(-1, 1), o_x.reshape(-1, 1))
    r_score = pearsonr(o_x, a_x)
    print(r_score)
    fig_ax.plot(o_x, a_x, 'r.', markersize=1.5,
                # label=('Linear fit estimate\nR: %.2f, axis %d' % (r_score[0]**2, i)))
                label=('$R^2$ = %.2f' % (r_score[0]**2)))
    # print('i: ', i)
    # print("coef: %f   intercept: %f   r-score: %f"
    # % (regr.coef_, regr.intercept_, r_score))
    fig_ax.legend(markerscale=0, markerfirst=False)

ax1, ax2, ax3, ax4, ax5, ax6, = fig_axs
# ax1 = fig_axs[0]
# ax4 = fig_axs[3]
# ax2 = fig_axs[1]
# ax5 = fig_axs[4]
ax1.set_title('X Force')
ax2.set_title('Y Force')
ax3.set_title('Z Force')

ax4.set_title('X Moment')
ax5.set_title('Y Moment')
ax6.set_title('Z Moment')

ax1.set_ylabel("Prototype Sensor (mN)")
ax4.set_ylabel("Prototype Sensor (mN*m)")
ax2.set_xlabel("Commercial Sensor (mN)")
ax5.set_xlabel("Commercial Sensor (mN*m)")
# plt.suptitle("!--- Linear fits (uniaxial) ---!\n Perfect fit is black line\nRed dots are \
# tag measurements\n(1st order linear fit with affine term)")
# plt.gca().set_aspect('equal', adjustable='datalim')
plt.tight_layout()
# plt.axis('scaled')

plt.savefig('./plots/ridge+lessgunky+uniaxial-' + datafilename + '-' + 'delay-' + str(delay) +
            '-axis' + tagset + '.png')
plt.show()

read_both.py

'''
Read arucotag using openCV library.
Read optoforce, using library from ShadowHand.

Date: 18 Jan 2019
Author: nouyang

# https://github.com/shadow-robot/optoforce/blob/indigo-devel/optoforce/src/optoforce/optoforce_node.py
ctrl-backslash for extra serious force quit (or sudo killall python)

NOTE: may require running `sudo chmod 666 /dev/ttyACM0` due to permissions
issues
'''

import cv2
import cv2.aruco as aruco

import serial
import sys
import signal
import logging
import pprint 
import optoforcelibrary as optoforce
import copy


from datetime import datetime
import time

import threading
from argparse import ArgumentParser

import math
import numpy as np

#------------------------------------
# Run Flags 

# np.set_printoptions(suppress=True, precision=4)
arucoFlag = True
numTags = 2
optoFlag = True 
writeFlag = True # write to CSV file?

#------------------------------------
# Data recording constants

# writeFolder = "./data/"
writeFolder = "../analysis/data/"
strtime = datetime.now().strftime('%Y-%m-%d %H:%M:%S')
o_fname = writeFolder + strtime + '_optoforceData.csv'
a_fname = writeFolder + strtime + '_arucotagData.csv'
fmode = 'a'

inittime = time.time()

# ------------------------------------
# Sensor Constants
optoforce_port = "/dev/ttyACM0"

# ------------------------------------
# Camera Constants
# tag_size = 0.015
#tag_size = 0.0038
tag_size = 0.0055 # guesstimate for old protoype

width = 640
height = 480
fx = 640.0  # must be floats for apriltags._draw_pose to work
fy = 640.0
px = 300.0
py = 250.0

#camera_matrix = np.array([fx, fy, px, py])

# cameraMatrix = np.array([[619.6207333698724, 0.0, 283.87165814439834],
                   # [0.0, 613.2842389650563, 231.9993874728696],
                   # [0.0, 0.0, 1.0]])


# NOTE: I do not have actual camera matrix for the Amazon hawk sensor! (original
# flat)
# Approximate using another webcam...  HBV-1716-2019-01-29_calib.yaml

cameraMatrix = np.array([[521.92676671,   0.,         315.15287785], 
 [  0.,         519.01808261, 193.05763006],
 [  0.,           0.,           1.        ]])

distCoeffs, rvecs, tvecs = np.array([]), [], []

distCoeffs =  np.array([ 0.02207713,  0.18641578, -0.01917194, -0.01310851,
                        -0.11910311])

# ------------------------------------
# Methods to read aruco tag data

def isRotationMatrix(R) :
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype = R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6


# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R) :

    assert(isRotationMatrix(R))

    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])

    singular = sy < 1e-6

    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0

    rots = np.array([x, y, z])
    rots = np.array([math.degrees(r) for r in rots])

    rots[0] = 180 - rots[0] % 360 
    return rots 

## ------------------------------


# ------------------------------------
# Methods to read from optoforce

class OptoThread(threading.Thread):
    def __init__(self, inittime, fname):
        threading.Thread.__init__(self)

        self.inittime = inittime
        self.fname = fname

        optoforce_sensor_type = "s-ch/6-axis"
        starting_index = 0
        scaling_factors =  [[1,1,1,1,1,1]] #one per axis
        # if len(sys.argv) > 1:
        # port = "/dev/" + sys.argv[1]
        try:
            self.driver = optoforce.OptoforceDriver(optoforce_port,
                                                    optoforce_sensor_type,
                                                    scaling_factors)
            print('opened opto force!')
        except serial.SerialException as e:
            print('failed to open optoforce serial port!')
            raise


    def run(self):
        time.sleep(2) # wait for opencv tag to initialize
        while True:
            optotime = time.time()
            self.optodata = self.driver.read()

            if isinstance(self.optodata, optoforce.OptoforceData): #UNTESTED
                opto_data_str = 'Optoforce time, xyz, yawpitchroll; ' + str(optotime-self.inittime)+ '; '+\
                    '; '.join([str(o) for o in self.optodata.force[0]]) + '\n'
                a = ['{0: <8}'.format(x) for x in self.optodata.force[0]]
                #pprint.pprint(' '.join(a))  # NOTE

                #print(opto_data_str)
                if writeFlag:
                    with open(self.fname,'a') as outf :
                        outf.write(opto_data_str)
                        outf.flush()

            elif isinstance(data, optoforce.OptoforceSerialNumber):
                print("The sensor's serial number is " + str(data))


# ------------------------------------
# Methods to read from arucotags

class ArucoThread(threading.Thread):
    def __init__(self, inittime, fname, camera_matrix, dist_coeffs, tag_size):
        threading.Thread.__init__(self)

        self.inittime = inittime
        self.fname = fname
        #self.camera_matrix = camera_matrix
        self.cameraMatrix = camera_matrix
        self.tagSize = tag_size
        self.distCoeffs = dist_coeffs

        self.aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
        self.aruco_params = aruco.DetectorParameters_create()

        #width=1280
        #height=720
        width=640
        height=480
        fps=30

        # ------------------------------------
        self.w = 0.2 # filter_weight

        # ------------------------------------
        # Parse commandline arguments
        parser = ArgumentParser()

        parser.add_argument('device_or_movie', metavar='INPUT', nargs='?', default=0,
                            help='Movie to load or integer ID of camera device')
        options = parser.parse_args()

        # ------------------------------------
        # Open video stream
        try:
            self.stream = cv2.VideoCapture(int(options.device_or_movie))
        except ValueError:
            self.stream = cv2.VideoCapture(options.device_or_movie)

        self.stream.set(cv2.CAP_PROP_FRAME_WIDTH,width)
        self.stream.set(cv2.CAP_PROP_FRAME_HEIGHT,height)
        #self.stream.set(cv2.CAP_PROP_FPS,fps) 

    def run(self):
        # -----------------------------------
        # Set up filtering for angles
        cumTvec, cumRvec = np.ones((numTags, 3)), np.ones((numTags, 3))

        # -----------------------------------
        # Initialized zeros
        counter = 0
        avgN = 20
        
        # -- NOTE: this code is highly repetitive, refactor at some point
        while counter < avgN:
            print('looking for 20 tags... found: ', counter)
            success, frame = self.stream.read()
            if not success:
                print('failed to grab frame')
            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
            corners, ids, rejectedImgPoints = \
                aruco.detectMarkers(gray,
                                    self.aruco_dict,
                                    parameters=self.aruco_params)

            if (ids is not None) and (len(ids) == numTags):
                tagData = zip(ids, corners) 
                tagData = sorted(tagData, key=lambda x: x[0]) # put corners in order of id
                ids = [tag[0] for tag in tagData]
                corners = [tag[1] for tag in tagData]

            rvec, tvec, objPoints = \
                cv2.aruco.estimatePoseSingleMarkers(corners, self.tagSize,
                                                    self.cameraMatrix,
                                                    self.distCoeffs);

            if rvec is not None and rvec.shape[0] == numTags:
                counter += 1
                tvec = tvec.reshape(numTags,3)

                cumTvec += tvec
                for i in range(numTags):
                    rotMat, jacob = cv2.Rodrigues(rvec[i])
                    rots = rotationMatrixToEulerAngles(rotMat)
                    cumRvec[i] = rots 

        if writeFlag == True:
            cv2.imwrite(self.fname + '_.jpg', frame); # for debugging, save a frame

        zeroThetas = copy.deepcopy(cumRvec/avgN)
        zeroDists = copy.deepcopy(cumTvec/avgN)

        # ------------------------------------
        # Look for arucotags
        #out, prevOut = np.ones((numTags, 3)), np.ones((numTags, 3))
        out, prevOut = copy.deepcopy(zeroThetas), copy.deepcopy(zeroDists)

        while True:
            atime = time.time()
            success, frame = self.stream.read()

            if not success:
                print('failed to grab frame')

            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

            corners, ids, rejectedImgPoints = \
                aruco.detectMarkers(gray,
                                    self.aruco_dict,
                                    parameters=self.aruco_params)


            if (ids is not None) and (len(ids) == numTags):
                tagData = zip(ids, corners) 
                tagData = sorted(tagData, key=lambda x: x[0]) # put corners in order of id
                ids = [tag[0] for tag in tagData]
                corners = [tag[1] for tag in tagData]
                rvec, tvec, objPoints = \
                    cv2.aruco.estimatePoseSingleMarkers(corners, self.tagSize,
                                                        self.cameraMatrix,
                                                        self.distCoeffs);
            gray = aruco.drawDetectedMarkers(gray, corners)
            cv2.imshow('frame', gray)

            if rvec is not None and rvec.shape[0] == numTags:
                tvec = tvec.reshape(numTags,3)
                atime = time.time()
                for i in range(numTags): 
                    rotMat, jacob = cv2.Rodrigues(rvec[i])
                    rots = rotationMatrixToEulerAngles(rotMat)

                    out[i] = self.w * rots + (1- self.w) * prevOut[i]
                    prevOut[i] = out[i]
                    #print('Aruco Tag ID:', i, ': ', out[i])
                    # print('Aruco Tag ID:', 1, ': ', out[1])

                # collect zeros at the beginning
                # x,y,z should be zero'd and then changes averaged between the
                # two tags (maybe?)

                # NOTE: we are exponential filtering thetas but not distances
                calcDists = tvec - zeroDists
                calcThetas = out - zeroThetas
                avgCalcDists = np.average(calcDists, axis=0)
                avgCalcThetas = np.average(calcThetas, axis=0)

                # print(tvec[1]* 1000)
                # TODO print readings 

                #print('shapes', [x.shape for x in [zeroThetas, out, calcThetas,
                                                   #avgCalcThetas]])
                #print('zeroThetas', zeroThetas)
                #print('zeroDists', zeroDists)
                #print('filtered reading\n', out)
                print(out)
                #print('zerod reading', calcThetas)
                #print('averaged between two', avgCalcThetas)
                #print('\n')
                # print(outD)
                # print(zeroDists)

                if writeFlag:
                    data_str = 'Arucotag time, tag1 xyz, tag2 xyz, ' + \
                        'tag1 rollpitchyaw (xyz), tag2 -- xyz zerod averaged; ' + \
                        str(atime -self.inittime)+ '; '+ \
                        '; '.join([str(t) for t in calcDists.flatten()]) + '; ' + \
                        '; '.join([str(r) for r in calcThetas.flatten()]) + '; '+ \
                        '; '.join([str(t) for t in avgCalcDists.flatten()]) + '; ' + \
                        '; '.join([str(r) for r in avgCalcThetas.flatten()]) + '\n'
                        # '; '.join([str(t) for t in tvec.flatten()]) + '; ' + \
                        # '; '.join([str(r) for r in out.flatten()]) + '; '+ \

                    with open(self.fname,'a') as outf:
                        outf.write(data_str)
                        outf.flush()

                # a = ['{0: <8}'.format(x) for x in self.optodata.force[0]]
                # pprint.pprint(' '.join(a))




# ------------------------------------
def main():

    inittime = time.time()

    if optoFlag:
        optot = OptoThread(inittime, o_fname)
        optot.daemon = True
        optot.start()
    if arucoFlag:
        # arucot = ArucoThread(inittime, camera_matrix, a_fname)
        arucot = ArucoThread(inittime, a_fname, cameraMatrix, distCoeffs, tag_size)
        arucot.daemon = True
        arucot.start()

    cv2.startWindowThread()
    cv2.namedWindow("Aruco Camera")

    while threading.active_count() > 0:
        # cv2.imshow("Camera", _overlay)
        # cv2.waitKey(0) #IMPORTANT, without this line, window hangs

        k = cv2.waitKey(0)
        if k == 27 or k == ord('q'):  # Escape key
            # self.stopped = True
            print('!! ---- Caught escape key')
            break
        time.sleep(0.001)

        # outf.close()
    print('!! ---- Destroying windows')
    print('=============== \nData STARTED at\n' + strtime)
    print('---------------\nData RECORDED to: ', a_fname)
    endtime = datetime.now().strftime('%Y-%m-%d %H:%M:%S')
    print('---------------\nData collection ENDED at', endtime, '\n==================')
    cv2.destroyAllWindows()
    sys.exit(0)


# ------------------------------------
if __name__ == '__main__':
    #print('=============== \nData STARTED at', strtime, '\n================')
    print('Time: ', datetime.now().strftime('%Y-%m-%d %H:%M:%S'))

    def signal_handler(sig, frame):
        print('You pressed Ctrl+C!')
        sys.exit(0)

    signal.signal(signal.SIGINT, signal_handler)

    try:
        main()
    except KeyboardInterrupt:
        print('!! --- Keyboard interrupt in progress')
    finally:
        print('!! --- Finished cleaning up.')