diff --git a/example_scripts/record/watch_phone_uarm_record.py b/example_scripts/record/watch_phone_uarm_record.py index 6d627f1..38542c3 100644 --- a/example_scripts/record/watch_phone_uarm_record.py +++ b/example_scripts/record/watch_phone_uarm_record.py @@ -17,8 +17,7 @@ parser.add_argument('ip', type=str, help=f'put your local IP here. ' f'The script will listen to watch data on ' - f'PORT {config.PORT_LISTEN_WATCH_PHONE_IMU_LEFT} for left hand or' - f'PORT {config.PORT_LISTEN_WATCH_PHONE_IMU_RIGHT} for right hand') + f'PORT {config.PORT_LISTEN_WATCH_PHONE_IMU_LEFT}') parser.add_argument('file', type=str, help=f'recorded data will be written into this file') args = parser.parse_args() @@ -31,7 +30,6 @@ # This script has two separate queues for the case that the # user streams from a left-hand and a right-hand device at the same time left_q = queue.Queue() # data for left-hand mode -right_q = queue.Queue() # data for right-hand mode # left listener imu_l = ImuListener( @@ -45,47 +43,22 @@ args=(left_q,) ) -# right listener -imu_r = ImuListener( - ip=args.ip, - msg_size=messaging.watch_phone_imu_msg_len, - port=config.PORT_LISTEN_WATCH_PHONE_IMU_RIGHT, - tag="LISTEN IMU RIGHT" -) -r_thread = threading.Thread( - target=imu_r.listen, - args=(right_q,) -) - -# left publisher wp_rl = WatchPhoneUarmRecorder(file=args.file) rl_thread = threading.Thread( - target=wp_rl.stream_loop, + target=wp_rl.processing_loop, args=(left_q,) ) -# right publisher -wp_rr = WatchPhoneUarmRecorder(file=args.file, left_hand_mode=False) -rr_thread = threading.Thread( - target=wp_rr.stream_loop, - args=(right_q,) -) - l_thread.start() -r_thread.start() rl_thread.start() -rr_thread.start() def terminate_all(*args): imu_l.terminate() wp_rl.terminate() - imu_r.terminate() - wp_rr.terminate() # make sure all handler exit on termination atexit.register(terminate_all) signal.signal(signal.SIGTERM, terminate_all) signal.signal(signal.SIGINT, terminate_all) - diff --git a/example_scripts/stream/watch_only.py b/example_scripts/stream/watch_only.py index 9048580..a075d99 100644 --- a/example_scripts/stream/watch_only.py +++ b/example_scripts/stream/watch_only.py @@ -64,4 +64,4 @@ def terminate_all(*args): f'pose data on PORT {config.PORT_PUB_LEFT_ARM}') args = parser.parse_args() - run_watch_only_nn_udp(ip=args.ip, smooth=5, stream_mc=True) + run_watch_only_nn_udp(ip=args.ip, smooth=10, stream_mc=True) diff --git a/example_scripts/stream/watch_phone_pocket_kalman.py b/example_scripts/stream/watch_phone_pocket.py similarity index 96% rename from example_scripts/stream/watch_phone_pocket_kalman.py rename to example_scripts/stream/watch_phone_pocket.py index 85d1366..2ffe787 100644 --- a/example_scripts/stream/watch_phone_pocket_kalman.py +++ b/example_scripts/stream/watch_phone_pocket.py @@ -33,7 +33,7 @@ def run_watch_phone_pocket_kalman(ip: str, smooth: int, stream_mc: bool) -> Watc port=config.PORT_PUB_LEFT_ARM, window_size=10, stream_mc=stream_mc, - model_hash="SW-model-1211") + model_hash="SW-v3.8-model-436400") p_thread = threading.Thread( target=kpp.processing_loop, args=(left_q,) diff --git a/example_scripts/stream/watch_phone_uarm.py b/example_scripts/stream/watch_phone_uarm.py index 465cc78..8d00689 100644 --- a/example_scripts/stream/watch_phone_uarm.py +++ b/example_scripts/stream/watch_phone_uarm.py @@ -7,11 +7,11 @@ import wear_mocap_ape.config as config from wear_mocap_ape.stream.listener.imu import ImuListener +from wear_mocap_ape.stream.publisher.watch_phone_uarm_nn_udp import WatchPhoneUarmNnUDP from wear_mocap_ape.data_types import messaging -from wear_mocap_ape.stream.publisher.watch_phone_uarm_udp import WatchPhoneUarmUDP -def run_watch_phone_uarm_udp(ip, smooth): +def run_watch_phone_uarm_nn_udp(ip, smooth, stream_mc): # data processing happens in independent threads. # We exchange data via queues. left_q = queue.Queue() # data for left-hand mode @@ -28,10 +28,11 @@ def run_watch_phone_uarm_udp(ip, smooth): ) # left publisher - wp2ul = WatchPhoneUarmUDP( + wp2ul = WatchPhoneUarmNnUDP( ip=ip, port=config.PORT_PUB_LEFT_ARM, - smooth=smooth + smooth=smooth, + stream_mc=stream_mc, ) ul_thread = threading.Thread( target=wp2ul.processing_loop, @@ -64,7 +65,7 @@ def terminate_all(*args): help=f'put your local IP here. ' f'The script will publish arm ' f'pose data on PORT {config.PORT_PUB_LEFT_ARM} (left hand)') - parser.add_argument('smooth', nargs='?', type=int, default=5, help=f'smooth predicted trajectories') + parser.add_argument('smooth', nargs='?', type=int, default=10, help=f'smooth predicted trajectories') args = parser.parse_args() - run_watch_phone_uarm_udp(ip=args.ip, smooth=args.smooth) + run_watch_phone_uarm_nn_udp(ip=args.ip, smooth=args.smooth, stream_mc=True) diff --git a/example_scripts/stream/watch_phone_pocket_nn.py b/experimental_scripts/watch_phone_pocket_nn.py similarity index 100% rename from example_scripts/stream/watch_phone_pocket_nn.py rename to experimental_scripts/watch_phone_pocket_nn.py diff --git a/example_scripts/stream/watch_phone_uarm_nn.py b/experimental_scripts/watch_phone_uarm_raw.py similarity index 85% rename from example_scripts/stream/watch_phone_uarm_nn.py rename to experimental_scripts/watch_phone_uarm_raw.py index d73748d..465cc78 100644 --- a/example_scripts/stream/watch_phone_uarm_nn.py +++ b/experimental_scripts/watch_phone_uarm_raw.py @@ -7,11 +7,11 @@ import wear_mocap_ape.config as config from wear_mocap_ape.stream.listener.imu import ImuListener -from wear_mocap_ape.stream.publisher.watch_phone_uarm_nn_udp import WatchPhoneUarmNnUDP from wear_mocap_ape.data_types import messaging +from wear_mocap_ape.stream.publisher.watch_phone_uarm_udp import WatchPhoneUarmUDP -def run_watch_phone_uarm_nn_udp(ip, smooth, stream_mc): +def run_watch_phone_uarm_udp(ip, smooth): # data processing happens in independent threads. # We exchange data via queues. left_q = queue.Queue() # data for left-hand mode @@ -28,11 +28,10 @@ def run_watch_phone_uarm_nn_udp(ip, smooth, stream_mc): ) # left publisher - wp2ul = WatchPhoneUarmNnUDP( + wp2ul = WatchPhoneUarmUDP( ip=ip, port=config.PORT_PUB_LEFT_ARM, - smooth=smooth, - stream_mc=stream_mc, + smooth=smooth ) ul_thread = threading.Thread( target=wp2ul.processing_loop, @@ -68,4 +67,4 @@ def terminate_all(*args): parser.add_argument('smooth', nargs='?', type=int, default=5, help=f'smooth predicted trajectories') args = parser.parse_args() - run_watch_phone_uarm_nn_udp(ip=args.ip, smooth=args.smooth, stream_mc=False) + run_watch_phone_uarm_udp(ip=args.ip, smooth=args.smooth) diff --git a/experimental_scripts/watch_raw_record.py b/experimental_scripts/watch_raw_record.py new file mode 100644 index 0000000..7a52a83 --- /dev/null +++ b/experimental_scripts/watch_raw_record.py @@ -0,0 +1,136 @@ +import atexit +import logging +import queue +import signal +import threading +from datetime import datetime +from pathlib import Path + +import wear_mocap_ape.config as config +from wear_mocap_ape.record.watch_only_rec import WatchOnlyRecorder + +from wear_mocap_ape.stream.listener.imu import ImuListener +from wear_mocap_ape.data_types import messaging + +import argparse + +# Instantiate the parser +parser = argparse.ArgumentParser(description='records data from the watch in standalone mode') +# Required arguments +parser.add_argument('ip', type=str, + help=f'put your local IP here. ' + f'The script will listen to watch data on ' + f'PORT {config.PORT_LISTEN_WATCH_IMU_LEFT}') +parser.add_argument('file', type=str, + help=f'recorded data will be written into this file') +args = parser.parse_args() + +logging.basicConfig(level=logging.INFO) + +# listener and publisher run in separate threads. Listener fills the queue, publisher empties it +q = queue.Queue() + +# the listener fills the que with received and parsed smartwatch data +imu_l = ImuListener( + ip=args.ip, + msg_size=messaging.watch_only_imu_msg_len, + port=config.PORT_LISTEN_WATCH_IMU_LEFT # the smartwatch app sends on this PORT +) +imu_w_l = threading.Thread( + target=imu_l.listen, + args=(q,) +) + +watch_rec = WatchOnlyRecorder(file=args.file) + +prev_time = datetime.now() +active = True +file = Path(args.file) + +header = [ + "timestamp", + "sw_dt", + "sw_h", + "sw_m", + "sw_s", + "sw_ns", + "sw_rotvec_w", + "sw_rotvec_x", + "sw_rotvec_y", + "sw_rotvec_z", + "sw_rotvec_conf", + "sw_gyro_x", + "sw_gyro_y", + "sw_gyro_z", + "sw_lvel_x", + "sw_lvel_y", + "sw_lvel_z", + "sw_lacc_x", + "sw_lacc_y", + "sw_lacc_z", + "sw_pres", + "sw_grav_x", + "sw_grav_y", + "sw_grav_z", + "sw_forward_w", + "sw_forward_x", + "sw_forward_y", + "sw_forward_z", + "sw_init_pres" +] + +if not file.parent.exists(): + raise UserWarning(f"Directory does not exist {file.parent}") + +with open(file, 'w') as fd: + fd.write(",".join(header) + "\n") + + +def processing_loop(sensor_q: queue = None): + logging.info(f"[RAW_RECORDER] starting loop") + + # used to estimate delta time and processing speed in Hz + start = datetime.now() + dat = 0 + + # this loops while the socket is listening and/or receiving data + while active: + + # processing speed output + now = datetime.now() + if (now - start).seconds >= 5: + start = now + logging.info(f"[RAW_RECORDER] {dat / 5} Hz") + dat = 0 + + # get the most recent smartwatch data row from the queue + row = sensor_q.get() + while sensor_q.qsize() > 5: + row = sensor_q.get() + + msg = list(row) + with open(file, 'a') as fd: + msg.insert(0, datetime.now()) + msg = [str(x) for x in msg] + fd.write(",".join(msg) + "\n") + dat += 1 + + +recorder = threading.Thread( + target=processing_loop, + args=(q,) +) + +imu_w_l.start() +recorder.start() + + +def terminate_all(*args): + imu_l.terminate() + watch_rec.terminate() + + +# make sure all handler exit on termination +atexit.register(terminate_all) +signal.signal(signal.SIGTERM, terminate_all) +signal.signal(signal.SIGINT, terminate_all) diff --git a/src/wear_mocap_ape/config.py b/src/wear_mocap_ape/config.py index d4e8ff4..2fa6bb2 100644 --- a/src/wear_mocap_ape/config.py +++ b/src/wear_mocap_ape/config.py @@ -17,5 +17,6 @@ # experimental modes PORT_LISTEN_AUDIO = 65001 PORT_PUB_TRANSCRIBED_KEYS = 50006 + # PORT_LISTEN_WATCH_PHONE_IMU_RIGHT = 65003 # PORT_PUB_RIGHT_ARM = 50004 diff --git a/src/wear_mocap_ape/data_deploy/nn/7cb5cdf94ef4c66388c7f15f642005d5e008146a/checkpoint.pt b/src/wear_mocap_ape/data_deploy/nn/7cb5cdf94ef4c66388c7f15f642005d5e008146a/checkpoint.pt new file mode 100644 index 0000000..ea93a8f Binary files /dev/null and b/src/wear_mocap_ape/data_deploy/nn/7cb5cdf94ef4c66388c7f15f642005d5e008146a/checkpoint.pt differ diff --git a/src/wear_mocap_ape/data_deploy/nn/7cb5cdf94ef4c66388c7f15f642005d5e008146a/results.json b/src/wear_mocap_ape/data_deploy/nn/7cb5cdf94ef4c66388c7f15f642005d5e008146a/results.json new file mode 100644 index 0000000..404ff40 --- /dev/null +++ b/src/wear_mocap_ape/data_deploy/nn/7cb5cdf94ef4c66388c7f15f642005d5e008146a/results.json @@ -0,0 +1,76 @@ +{ + "model": "DropoutLSTM", + "hidden_layer_count": 3, + "hidden_layer_size": 128, + "epochs": 200, + "batch_size": 128, + "learning_rate": 0.0015, + "dropout": 0.2, + "sequence_len": 6, + "normalize": true, + "seq_overlap": true, + "create_norm_stats": true, + "early_stopping": 10, + "hash": "7cb5cdf94ef4c66388c7f15f642005d5e008146a", + "y_targets_n": "ORI_CAL_LARM_UARM", + "x_inputs_n": "WATCH_PHONE_CAL", + "y_targets_v": [ + "gt_larm_6drr_cal_1", + "gt_larm_6drr_cal_2", + "gt_larm_6drr_cal_3", + "gt_larm_6drr_cal_4", + "gt_larm_6drr_cal_5", + "gt_larm_6drr_cal_6", + "gt_uarm_6drr_cal_1", + "gt_uarm_6drr_cal_2", + "gt_uarm_6drr_cal_3", + "gt_uarm_6drr_cal_4", + "gt_uarm_6drr_cal_5", + "gt_uarm_6drr_cal_6" + ], + "x_inputs_v": [ + "sw_dt", + "sw_gyro_x", + "sw_gyro_y", + "sw_gyro_z", + "sw_lvel_x", + "sw_lvel_y", + "sw_lvel_z", + "sw_lacc_x", + "sw_lacc_y", + "sw_lacc_z", + "sw_grav_x", + "sw_grav_y", + "sw_grav_z", + "sw_6drr_cal_1", + "sw_6drr_cal_2", + "sw_6drr_cal_3", + "sw_6drr_cal_4", + "sw_6drr_cal_5", + "sw_6drr_cal_6", + "sw_pres_cal", + "ph_gyro_x", + "ph_gyro_y", + "ph_gyro_z", + "ph_lvel_x", + "ph_lvel_y", + "ph_lvel_z", + "ph_lacc_x", + "ph_lacc_y", + "ph_lacc_z", + "ph_grav_x", + "ph_grav_y", + "ph_grav_z", + "ph_6drr_cal_1", + "ph_6drr_cal_2", + "ph_6drr_cal_3", + "ph_6drr_cal_4", + "ph_6drr_cal_5", + "ph_6drr_cal_6" + ], + "datetime": "2024-02-02 14:17:35", + "Loss/train": 0.09550117918352286, + "Loss/test": 0.21292609498402634, + "Loss/b_train": 0.09740018165459235, + "Loss/b_test": 0.21205891988034561 +} \ No newline at end of file diff --git a/src/wear_mocap_ape/data_deploy/nn/7ffa47dfbf2e1a16057e0253a1c2ba01e465d55a/checkpoint.pt b/src/wear_mocap_ape/data_deploy/nn/7ffa47dfbf2e1a16057e0253a1c2ba01e465d55a/checkpoint.pt new file mode 100644 index 0000000..2f44dc7 Binary files /dev/null and b/src/wear_mocap_ape/data_deploy/nn/7ffa47dfbf2e1a16057e0253a1c2ba01e465d55a/checkpoint.pt differ diff --git a/src/wear_mocap_ape/data_deploy/nn/deploy_models.py b/src/wear_mocap_ape/data_deploy/nn/deploy_models.py index e482e70..bdf535f 100644 --- a/src/wear_mocap_ape/data_deploy/nn/deploy_models.py +++ b/src/wear_mocap_ape/data_deploy/nn/deploy_models.py @@ -2,6 +2,6 @@ class LSTM(Enum): - WATCH_PHONE_POCKET = "" - WATCH_PHONE_UARM = "" - WATCH_ONLY = "f99ed72bc0d1aeb27c533f1bd93010e0164fcde6" + WATCH_PHONE_POCKET = "7ffa47dfbf2e1a16057e0253a1c2ba01e465d55a" + WATCH_PHONE_UARM = "7cb5cdf94ef4c66388c7f15f642005d5e008146a" + WATCH_ONLY = "04f4ad63bfccb3668f7598c9375403e10b1fae2a"