From 98e0e79f3494c0649bdf971cc17391dfa3b07c13 Mon Sep 17 00:00:00 2001 From: Fabian Weigend Date: Tue, 10 Oct 2023 14:47:22 -0700 Subject: [PATCH 1/2] refactor kalman filter to established class organization --- example_scripts/watch_phone_pocket_stream.py | 20 +- example_scripts/watch_phone_uarm_record.py | 6 +- example_scripts/watch_phone_uarm_stream.py | 6 +- experimental_scripts/watch_phone_to_ros.py | 2 +- src/wear_mocap_ape/data_types/bone.py | 2 +- .../estimate/phone_uarm_free_hips_udp.py | 298 ------------- src/wear_mocap_ape/estimate/watch_only.py | 4 - .../watch_phone_pocket.py} | 395 +++++++----------- .../{watch_phone.py => watch_phone_uarm.py} | 13 +- src/wear_mocap_ape/record/watch_only_rec.py | 20 +- ...h_phone_rec.py => watch_phone_uarm_rec.py} | 24 +- .../publisher/watch_phone_pocket_udp.py | 39 ++ ...h_phone_ros.py => watch_phone_uarm_ros.py} | 4 +- ...h_phone_udp.py => watch_phone_uarm_udp.py} | 14 +- 14 files changed, 237 insertions(+), 610 deletions(-) delete mode 100644 src/wear_mocap_ape/estimate/phone_uarm_free_hips_udp.py rename src/wear_mocap_ape/{stream/publisher/kalman_pocket_phone_udp.py => estimate/watch_phone_pocket.py} (51%) rename src/wear_mocap_ape/estimate/{watch_phone.py => watch_phone_uarm.py} (95%) rename src/wear_mocap_ape/record/{watch_phone_rec.py => watch_phone_uarm_rec.py} (62%) create mode 100644 src/wear_mocap_ape/stream/publisher/watch_phone_pocket_udp.py rename src/wear_mocap_ape/stream/publisher/{watch_phone_ros.py => watch_phone_uarm_ros.py} (92%) rename src/wear_mocap_ape/stream/publisher/{watch_phone_udp.py => watch_phone_uarm_udp.py} (78%) diff --git a/example_scripts/watch_phone_pocket_stream.py b/example_scripts/watch_phone_pocket_stream.py index 78c8ed9..f9f72ca 100644 --- a/example_scripts/watch_phone_pocket_stream.py +++ b/example_scripts/watch_phone_pocket_stream.py @@ -8,7 +8,7 @@ from wear_mocap_ape import config from wear_mocap_ape.data_types import messaging from wear_mocap_ape.stream.listener.imu import ImuListener -from wear_mocap_ape.stream.publisher.kalman_pocket_phone_udp import KalmanPhonePocket +from wear_mocap_ape.stream.publisher.watch_phone_pocket_udp import WatchPhonePocketUDP if __name__ == "__main__": logging.basicConfig(level=logging.INFO) @@ -18,7 +18,8 @@ # Required IP argument parser.add_argument('ip', type=str, help=f'put your local IP here.') parser.add_argument('smooth', nargs='?', type=int, default=5, help=f'smooth predicted trajectories') - parser.add_argument('stream_mc', nargs='?', type=bool, default=True, help=f'whether you want to stream the full pose ensemble') + parser.add_argument('stream_mc', nargs='?', type=bool, default=True, + help=f'whether you want to stream the full pose ensemble') args = parser.parse_args() ip_arg = args.ip @@ -41,13 +42,13 @@ ) # process into arm pose and body orientation - kpp = KalmanPhonePocket(ip=ip_arg, - smooth=smooth_arg, - num_ensemble=48, - port=config.PORT_PUB_LEFT_ARM, - window_size=10, - stream_mc=stream_mc_arg, - model_name="SW-model-sept-4") + kpp = WatchPhonePocketUDP(ip=ip_arg, + smooth=smooth_arg, + num_ensemble=48, + port=config.PORT_PUB_LEFT_ARM, + window_size=10, + stream_mc=stream_mc_arg, + model_name="SW-model-sept-4") p_thread = threading.Thread( target=kpp.stream_wearable_devices, args=(left_q, True,) @@ -66,4 +67,3 @@ def terminate_all(*args): atexit.register(terminate_all) signal.signal(signal.SIGTERM, terminate_all) signal.signal(signal.SIGINT, terminate_all) - diff --git a/example_scripts/watch_phone_uarm_record.py b/example_scripts/watch_phone_uarm_record.py index 814add7..6d627f1 100644 --- a/example_scripts/watch_phone_uarm_record.py +++ b/example_scripts/watch_phone_uarm_record.py @@ -4,7 +4,7 @@ import signal import threading import wear_mocap_ape.config as config -from wear_mocap_ape.record.watch_phone_rec import WatchPhoneRecorder +from wear_mocap_ape.record.watch_phone_uarm_rec import WatchPhoneUarmRecorder from wear_mocap_ape.stream.listener.imu import ImuListener from wear_mocap_ape.data_types import messaging @@ -58,14 +58,14 @@ ) # left publisher -wp_rl = WatchPhoneRecorder(file=args.file) +wp_rl = WatchPhoneUarmRecorder(file=args.file) rl_thread = threading.Thread( target=wp_rl.stream_loop, args=(left_q,) ) # right publisher -wp_rr = WatchPhoneRecorder(file=args.file, left_hand_mode=False) +wp_rr = WatchPhoneUarmRecorder(file=args.file, left_hand_mode=False) rr_thread = threading.Thread( target=wp_rr.stream_loop, args=(right_q,) diff --git a/example_scripts/watch_phone_uarm_stream.py b/example_scripts/watch_phone_uarm_stream.py index 7f2d076..1fa84d8 100644 --- a/example_scripts/watch_phone_uarm_stream.py +++ b/example_scripts/watch_phone_uarm_stream.py @@ -7,7 +7,7 @@ import wear_mocap_ape.config as config from wear_mocap_ape.stream.listener.imu import ImuListener -from wear_mocap_ape.stream.publisher.watch_phone_udp import WatchPhoneUDP +from wear_mocap_ape.stream.publisher.watch_phone_uarm_udp import WatchPhoneUarmUDP from wear_mocap_ape.data_types import messaging # enable basic logging @@ -57,7 +57,7 @@ ) # left publisher -wp2ul = WatchPhoneUDP( +wp2ul = WatchPhoneUarmUDP( ip=ip, port=config.PORT_PUB_LEFT_ARM, tag="PUBLISH LEFT" @@ -68,7 +68,7 @@ ) # right publisher -wp2ur = WatchPhoneUDP( +wp2ur = WatchPhoneUarmUDP( ip=ip, port=config.PORT_PUB_RIGHT_ARM, tag="PUBLISH RIGHT", diff --git a/experimental_scripts/watch_phone_to_ros.py b/experimental_scripts/watch_phone_to_ros.py index 7d843f5..e343d36 100644 --- a/experimental_scripts/watch_phone_to_ros.py +++ b/experimental_scripts/watch_phone_to_ros.py @@ -4,7 +4,7 @@ import wear_mocap_ape.config as config from wear_mocap_ape.stream.listener.imu import ImuListener -from wear_mocap_ape.stream.publisher.watch_phone_ros import WatchPhoneROS +from wear_mocap_ape.stream.publisher.watch_phone_uarm_ros import WatchPhoneROS from wear_mocap_ape.data_types import messaging # parse command line arguments diff --git a/src/wear_mocap_ape/data_types/bone.py b/src/wear_mocap_ape/data_types/bone.py index ff89647..cc8a88b 100644 --- a/src/wear_mocap_ape/data_types/bone.py +++ b/src/wear_mocap_ape/data_types/bone.py @@ -14,4 +14,4 @@ def __init__(self, ): self.bone_id = bone_id # int self.default_pos = default_pos # position as vec3 - self.default_rot = default_rot # rotation as quaternion \ No newline at end of file + self.default_rot = default_rot # rotation as quaternion diff --git a/src/wear_mocap_ape/estimate/phone_uarm_free_hips_udp.py b/src/wear_mocap_ape/estimate/phone_uarm_free_hips_udp.py deleted file mode 100644 index 2a8f763..0000000 --- a/src/wear_mocap_ape/estimate/phone_uarm_free_hips_udp.py +++ /dev/null @@ -1,298 +0,0 @@ -import logging -import socket -import struct -import time -from datetime import datetime -import queue - -import numpy as np -import torch - -from wear_mocap_ape.data_types.bone_map import BoneMap -from wear_mocap_ape.estimate import models, estimate_joints -from wear_mocap_ape.utility import transformations as ts, data_stats -from wear_mocap_ape.data_types import messaging -from wear_mocap_ape.utility.names import NNS_TARGETS, NNS_INPUTS - - -class FreeHipsUarmUDP: - def __init__(self, - ip: str, - port: int, - model_hash: str, - smooth: int = 5, - stream_monte_carlo=True, - monte_carlo_samples=25, - bonemap: BoneMap = None, - tag: str = "PUB FREE HIPS"): - - self.__tag = tag - self.__port = port - self.__ip = ip - self.__stream_mc = stream_monte_carlo - self.__mc_samples = monte_carlo_samples - - # average over multiple time steps - self.__smooth = smooth - self.__smooth_hist = [] - self.__last_hip_pred = None - - # simple lookup for values of interest - self.__slp = messaging.WATCH_PHONE_IMU_LOOKUP - - self.__start = datetime.now() - self.__dat = 0 - self.__prev_t = datetime.now() - self.__row_hist = [] - - # use arm length measurements for predictions - if bonemap is None: - # default values - self.__larm_vec = np.array([-BoneMap.DEFAULT_LARM_LEN, 0, 0]) - self.__uarm_vec = np.array([-BoneMap.DEFAULT_UARM_LEN, 0, 0]) - self.__shou_orig = BoneMap.DEFAULT_L_SHOU_ORIG_RH - - else: - self.__larm_vec = bonemap.left_lower_arm_vec - self.__uarm_vec = bonemap.left_upper_arm_vec - self.__shou_orig = bonemap.left_upper_arm_origin_rh - - # for quicker access we store a matrix with relevant body measurements for quick multiplication - self.__body_measurements = np.repeat( - np.r_[self.__uarm_vec, self.__larm_vec, self.__shou_orig][np.newaxis, :], - self.__mc_samples * self.__smooth, - axis=0 - ) - - self.__udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - - # load the trained network - torch.set_default_dtype(torch.float64) - self.__device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') - - # load model from given parameters - self.__nn_model, params = models.load_deployed_model_from_hash(hash_str=model_hash) - self.__y_targets = NNS_TARGETS(params["y_targets_v"]) - self.__x_inputs = NNS_INPUTS(params["x_inputs_v"]) - self.__sequence_len = params["sequence_len"] - self.__normalize = params["normalize"] - - # load normalized data stats if required - if self.__normalize: - # data is normalized and has to be transformed with pre-calculated mean and std - stats = data_stats.get_norm_stats(x_inputs=self.__x_inputs, y_targets=self.__y_targets) - self.__xx_m, self.__xx_s = stats["xx_m"], stats["xx_s"] - self.__yy_m, self.__yy_s = stats["yy_m"], stats["yy_s"] - else: - self.__xx_m, self.__xx_s = 0., 1. - self.__yy_m, self.__yy_s = 0., 1. - - def row_to_pose(self, row): - - # second-wise updates to determine message frequency - now = datetime.now() - if (now - self.__start).seconds >= 5: - self.__start = now - logging.info(f"[{self.__tag}] {self.__dat / 5} Hz") - self.__dat = 0 - delta_t = now - self.__prev_t - delta_t = delta_t.microseconds * 0.000001 - self.__prev_t = now - - # process watch and phone data where needed - ph_rot = np.array([ - row[self.__slp["ph_rotvec_w"]], row[self.__slp["ph_rotvec_x"]], - row[self.__slp["ph_rotvec_y"]], row[self.__slp["ph_rotvec_z"]] - ]) - ph_rot_g = ts.android_quat_to_global_no_north(ph_rot) - ph_rot_6drr = ts.quat_to_6drr_1x6(ph_rot_g) - - sw_rot = np.array([ - row[self.__slp["sw_rotvec_w"]], row[self.__slp["sw_rotvec_x"]], - row[self.__slp["sw_rotvec_y"]], row[self.__slp["sw_rotvec_z"]] - ]) - sw_rot_g = ts.android_quat_to_global_no_north(sw_rot) - sw_rot_6drr = ts.quat_to_6drr_1x6(sw_rot_g) - - # pressure - calibrated initial pressure = relative pressure - r_pres = row[self.__slp["sw_pres"]] - row[self.__slp["sw_init_pres"]] - - if self.__last_hip_pred is not None: - gt_hips_yrot_cal_sin_tm1 = self.__last_hip_pred[0] - gt_hips_yrot_cal_cos_tm1 = self.__last_hip_pred[1] - else: - y_rot = ts.reduce_global_quat_to_y_rot(sw_rot_g) - gt_hips_yrot_cal_sin_tm1 = np.sin(y_rot) - gt_hips_yrot_cal_cos_tm1 = np.cos(y_rot) - - # assemble the entire input vector of one time step - xx = np.hstack([ - delta_t, - row[self.__slp["sw_gyro_x"]], row[self.__slp["sw_gyro_y"]], row[self.__slp["sw_gyro_z"]], - row[self.__slp["sw_lvel_x"]], row[self.__slp["sw_lvel_y"]], row[self.__slp["sw_lvel_z"]], - row[self.__slp["sw_lacc_x"]], row[self.__slp["sw_lacc_y"]], row[self.__slp["sw_lacc_z"]], - row[self.__slp["sw_grav_x"]], row[self.__slp["sw_grav_y"]], row[self.__slp["sw_grav_z"]], - sw_rot_6drr, - r_pres, - row[self.__slp["ph_gyro_x"]], row[self.__slp["ph_gyro_y"]], row[self.__slp["ph_gyro_z"]], - row[self.__slp["ph_lvel_x"]], row[self.__slp["ph_lvel_y"]], row[self.__slp["ph_lvel_z"]], - row[self.__slp["ph_lacc_x"]], row[self.__slp["ph_lacc_y"]], row[self.__slp["ph_lacc_z"]], - row[self.__slp["ph_grav_x"]], row[self.__slp["ph_grav_y"]], row[self.__slp["ph_grav_z"]], - ph_rot_6drr, - gt_hips_yrot_cal_sin_tm1, - gt_hips_yrot_cal_cos_tm1 - ]) - - if self.__normalize: - # normalize measurements with pre-calculated mean and std - xx = (xx - self.__xx_m) / self.__xx_s - - # sequences are used for recurrent nets. Stack time steps along 2nd axis - self.__row_hist.append(xx) - if len(self.__row_hist) < self.__sequence_len: - return None - - while len(self.__row_hist) > self.__sequence_len: - del self.__row_hist[0] - xx = np.vstack(self.__row_hist) - - # finally, cast to a torch tensor with batch size 1 - xx = torch.tensor(xx[None, :, :]) - with torch.no_grad(): - # make mote carlo predictions if the model makes use of dropout - t_preds = self.__nn_model.monte_carlo_predictions(x=xx, n_samples=self.__mc_samples) - - # if on GPU copy the tensor to host memory first - if self.__device.type == 'cuda': - t_preds = t_preds.cpu() - t_preds = t_preds.numpy() - - # we are only interested in the last prediction of the sequence - t_preds = t_preds[:, -1, :] - - if self.__normalize: - # transform predictions back from normalized labels - t_preds = t_preds * self.__yy_s + self.__yy_m - - # store t_preds in history if smoothing is required - if self.__smooth > 1: - self.__smooth_hist.append(t_preds) - if len(self.__smooth_hist) < self.__smooth: - return None - - while len(self.__smooth_hist) > self.__smooth: - del self.__smooth_hist[0] - - t_preds = np.vstack(self.__smooth_hist) - - # finally, estimate hand and lower arm origins from prediction data - est = estimate_joints.arm_pose_from_nn_targets( - preds=t_preds, - body_measurements=self.__body_measurements, - y_targets=self.__y_targets - ) - - self.__last_hip_pred = [np.mean(t_preds[:, 12]), np.mean(t_preds[:, 13])] - - # estimate mean of rotations if we got multiple MC predictions - if est.shape[0] > 1: - # Calculate the mean of all predictions mean - p_larm_quat_g = ts.average_quaternions(est[:, 9:13]) - p_uarm_quat_g = ts.average_quaternions(est[:, 13:17]) - p_hips_quat_g = ts.average_quaternions(est[:, 17:]) - - # get the transition from upper arm origin to lower arm origin - p_uarm_orig_rh = ts.quat_rotate_vector(p_hips_quat_g, self.__shou_orig) - p_larm_orig_rh = ts.quat_rotate_vector(p_uarm_quat_g, self.__uarm_vec) + p_uarm_orig_rh - p_hand_orig_rh = ts.quat_rotate_vector(p_larm_quat_g, self.__larm_vec) + p_larm_orig_rh - - else: - p_hand_orig_rh = est[0, 0:3] - p_larm_orig_rh = est[0, 3:6] - p_uarm_orig_rh = est[0, 6:9] - p_larm_quat_g = est[0, 9:13] - p_uarm_quat_g = est[0, 13:17] - p_hips_quat_g = est[0, 17:] - - # this is the list for the actual joint positions and rotations - msg = [ - # hand - p_larm_quat_g[0], - p_larm_quat_g[1], - p_larm_quat_g[2], - p_larm_quat_g[3], - - p_hand_orig_rh[0], - p_hand_orig_rh[1], - p_hand_orig_rh[2], - - # larm - p_larm_quat_g[0], - p_larm_quat_g[1], - p_larm_quat_g[2], - p_larm_quat_g[3], - - p_larm_orig_rh[0], - p_larm_orig_rh[1], - p_larm_orig_rh[2], - - # uarm - p_uarm_quat_g[0], - p_uarm_quat_g[1], - p_uarm_quat_g[2], - p_uarm_quat_g[3], - - p_uarm_orig_rh[0], - p_uarm_orig_rh[1], - p_uarm_orig_rh[2], - - # hips - p_hips_quat_g[0], - p_hips_quat_g[1], - p_hips_quat_g[2], - p_hips_quat_g[3] - ] - - # now we attach the monte carlo predictions for XYZ positions - if self.__stream_mc: - if est.shape[0] > 1: - for e_row in est: - msg += list(e_row[:9]) - - return msg - - def stream_loop(self, sensor_q: queue): - - logging.info("[{}] starting Unity stream loop".format(self.__tag)) - - # used to estimate delta time and processing speed in Hz - self.__start = datetime.now() - self.__dat = 0 - self.__prev_t = datetime.now() - - # this loops while the socket is listening and/or receiving data - while True: - # get the most recent smartwatch data row from the queue - row = sensor_q.get() - - while sensor_q.qsize() > 5: - row = sensor_q.get() - - # process received data - if row is not None: - - # get message as numpy array - np_msg = self.row_to_pose(row) - # can return None if not enough historical data for smoothing is available - if np_msg is None: - continue - - # send message to Unity - self.send_np_msg(msg=np_msg) - self.__dat += 1 - time.sleep(0.01) - - def send_np_msg(self, msg: np.array) -> int: - # craft UDP message and send - msg = struct.pack('f' * len(msg), *msg) - return self.__udp_socket.sendto(msg, (self.__ip, self.__port)) diff --git a/src/wear_mocap_ape/estimate/watch_only.py b/src/wear_mocap_ape/estimate/watch_only.py index 1f3f1a5..b9e5cb5 100644 --- a/src/wear_mocap_ape/estimate/watch_only.py +++ b/src/wear_mocap_ape/estimate/watch_only.py @@ -1,15 +1,11 @@ import logging -import pickle -import struct from abc import abstractmethod from datetime import datetime -from pathlib import Path import torch import queue import numpy as np -import wear_mocap_ape.config as config from wear_mocap_ape.data_deploy.nn import deploy_models from wear_mocap_ape.estimate import estimate_joints, models from wear_mocap_ape.data_types.bone_map import BoneMap diff --git a/src/wear_mocap_ape/stream/publisher/kalman_pocket_phone_udp.py b/src/wear_mocap_ape/estimate/watch_phone_pocket.py similarity index 51% rename from src/wear_mocap_ape/stream/publisher/kalman_pocket_phone_udp.py rename to src/wear_mocap_ape/estimate/watch_phone_pocket.py index 813b124..8f96b1d 100644 --- a/src/wear_mocap_ape/stream/publisher/kalman_pocket_phone_udp.py +++ b/src/wear_mocap_ape/estimate/watch_phone_pocket.py @@ -1,7 +1,5 @@ import logging -import socket -import struct -import threading +from abc import abstractmethod from datetime import datetime from pathlib import Path @@ -16,16 +14,13 @@ from wear_mocap_ape import config from wear_mocap_ape.estimate import estimate_joints from wear_mocap_ape.data_types.bone_map import BoneMap -from wear_mocap_ape.stream.listener.imu import ImuListener from wear_mocap_ape.utility import transformations as ts, data_stats from wear_mocap_ape.data_types import messaging from wear_mocap_ape.utility.names import NNS_TARGETS, NNS_INPUTS -class KalmanPhonePocket: +class WatchPhonePocket: def __init__(self, - ip, - port, smooth: int = 1, num_ensemble: int = 32, model_name="SW-model", @@ -42,31 +37,22 @@ def __init__(self, self.__smooth = smooth self.__smooth_hist = [] self.__stream_mc = stream_mc + self.__slp = messaging.WATCH_PHONE_IMU_LOOKUP self.__row_hist = [] self.__uarm_vec = np.array([-BoneMap.DEFAULT_UARM_LEN, 0, 0]) self.__larm_vec = np.array([-BoneMap.DEFAULT_LARM_LEN, 0, 0]) self.__shou_orig = BoneMap.DEFAULT_L_SHOU_ORIG_RH - - self.__port = port - self.__ip = ip - self.__udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - self.__udp_socket.settimeout(5) self.last_msg = None self.__normalize = True self.batch_size = 1 self.dim_x = 14 self.dim_z = 14 - self.dim_a = "" self.dim_gt = 14 self.sensor_len = 1 - self.channel_img_1 = "" - self.channel_img_2 = "" self.input_size_1 = 22 - self.input_size_2 = "" - self.input_size_3 = "" self.num_ensemble = num_ensemble self.win_size = window_size self.mode = "Test" @@ -113,26 +99,9 @@ def __init__(self, self.__body_measurements = np.r_[self.__larm_vec, self.__uarm_vec, self.__shou_orig][np.newaxis, :] self.__init_step = 0 - self.__larm_vert = np.array([ - [-BoneMap.DEFAULT_LARM_LEN, 0.03, 0.03], - [-BoneMap.DEFAULT_LARM_LEN, 0.03, -0.03], - [-BoneMap.DEFAULT_LARM_LEN, -0.03, -0.03], - [-BoneMap.DEFAULT_LARM_LEN, -0.03, 0.03], - [0, 0.03, 0.03], - [0, 0.03, -0.03], - [0, -0.03, -0.03], - [0, -0.03, 0.03], - ]) - self.__uarm_vert = np.array([ - [-BoneMap.DEFAULT_UARM_LEN, 0.05, 0.05], - [-BoneMap.DEFAULT_UARM_LEN, 0.05, -0.05], - [-BoneMap.DEFAULT_UARM_LEN, -0.05, -0.05], - [-BoneMap.DEFAULT_UARM_LEN, -0.05, 0.05], - [0, 0.05, 0.05], - [0, 0.05, -0.05], - [0, -0.05, -0.05], - [0, -0.05, 0.05], - ]) + @property + def sequence_len(self): + return self.win_size def is_active(self): return self.__active @@ -146,7 +115,7 @@ def get_y_targets(self): def get_body_measurements(self): return self.__body_measurements - def errors_from_file(self, file_p: Path, publish: bool = False): + def errors_from_file(self, file_p: Path, process_msg: bool = False): logging.info(f"[{self.__tag}] processing from file") # init some dummy input input_state = np.zeros((self.batch_size, self.num_ensemble, self.win_size, self.dim_x)) @@ -161,7 +130,7 @@ def errors_from_file(self, file_p: Path, publish: bool = False): for i, row in dat.iterrows(): ## PREDICTIONS xx = row[self.x_inputs.value].to_numpy() - t_pred, input_state = self.make_prediction(xx, input_state) + t_pred, input_state = self.add_obs_and_make_prediction(xx, input_state) est = estimate_joints.arm_pose_from_nn_targets( preds=t_pred, @@ -188,7 +157,7 @@ def errors_from_file(self, file_p: Path, publish: bool = False): p_hips_quat_rh = est[0, 17:] # publish the estimation for our unity visualization - if publish: + if process_msg: msg = np.hstack([ p_larm_quat_rh, p_hand_orig_rh, @@ -201,7 +170,7 @@ def errors_from_file(self, file_p: Path, publish: bool = False): if est.shape[0] > 1: for e_row in est: msg = np.hstack([msg, e_row[:9]]) - self.send_np_msg(msg) + self.process_msg(msg) # GROUND TRUTH yy = row[self.__y_targets.value].to_numpy()[np.newaxis, :] @@ -217,34 +186,6 @@ def errors_from_file(self, file_p: Path, publish: bool = False): gt_uarm_quat_g = est[0, 13:17] gt_hips_quat_g = est[0, 17:] - # p_larm_vrtx = ts.quat_rotate_vector(p_larm_quat_rh, self.__larm_vert) + p_larm_orig_rh - # p_uarm_vrtx = ts.quat_rotate_vector(p_uarm_quat_rh, self.__uarm_vert) + p_uarm_orig_rh - # - # gt_larm_vrtx = ts.quat_rotate_vector(gt_larm_quat_g, self.__larm_vert) + gt_larm_orig_rh - # gt_uarm_vrtx = ts.quat_rotate_vector(gt_uarm_quat_g, self.__uarm_vert) + gt_uarm_orig_rh - # - # le = gt_larm_vrtx - p_larm_vrtx - # ue = gt_uarm_vrtx - p_uarm_vrtx - # ae = np.vstack([le, ue]) - # me = np.linalg.norm(ae, axis=1) - # - # mpjve.append(np.mean(me) * 100) - - # hand_l_err.append(np.linalg.norm(gt_hand_orig_rh - p_hand_orig_rh) * 100) - # q = ts.hamilton_product(gt_larm_quat_g, ts.quat_invert(p_larm_quat_rh)) - # e = ts.quat_to_euler(q) - # hand_r_err.append(np.sum(np.abs(np.degrees(e)))) - # - # elbow_l_err.append(np.linalg.norm(gt_larm_orig_rh - p_larm_orig_rh) * 100) - # eq = ts.hamilton_product(gt_uarm_quat_g, ts.quat_invert(p_uarm_quat_rh)) - # ee = ts.quat_to_euler(eq) - # elbow_r_err.append(np.sum(np.abs(np.degrees(ee)))) - # - # ydiff = ts.hamilton_product(gt_hips_quat_g, ts.quat_invert(p_hips_quat_rh)) - # ydiffe = ts.quat_to_euler(ydiff)[1] - # err = np.abs(np.degrees(np.arctan2(np.sin(ydiffe), np.cos(ydiffe)))) - # hip_rot_errors.append(err) - hand_l_err.append(np.linalg.norm(gt_hand_orig_rh - p_hand_orig_rh) * 100) hre = ts.quat_to_euler(gt_larm_quat_g) - ts.quat_to_euler(p_larm_quat_rh) hree = np.sum(np.abs(np.degrees(np.arctan2(np.sin(hre), np.cos(hre))))) @@ -259,22 +200,121 @@ def errors_from_file(self, file_p: Path, publish: bool = False): err = np.abs(np.degrees(np.arctan2(np.sin(ydiff), np.cos(ydiff)))) hip_rot_errors.append(err) - if (int(i) + 1) % 100 == 0: logging.info(f"[{self.__tag}] processed {i} rows") return hand_l_err, hand_r_err, elbow_l_err, elbow_r_err, hip_rot_errors - def stream_wearable_devices(self, sensor_q: queue = None, send_msg: bool = True): + def parse_row_to_xx(self, row): + # process the data + # pressure - calibrated initial pressure = relative pressure + r_pres = row[self.__slp["sw_pres"]] - row[self.__slp["sw_init_pres"]] + # calibrate smartwatch rotation + sw_rot = np.array([ + row[self.__slp["sw_rotvec_w"]], + row[self.__slp["sw_rotvec_x"]], + row[self.__slp["sw_rotvec_y"]], + row[self.__slp["sw_rotvec_z"]] + ]) + sw_fwd = np.array([ + row[self.__slp["sw_forward_w"]], + row[self.__slp["sw_forward_x"]], + row[self.__slp["sw_forward_y"]], + row[self.__slp["sw_forward_z"]] + ]) + + # estimate north quat to align Z-axis of global and android coord system + r = ts.android_quat_to_global_no_north(sw_fwd) + y_rot = ts.reduce_global_quat_to_y_rot(r) + quat_north = ts.euler_to_quat(np.array([0, -y_rot, 0])) + # calibrate watch + sw_cal_g = ts.android_quat_to_global(sw_rot, quat_north) + sw_6drr_cal = ts.quat_to_6drr_1x6(sw_cal_g) + + ph_fwd = np.array([ + row[self.__slp["ph_forward_w"]], row[self.__slp["ph_forward_x"]], + row[self.__slp["ph_forward_y"]], row[self.__slp["ph_forward_z"]] + ]) + ph_rot = np.array([ + row[self.__slp["ph_rotvec_w"]], row[self.__slp["ph_rotvec_x"]], + row[self.__slp["ph_rotvec_y"]], row[self.__slp["ph_rotvec_z"]] + ]) + # the device orientations if the calib position with left arm forward is perfect + hips_dst_g = np.array([1, 0, 0, 0]) + ph_rot_g = ts.android_quat_to_global(ph_rot, quat_north) + ph_fwd_g = ts.android_quat_to_global(ph_fwd, quat_north) + ph_off_g = ts.hamilton_product(ts.quat_invert(ph_fwd_g), hips_dst_g) + ph_cal_g = ts.hamilton_product(ph_rot_g, ph_off_g) + + # hip y rotation from phone + hips_y_rot = ts.reduce_global_quat_to_y_rot(ph_cal_g) + hips_yrot_cal_sin = np.sin(hips_y_rot) + hips_yrot_cal_cos = np.cos(hips_y_rot) + + # assemble the entire input vector of one time step + return np.hstack([ + row[self.__slp["sw_dt"]], + row[self.__slp["sw_gyro_x"]], row[self.__slp["sw_gyro_y"]], row[self.__slp["sw_gyro_z"]], + row[self.__slp["sw_lvel_x"]], row[self.__slp["sw_lvel_y"]], row[self.__slp["sw_lvel_z"]], + row[self.__slp["sw_lacc_x"]], row[self.__slp["sw_lacc_y"]], row[self.__slp["sw_lacc_z"]], + row[self.__slp["sw_grav_x"]], row[self.__slp["sw_grav_y"]], row[self.__slp["sw_grav_z"]], + sw_6drr_cal, + r_pres, + hips_yrot_cal_sin, + hips_yrot_cal_cos + ]) + + def msg_from_pred(self, pred): + est = estimate_joints.arm_pose_from_nn_targets( + preds=pred, + body_measurements=self.__body_measurements, + y_targets=self.__y_targets + ) + + # estimate mean of rotations if we got multiple MC predictions + if est.shape[0] > 1: + # Calculate the mean of all predictions mean + p_hips_quat_g = ts.average_quaternions(est[:, 17:]) + p_larm_quat_g = ts.average_quaternions(est[:, 9:13]) + p_uarm_quat_g = ts.average_quaternions(est[:, 13:17]) + + # get the transition from upper arm origin to lower arm origin + p_uarm_orig_g = ts.quat_rotate_vector(p_hips_quat_g, self.__shou_orig) + p_larm_orig_g = ts.quat_rotate_vector(p_uarm_quat_g, self.__uarm_vec) + p_uarm_orig_g + p_hand_orig_g = ts.quat_rotate_vector(p_larm_quat_g, self.__larm_vec) + p_larm_orig_g + else: + p_hand_orig_g = est[0, 0:3] + p_larm_orig_g = est[0, 3:6] + p_uarm_orig_g = est[0, 6:9] + p_larm_quat_g = est[0, 9:13] + p_uarm_quat_g = est[0, 13:17] + p_hips_quat_g = est[0, 17:] + + # this is the list for the actual joint positions and rotations + msg = np.hstack([ + p_larm_quat_g, # hand rot + p_hand_orig_g, # hand orig + p_larm_quat_g, # larm rot + p_larm_orig_g, # larm orig + p_uarm_quat_g, # uarm rot + p_uarm_orig_g, # uarm orig + p_hips_quat_g # hips rot + ]) + # store as last msg for getter + self.last_msg = msg.copy() + + if self.__stream_mc: + msg = list(msg) + # now we attach the monte carlo predictions for XYZ positions + if est.shape[0] > 1: + for e_row in est: + msg += list(e_row[:6]) + return msg + + def stream_wearable_devices(self, sensor_q: queue = None, process_msg: bool = True): logging.info(f"[{self.__tag}] wearable streaming loop") self.__init_step = 0 - # init some dummy input - # input -> [batch_size, ensemble, timestep, dim_x] - input_state = np.zeros((self.batch_size, self.num_ensemble, self.win_size, self.dim_x)) - input_state = torch.tensor(input_state, dtype=torch.float32) - input_state = input_state.to(self.device) - # used to estimate delta time and processing speed in Hz start = datetime.now() self.__prev_time = start @@ -283,11 +323,8 @@ def stream_wearable_devices(self, sensor_q: queue = None, send_msg: bool = True) # this loops while the socket is listening and/or receiving data self.__active = True - # dataset = pickle.load(open(dojo_config.PATHS["deploy"] / "test_dataset.pkl", "rb")) - # simple lookup for values of interest - slp = messaging.WATCH_PHONE_IMU_LOOKUP - logging.info("loaded") + input_state = None while self.__active: # processing speed output @@ -296,8 +333,6 @@ def stream_wearable_devices(self, sensor_q: queue = None, send_msg: bool = True) start = now logging.info(f"[{self.__tag}] {dat / 5} Hz") dat = 0 - delta_t = now - self.__prev_time - delta_t = delta_t.microseconds * 0.000001 self.__prev_time = now # get the most recent smartwatch data row from the queue @@ -305,142 +340,30 @@ def stream_wearable_devices(self, sensor_q: queue = None, send_msg: bool = True) while sensor_q.qsize() > 5: row = sensor_q.get() - # process the data - # pressure - calibrated initial pressure = relative pressure - r_pres = row[slp["sw_pres"]] - row[slp["sw_init_pres"]] - # calibrate smartwatch rotation - sw_rot = np.array([ - row[slp["sw_rotvec_w"]], - row[slp["sw_rotvec_x"]], - row[slp["sw_rotvec_y"]], - row[slp["sw_rotvec_z"]] - ]) - sw_fwd = np.array([ - row[slp["sw_forward_w"]], - row[slp["sw_forward_x"]], - row[slp["sw_forward_y"]], - row[slp["sw_forward_z"]] - ]) - - # estimate north quat to align Z-axis of global and android coord system - r = ts.android_quat_to_global_no_north(sw_fwd) - y_rot = ts.reduce_global_quat_to_y_rot(r) - quat_north = ts.euler_to_quat(np.array([0, -y_rot, 0])) - # calibrate watch - sw_cal_g = ts.android_quat_to_global(sw_rot, quat_north) - sw_6drr_cal = ts.quat_to_6drr_1x6(sw_cal_g) - - ph_fwd = np.array([ - row[slp["ph_forward_w"]], row[slp["ph_forward_x"]], - row[slp["ph_forward_y"]], row[slp["ph_forward_z"]] - ]) - ph_rot = np.array([ - row[slp["ph_rotvec_w"]], row[slp["ph_rotvec_x"]], - row[slp["ph_rotvec_y"]], row[slp["ph_rotvec_z"]] - ]) - # the device orientations if the calib position with left arm forward is perfect - hips_dst_g = np.array([1, 0, 0, 0]) - ph_rot_g = ts.android_quat_to_global(ph_rot, quat_north) - ph_fwd_g = ts.android_quat_to_global(ph_fwd, quat_north) - ph_off_g = ts.hamilton_product(ts.quat_invert(ph_fwd_g), hips_dst_g) - ph_cal_g = ts.hamilton_product(ph_rot_g, ph_off_g) - - # hip y rotation from phone - hips_y_rot = ts.reduce_global_quat_to_y_rot(ph_cal_g) - hips_yrot_cal_sin = np.sin(hips_y_rot) - hips_yrot_cal_cos = np.cos(hips_y_rot) - - # assemble the entire input vector of one time step - xx = np.hstack([ - delta_t, - row[slp["sw_gyro_x"]], row[slp["sw_gyro_y"]], row[slp["sw_gyro_z"]], - row[slp["sw_lvel_x"]], row[slp["sw_lvel_y"]], row[slp["sw_lvel_z"]], - row[slp["sw_lacc_x"]], row[slp["sw_lacc_y"]], row[slp["sw_lacc_z"]], - row[slp["sw_grav_x"]], row[slp["sw_grav_y"]], row[slp["sw_grav_z"]], - sw_6drr_cal, - r_pres, - hips_yrot_cal_sin, - hips_yrot_cal_cos - ]) - - # normalize measurements with pre-calculated mean and std - # finally get predicted positions etc - t_pred, input_state = self.make_prediction(xx, input_state) - - est = estimate_joints.arm_pose_from_nn_targets( - preds=t_pred, - body_measurements=self.__body_measurements, - y_targets=self.__y_targets - ) - - # estimate mean of rotations if we got multiple MC predictions - if est.shape[0] > 1: - # Calculate the mean of all predictions mean - p_hips_quat_g = ts.average_quaternions(est[:, 17:]) - p_larm_quat_g = ts.average_quaternions(est[:, 9:13]) - p_uarm_quat_g = ts.average_quaternions(est[:, 13:17]) - - # get the transition from upper arm origin to lower arm origin - p_uarm_orig_g = ts.quat_rotate_vector(p_hips_quat_g, self.__shou_orig) - p_larm_orig_g = ts.quat_rotate_vector(p_uarm_quat_g, self.__uarm_vec) + p_uarm_orig_g - p_hand_orig_g = ts.quat_rotate_vector(p_larm_quat_g, self.__larm_vec) + p_larm_orig_g - else: - p_hand_orig_g = est[0, 0:3] - p_larm_orig_g = est[0, 3:6] - p_uarm_orig_g = est[0, 6:9] - p_larm_quat_g = est[0, 9:13] - p_uarm_quat_g = est[0, 13:17] - p_hips_quat_g = est[0, 17:] - - # this is the list for the actual joint positions and rotations - msg = np.hstack([ - p_larm_quat_g, # hand rot - p_hand_orig_g, # hand orig - p_larm_quat_g, # larm rot - p_larm_orig_g, # larm orig - p_uarm_quat_g, # uarm rot - p_uarm_orig_g, # uarm orig - p_hips_quat_g # hips rot - ]) - # store as last msg for getter - self.last_msg = msg.copy() - - if self.__stream_mc: - msg = list(msg) - # now we attach the monte carlo predictions for XYZ positions - if est.shape[0] > 1: - for e_row in est: - msg += list(e_row[:6]) - - if send_msg: - self.send_np_msg(msg) + xx = self.parse_row_to_xx(row) + t_pred, input_state = self.add_obs_and_make_prediction(xx, input_state) + msg = self.msg_from_pred(t_pred) + if process_msg: + self.process_msg(msg) dat += 1 - def send_np_msg(self, msg: np.array) -> int: - # craft UDP message and send - msg = struct.pack('f' * len(msg), *msg) - try: - return self.__udp_socket.sendto(msg, (self.__ip, self.__port)) - except TimeoutError: - logging.info(f"[{self.__tag}] timed out") - return -1 + @abstractmethod + def process_msg(self, msg: np.array): + return - def make_prediction(self, xx, input_state): + def make_prediction_from_row_hist(self, xx, input_state=None): + + if input_state is None: + # init some dummy input -> [batch_size, ensemble, timestep, dim_x] + input_state = np.zeros((self.batch_size, self.num_ensemble, self.win_size, self.dim_x)) + input_state = torch.tensor(input_state, dtype=torch.float32) + input_state = input_state.to(self.device) if self.__normalize: xx = (xx - self.__xx_m) / self.__xx_s - self.__row_hist.append(xx) - # if not enough data is available yet, simply repeat the input as a first estimate - while len(self.__row_hist) < self.win_size: - self.__row_hist.append(xx) - # if the history is too long, delete old values - while len(self.__row_hist) > self.win_size: - del self.__row_hist[0] - - xx_seq = np.vstack(self.__row_hist) # seq x feat - xx_seq = rearrange(xx_seq, "(bs seq) (en feat) -> bs seq en feat", bs=1, en=1) + xx_seq = rearrange(xx, "(bs seq) (en feat) -> bs seq en feat", bs=1, en=1) xx_seq = torch.tensor(xx_seq, dtype=torch.float32).to(self.device) with torch.no_grad(): @@ -468,7 +391,6 @@ def make_prediction(self, xx, input_state): return smp, input_state # return only sensor model prediction ensemble = output[0] # -> output ensemble - learned_obs = output[3] # -> learned observations ensemble_ = rearrange(ensemble, "bs (en k) dim -> bs en k dim", k=1) input_state = torch.cat( (input_state[:, :, 1:, :], ensemble_), axis=2 @@ -478,11 +400,9 @@ def make_prediction(self, xx, input_state): # if on GPU copy the tensor to host memory first if self.device.type == 'cuda': ensemble = ensemble.cpu() - # learned_obs = learned_obs.cpu() # get the output t_preds = ensemble.detach().numpy()[0] - # t_preds = learned_obs.detach().numpy()[0] if self.__normalize: t_preds = t_preds * self.__yy_s + self.__yy_m @@ -496,37 +416,16 @@ def make_prediction(self, xx, input_state): del self.__smooth_hist[0] t_preds = np.vstack(self.__smooth_hist) - return t_preds, input_state # return entire ensemble - - -def run_kalman(ip: str, ensembles=64, smooth: int = 4, publish_to_unity = False) -> KalmanPhonePocket: - # data for left-hand mode - left_q = queue.Queue() - - # listen for imu data from phone and watch - imu_l = ImuListener( - ip=ip, - msg_size=messaging.watch_phone_imu_msg_len, - port=config.PORT_LISTEN_WATCH_PHONE_IMU_LEFT, - tag="LISTEN IMU LEFT" - ) - l_thread = threading.Thread( - target=imu_l.listen, - args=(left_q,) - ) - l_thread.start() - - kpp = KalmanPhonePocket(ip=ip, - smooth=smooth, - num_ensemble=ensembles, - port=config.PORT_PUB_LEFT_ARM, - window_size=5, - stream_mc=False, # False - model_name="SW-model") - p_thread = threading.Thread( - target=kpp.stream_wearable_devices, - args=(left_q, publish_to_unity,) - ) - p_thread.start() - - return kpp + return t_preds, input_state + + def add_obs_and_make_prediction(self, x, input_state=None): + self.__row_hist.append(x) + # if not enough data is available yet, simply repeat the input as a first estimate + while len(self.__row_hist) < self.win_size: + self.__row_hist.append(x) + # if the history is too long, delete old values + while len(self.__row_hist) > self.win_size: + del self.__row_hist[0] + + t_preds, input_state = self.make_prediction_from_row_hist(np.vstack(self.__row_hist), input_state) + return t_preds, input_state diff --git a/src/wear_mocap_ape/estimate/watch_phone.py b/src/wear_mocap_ape/estimate/watch_phone_uarm.py similarity index 95% rename from src/wear_mocap_ape/estimate/watch_phone.py rename to src/wear_mocap_ape/estimate/watch_phone_uarm.py index da06628..d1ccfe7 100644 --- a/src/wear_mocap_ape/estimate/watch_phone.py +++ b/src/wear_mocap_ape/estimate/watch_phone_uarm.py @@ -10,11 +10,11 @@ from wear_mocap_ape.data_types import messaging -class WatchPhone: +class WatchPhoneUarm: def __init__(self, smooth: int = 5, left_hand_mode=True, - tag: str = "PUB WATCH PHONE", + tag: str = "PUB WATCH PHONE UARM", bonemap: BoneMap = None): self.__active = True @@ -44,6 +44,7 @@ def __init__(self, if left_hand_mode: self.__larm_vec[0] = -self.__larm_vec[0] self.__uarm_vec[0] = -self.__uarm_vec[0] + else: self.__uarm_orig = self.__uarm_orig * np.array([-1, 1, 1]) # invert x def calibrate_imus_with_offset(self, row: np.array): @@ -118,16 +119,16 @@ def row_to_arm_pose(self, row): # get the transition from upper arm origin to lower arm origin # get transitions from lower arm origin to hand - larm_origin_rua = ts.quat_rotate_vector(avg_uarm_rot_r, self.__uarm_vec) + larm_origin_rh = ts.quat_rotate_vector(avg_uarm_rot_r, self.__uarm_vec) + self.__uarm_orig larm_rotated = ts.quat_rotate_vector(avg_larm_rot_r, self.__larm_vec) - hand_origin_rua = larm_rotated + larm_origin_rua + hand_origin_rh = larm_rotated + larm_origin_rh # this is the list for the actual joint positions and rotations return np.hstack([ avg_larm_rot_r, # hand quat - hand_origin_rua, # hand orig + hand_origin_rh, # hand orig avg_larm_rot_r, # larm quat - larm_origin_rua, # larm orig + larm_origin_rh, # larm orig avg_uarm_rot_r, # uarm quat self.__uarm_orig, self.__hips_quat diff --git a/src/wear_mocap_ape/record/watch_only_rec.py b/src/wear_mocap_ape/record/watch_only_rec.py index 46c7700..82885d0 100644 --- a/src/wear_mocap_ape/record/watch_only_rec.py +++ b/src/wear_mocap_ape/record/watch_only_rec.py @@ -29,19 +29,13 @@ def __init__(self, # the header matches the msg in process_msg header = [ "time", - # relative hand orientation - "hand_quat_w", "hand_quat_x", - "hand_quat_y", "hand_quat_z", - # hand origin (wrist) relative to shoulder - "hand_orig_rua_x", "hand_orig_rua_y", "hand_orig_rua_z", - # relative lower arm orientation - "larm_quat_rh_w", "larm_quat_rh_x", - "larm_quat_rh_y", "larm_quat_rh_z", - # lower arm origin (elbow) relative to shoulder - "larm_orig_rua_x", "larm_orig_rua_y", "larm_orig_rua_z", - # relative upper arm orientation - "uarm_quat_rh_w", "uarm_quat_rh_x", - "uarm_quat_rh_y", "uarm_quat_rh_z" + "hand_quat_w", "hand_quat_x", "hand_quat_y", "hand_quat_z", + "hand_orig_rh_x", "hand_orig_rh_y", "hand_orig_rh_z", + "larm_quat_rh_w", "larm_quat_rh_x", "larm_quat_rh_y", "larm_quat_rh_z", + "larm_orig_rh_x", "larm_orig_rh_y", "larm_orig_rh_z", + "uarm_quat_rh_w", "uarm_quat_rh_x", "uarm_quat_rh_y", "uarm_quat_rh_z", + "uarm_orig_rh_x", "uarm_orig_rh_y", "uarm_orig_rh_z", + "hips_quat_g_w", "hips_quat_g_x", "hips_quat_g_y", "hips_quat_g_z" ] if stream_monte_carlo: diff --git a/src/wear_mocap_ape/record/watch_phone_rec.py b/src/wear_mocap_ape/record/watch_phone_uarm_rec.py similarity index 62% rename from src/wear_mocap_ape/record/watch_phone_rec.py rename to src/wear_mocap_ape/record/watch_phone_uarm_rec.py index 12ca47d..368769d 100644 --- a/src/wear_mocap_ape/record/watch_phone_rec.py +++ b/src/wear_mocap_ape/record/watch_phone_uarm_rec.py @@ -2,10 +2,10 @@ from pathlib import Path import numpy as np from wear_mocap_ape.data_types.bone_map import BoneMap -from wear_mocap_ape.estimate.watch_phone import WatchPhone +from wear_mocap_ape.estimate.watch_phone_uarm import WatchPhoneUarm -class WatchPhoneRecorder(WatchPhone): +class WatchPhoneUarmRecorder(WatchPhoneUarm): def __init__(self, file: Path, smooth: int = 5, @@ -23,19 +23,13 @@ def __init__(self, # the header matches the msg in process_msg header = [ "time", - # relative hand orientation - "hand_quat_w", "hand_quat_x", - "hand_quat_y", "hand_quat_z", - # hand origin (wrist) relative to shoulder - "hand_orig_rua_x", "hand_orig_rua_y", "hand_orig_rua_z", - # relative lower arm orientation - "larm_quat_rh_w", "larm_quat_rh_x", - "larm_quat_rh_y", "larm_quat_rh_z", - # lower arm origin (elbow) relative to shoulder - "larm_orig_rua_x", "larm_orig_rua_y", "larm_orig_rua_z", - # relative upper arm orientation - "uarm_quat_rh_w", "uarm_quat_rh_x", - "uarm_quat_rh_y", "uarm_quat_rh_z" + "hand_quat_w", "hand_quat_x", "hand_quat_y", "hand_quat_z", + "hand_orig_rh_x", "hand_orig_rh_y", "hand_orig_rh_z", + "larm_quat_rh_w", "larm_quat_rh_x", "larm_quat_rh_y", "larm_quat_rh_z", + "larm_orig_rh_x", "larm_orig_rh_y", "larm_orig_rh_z", + "uarm_quat_rh_w", "uarm_quat_rh_x", "uarm_quat_rh_y", "uarm_quat_rh_z", + "uarm_orig_rh_x", "uarm_orig_rh_y", "uarm_orig_rh_z", + "hips_quat_g_w", "hips_quat_g_x", "hips_quat_g_y", "hips_quat_g_z" ] if not self.__file.parent.exists(): diff --git a/src/wear_mocap_ape/stream/publisher/watch_phone_pocket_udp.py b/src/wear_mocap_ape/stream/publisher/watch_phone_pocket_udp.py new file mode 100644 index 0000000..b187138 --- /dev/null +++ b/src/wear_mocap_ape/stream/publisher/watch_phone_pocket_udp.py @@ -0,0 +1,39 @@ +import logging +import socket +import struct +import numpy as np +from wear_mocap_ape.estimate.watch_phone_pocket import WatchPhonePocket + + +class WatchPhonePocketUDP(WatchPhonePocket): + def __init__(self, + ip, + port, + smooth: int = 1, + num_ensemble: int = 32, + model_name="SW-model", + window_size: int = 10, + stream_mc: bool = True, + tag: str = "KALMAN UDP POCKET PHONE"): + + super().__init__( + smooth=smooth, + num_ensemble=num_ensemble, + model_name=model_name, + window_size=window_size, + stream_mc=stream_mc + ) + self.__tag = tag + self.__port = port + self.__ip = ip + self.__udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.__udp_socket.settimeout(5) + + def process_msg(self, msg: np.array) -> int: + # craft UDP message and send + msg = struct.pack('f' * len(msg), *msg) + try: + return self.__udp_socket.sendto(msg, (self.__ip, self.__port)) + except TimeoutError: + logging.info(f"[{self.__tag}] timed out") + return -1 diff --git a/src/wear_mocap_ape/stream/publisher/watch_phone_ros.py b/src/wear_mocap_ape/stream/publisher/watch_phone_uarm_ros.py similarity index 92% rename from src/wear_mocap_ape/stream/publisher/watch_phone_ros.py rename to src/wear_mocap_ape/stream/publisher/watch_phone_uarm_ros.py index 86a997f..35a7ea3 100644 --- a/src/wear_mocap_ape/stream/publisher/watch_phone_ros.py +++ b/src/wear_mocap_ape/stream/publisher/watch_phone_uarm_ros.py @@ -3,10 +3,10 @@ from std_msgs.msg import Float32MultiArray from wear_mocap_ape.data_types.bone_map import BoneMap -from wear_mocap_ape.estimate.watch_phone import WatchPhone +from wear_mocap_ape.estimate.watch_phone_uarm import WatchPhoneUarm -class WatchPhoneROS(WatchPhone): +class WatchPhoneROS(WatchPhoneUarm): def __init__(self, ros_node_name="/wear_mocap", diff --git a/src/wear_mocap_ape/stream/publisher/watch_phone_udp.py b/src/wear_mocap_ape/stream/publisher/watch_phone_uarm_udp.py similarity index 78% rename from src/wear_mocap_ape/stream/publisher/watch_phone_udp.py rename to src/wear_mocap_ape/stream/publisher/watch_phone_uarm_udp.py index 304ce77..6e12346 100644 --- a/src/wear_mocap_ape/stream/publisher/watch_phone_udp.py +++ b/src/wear_mocap_ape/stream/publisher/watch_phone_uarm_udp.py @@ -4,10 +4,10 @@ import numpy as np from wear_mocap_ape.data_types.bone_map import BoneMap -from wear_mocap_ape.estimate.watch_phone import WatchPhone +from wear_mocap_ape.estimate.watch_phone_uarm import WatchPhoneUarm -class WatchPhoneUDP(WatchPhone): +class WatchPhoneUarmUDP(WatchPhoneUarm): def __init__(self, ip: str, port: int, @@ -15,10 +15,12 @@ def __init__(self, left_hand_mode=True, tag: str = "PUB WATCH PHONE", bonemap: BoneMap = None): - super().__init__(smooth=smooth, - left_hand_mode=left_hand_mode, - tag=tag, - bonemap=bonemap) + super().__init__( + smooth=smooth, + left_hand_mode=left_hand_mode, + tag=tag, + bonemap=bonemap + ) self.__ip = ip self.__port = port self.__tag = tag From 643103869afa1d4f2a81dd4699c8688261c06cdf Mon Sep 17 00:00:00 2001 From: Fabian Weigend Date: Tue, 10 Oct 2023 14:52:15 -0700 Subject: [PATCH 2/2] wip kalman code cleanup --- src/wear_mocap_ape/estimate/kalman_models.py | 24 +- .../estimate/phone_pocket_free_hips_udp.py | 466 ------------------ .../estimate/watch_phone_pocket.py | 4 +- 3 files changed, 13 insertions(+), 481 deletions(-) delete mode 100644 src/wear_mocap_ape/estimate/phone_pocket_free_hips_udp.py diff --git a/src/wear_mocap_ape/estimate/kalman_models.py b/src/wear_mocap_ape/estimate/kalman_models.py index 589ebc8..76426d9 100644 --- a/src/wear_mocap_ape/estimate/kalman_models.py +++ b/src/wear_mocap_ape/estimate/kalman_models.py @@ -6,7 +6,8 @@ import torch.nn as nn import torch.nn.functional as F -class utils: + +class Utils: def __init__(self, num_ensemble, dim_x, dim_z): self.num_ensemble = num_ensemble self.dim_x = dim_x @@ -30,10 +31,9 @@ def format_state(self, state): return state - -class Seq_MLP_process_model(nn.Module): +class ProcessModelSeqMLP(nn.Module): def __init__(self, num_ensemble, dim_x, win_size, dim_model, num_heads): - super(Seq_MLP_process_model, self).__init__() + super(ProcessModelSeqMLP, self).__init__() self.num_ensemble = num_ensemble self.dim_x = dim_x self.dim_model = dim_model @@ -89,7 +89,7 @@ def forward(self, inputs): return R -class SeqSensorModel(nn.Module): +class SensorModelSeq(nn.Module): """ the sensor model takes the current raw sensor (usually high-dimensional images) and map the raw sensor to low-dimension @@ -101,7 +101,7 @@ class SeqSensorModel(nn.Module): """ def __init__(self, num_ensemble, dim_z, win_size, input_size_1): - super(SeqSensorModel, self).__init__() + super(SensorModelSeq, self).__init__() self.dim_z = dim_z self.num_ensemble = num_ensemble @@ -137,11 +137,9 @@ def forward(self, x): return obs, obs_z, encoding - - -class new_smartwatch_model(nn.Module): +class KalmanSmartwatchModel(nn.Module): def __init__(self, num_ensemble, win_size, dim_x, dim_z, input_size_1): - super(new_smartwatch_model, self).__init__() + super(KalmanSmartwatchModel, self).__init__() self.num_ensemble = num_ensemble self.dim_x = dim_x self.dim_z = dim_z @@ -150,10 +148,10 @@ def __init__(self, num_ensemble, win_size, dim_x, dim_z, input_size_1): self.r_diag = self.r_diag.astype(np.float32) # instantiate model - self.process_model = Seq_MLP_process_model( + self.process_model = ProcessModelSeqMLP( self.num_ensemble, self.dim_x, self.win_size, 256, 8 ) - self.sensor_model = SeqSensorModel( + self.sensor_model = SensorModelSeq( self.num_ensemble, self.dim_z, win_size, input_size_1 ) self.observation_noise = NewObservationNoise(self.dim_z, self.r_diag) @@ -212,4 +210,4 @@ def forward(self, inputs, states): z.to(dtype=torch.float32), ensemble_z.to(dtype=torch.float32), ) - return output \ No newline at end of file + return output diff --git a/src/wear_mocap_ape/estimate/phone_pocket_free_hips_udp.py b/src/wear_mocap_ape/estimate/phone_pocket_free_hips_udp.py deleted file mode 100644 index ed331c6..0000000 --- a/src/wear_mocap_ape/estimate/phone_pocket_free_hips_udp.py +++ /dev/null @@ -1,466 +0,0 @@ -import logging -import socket -import struct -import threading -import time -from datetime import datetime -import queue -from pathlib import Path - -import numpy as np -import pandas as pd -import torch - -from wear_mocap_ape import config -from wear_mocap_ape.data_deploy.nn import deploy_models -from wear_mocap_ape.data_types.bone_map import BoneMap -from wear_mocap_ape.estimate import models, estimate_joints -from wear_mocap_ape.stream.listener.imu import ImuListener -from wear_mocap_ape.utility import transformations as ts, data_stats -from wear_mocap_ape.data_types import messaging -from wear_mocap_ape.utility.names import NNS_TARGETS, NNS_INPUTS - - -class FreeHipsPocketUDP: - def __init__(self, - ip: str, - port: int, - model_hash: str, - smooth: int = 1, - stream_monte_carlo=True, - monte_carlo_samples=25, - bonemap: BoneMap = None, - tag: str = "PUB FREE HIPS"): - - # smooth should not be smaller 1 - smooth = max(1, smooth) - - self.__tag = tag - self.__port = port - self.__ip = ip - self.__stream_mc = stream_monte_carlo - self.__mc_samples = monte_carlo_samples - self.__active = False - - # average over multiple time steps - self.__smooth = smooth - self.__smooth_hist = [] - self.__last_hip_pred = None - - # simple lookup for values of interest - self.__slp = messaging.WATCH_PHONE_IMU_LOOKUP - - self.__start = datetime.now() - self.__dat = 0 - self.__prev_t = datetime.now() - self.__row_hist = [] - - self.last_msg = np.zeros(25) - - # use arm length measurements for predictions - if bonemap is None: - # default values - self.__larm_vec = np.array([-BoneMap.DEFAULT_LARM_LEN, 0, 0]) - self.__uarm_vec = np.array([-BoneMap.DEFAULT_UARM_LEN, 0, 0]) - self.__uarm_orig = BoneMap.DEFAULT_L_SHOU_ORIG_RH - else: - self.__larm_vec = bonemap.left_lower_arm_vec - self.__uarm_vec = bonemap.left_upper_arm_vec - self.__uarm_orig = bonemap.left_upper_arm_origin_rh - - # for quicker access we store a matrix with relevant body measurements for quick multiplication - self.__body_measurements = np.r_[self.__larm_vec, self.__uarm_vec, self.__uarm_orig][np.newaxis, :] - self.__udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - - # load the trained network - torch.set_default_dtype(torch.float64) - self.__device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') - - # load model from given parameters - self.__nn_model, params = models.load_deployed_model_from_hash(hash_str=model_hash) - self.__y_targets = NNS_TARGETS(params["y_targets_v"]) - self.__x_inputs = NNS_INPUTS(params["x_inputs_v"]) - self.__sequence_len = params["sequence_len"] - self.__normalize = params["normalize"] - - # load normalized data stats if required - if self.__normalize: - # data is normalized and has to be transformed with pre-calculated mean and std - stats = data_stats.get_norm_stats(x_inputs=self.__x_inputs, y_targets=self.__y_targets) - self.__xx_m, self.__xx_s = stats["xx_m"], stats["xx_s"] - self.__yy_m, self.__yy_s = stats["yy_m"], stats["yy_s"] - else: - self.__xx_m, self.__xx_s = 0., 1. - self.__yy_m, self.__yy_s = 0., 1. - - def terminate(self): - self.__active = False - - @property - def sequence_len(self): - return self.__sequence_len - - def parse_row_to_xx(self, row): - # get relevant entries from the row - sw_fwd = np.c_[ - row[self.__slp["sw_forward_w"]], row[self.__slp["sw_forward_x"]], - row[self.__slp["sw_forward_y"]], row[self.__slp["sw_forward_z"]] - ] - - sw_rot = np.c_[ - row[self.__slp["sw_rotvec_w"]], row[self.__slp["sw_rotvec_x"]], - row[self.__slp["sw_rotvec_y"]], row[self.__slp["sw_rotvec_z"]] - ] - - ph_fwd = np.c_[ - row[self.__slp["ph_forward_w"]], row[self.__slp["ph_forward_x"]], - row[self.__slp["ph_forward_y"]], row[self.__slp["ph_forward_z"]] - ] - - ph_rot = np.c_[ - row[self.__slp["ph_rotvec_w"]], row[self.__slp["ph_rotvec_x"]], - row[self.__slp["ph_rotvec_y"]], row[self.__slp["ph_rotvec_z"]] - ] - - # estimate north quat to align Z-axis of global and android coord system - r = ts.android_quat_to_global_no_north(sw_fwd) - y_rot = ts.reduce_global_quat_to_y_rot(r)[0] - quat_north = ts.euler_to_quat(np.array([0, -y_rot, 0])) - # calibrate watch - sw_cal_g = ts.android_quat_to_global(sw_rot, quat_north) - sw_6drr_cal = ts.quat_to_6drr_1x6(sw_cal_g) - - # the device orientations if the calib position with left arm forward is perfect - hips_dst_g = np.array([1, 0, 0, 0]) - ph_rot_g = ts.android_quat_to_global(ph_rot, quat_north) - ph_fwd_g = ts.android_quat_to_global(ph_fwd, quat_north) - ph_off_g = ts.hamilton_product(ts.quat_invert(ph_fwd_g), hips_dst_g) - ph_cal_g = ts.hamilton_product(ph_rot_g, ph_off_g) - - # hip y rotation from phone - hips_y_rot = ts.reduce_global_quat_to_y_rot(ph_cal_g) - hips_yrot_cal_sin = np.sin(hips_y_rot) - hips_yrot_cal_cos = np.cos(hips_y_rot) - - - # pressure - calibrated initial pressure = relative pressure - r_pres = row[self.__slp["sw_pres"]] - row[self.__slp["sw_init_pres"]] - - # assemble the entire input vector of one time step - return np.c_[ - row[self.__slp["sw_dt"]], - row[self.__slp["sw_gyro_x"]], row[self.__slp["sw_gyro_y"]], row[self.__slp["sw_gyro_z"]], - row[self.__slp["sw_lvel_x"]], row[self.__slp["sw_lvel_y"]], row[self.__slp["sw_lvel_z"]], - row[self.__slp["sw_lacc_x"]], row[self.__slp["sw_lacc_y"]], row[self.__slp["sw_lacc_z"]], - row[self.__slp["sw_grav_x"]], row[self.__slp["sw_grav_y"]], row[self.__slp["sw_grav_z"]], - sw_6drr_cal, - r_pres, - hips_yrot_cal_sin, - hips_yrot_cal_cos - ] - - def msg_from_est(self, est): - # estimate mean of rotations if we got multiple MC predictions - if est.shape[0] > 1: - # Calculate the mean of all predictions mean - p_hips_quat_g = ts.average_quaternions(est[:, 17:]) - p_larm_quat_g = ts.average_quaternions(est[:, 9:13]) - p_uarm_quat_g = ts.average_quaternions(est[:, 13:17]) - - # get the transition from upper arm origin to lower arm origin - p_uarm_orig_g = ts.quat_rotate_vector(p_hips_quat_g, self.__uarm_orig) - p_larm_orig_g = ts.quat_rotate_vector(p_uarm_quat_g, self.__uarm_vec) + p_uarm_orig_g - p_hand_orig_g = ts.quat_rotate_vector(p_larm_quat_g, self.__larm_vec) + p_larm_orig_g - else: - p_hand_orig_g = est[0, 0:3] - p_larm_orig_g = est[0, 3:6] - p_uarm_orig_g = est[0, 6:9] - p_larm_quat_g = est[0, 9:13] - p_uarm_quat_g = est[0, 13:17] - p_hips_quat_g = est[0, 17:] - - # this is the list for the actual joint positions and rotations - msg = np.hstack([ - p_larm_quat_g, # hand rot - p_hand_orig_g, # hand orig - p_larm_quat_g, # larm rot - p_larm_orig_g, # larm orig - p_uarm_quat_g, # uarm rot - p_uarm_orig_g, # uarm orig - p_hips_quat_g # hips rot - ]) - - # now we attach the monte carlo predictions for XYZ positions - if self.__stream_mc: - if est.shape[0] > 1: - msg = np.hstack([msg, est[:, :9].flatten()]) - - return np.array(msg) - - def stream_loop(self, sensor_q: queue): - logging.info("[{}] starting Unity stream loop".format(self.__tag)) - # used to estimate delta time and processing speed in Hz - self.__start = datetime.now() - self.__dat = 0 - self.__prev_t = datetime.now() - row_hist = [] - # this loops while the socket is listening and/or receiving data - self.__active = True - while self.__active: - # get the most recent smartwatch data row from the queue - row_hist.append(self.parse_row_to_xx(sensor_q.get())) - - # add rows until the queue is nearly empty - while sensor_q.qsize() > 5: - # add row to history and immediately continue with the next - row_hist.append(self.parse_row_to_xx(sensor_q.get())) - - # if not enough data is available yet, simply repeat the input as a first estimate - while len(row_hist) < self.__sequence_len: - row_hist.append(row_hist[-1].copy()) - - # if the history is too long, delete old values - while len(row_hist) > self.__sequence_len: - del row_hist[0] - - # get message as numpy array - est = self.make_prediction_from_row_hist(row_hist) - - # estimate mean of rotations if we got multiple MC predictions - if est.shape[0] > 1: - # Calculate the mean of all predictions mean - p_hips_quat_g = ts.average_quaternions(est[:, 17:]) - p_larm_quat_g = ts.average_quaternions(est[:, 9:13]) - p_uarm_quat_g = ts.average_quaternions(est[:, 13:17]) - - # get the transition from upper arm origin to lower arm origin - p_uarm_orig_g = ts.quat_rotate_vector(p_hips_quat_g, self.__uarm_orig) - p_larm_orig_g = ts.quat_rotate_vector(p_uarm_quat_g, self.__uarm_vec) + p_uarm_orig_g - p_hand_orig_g = ts.quat_rotate_vector(p_larm_quat_g, self.__larm_vec) + p_larm_orig_g - else: - p_hand_orig_g = est[0, 0:3] - p_larm_orig_g = est[0, 3:6] - p_uarm_orig_g = est[0, 6:9] - p_larm_quat_g = est[0, 9:13] - p_uarm_quat_g = est[0, 13:17] - p_hips_quat_g = est[0, 17:] - - # this is the list for the actual joint positions and rotations - msg = np.hstack([ - p_larm_quat_g, # hand rot - p_hand_orig_g, # hand orig - p_larm_quat_g, # larm rot - p_larm_orig_g, # larm orig - p_uarm_quat_g, # uarm rot - p_uarm_orig_g, # uarm orig - p_hips_quat_g # hips rot - ]) - - # now we attach the monte carlo predictions for XYZ positions - if self.__stream_mc: - if est.shape[0] > 1: - msg = np.hstack([msg, est[:, :9].flatten()]) - - np_msg = np.array(msg) - self.last_msg = np_msg - - # five-secondly updates to log message frequency - now = datetime.now() - if (now - self.__start).seconds >= 5: - self.__start = now - logging.info(f"[{self.__tag}] {self.__dat / 5} Hz") - self.__dat = 0 - - # send message to Unity - self.send_np_msg(msg=np_msg) - self.__dat += 1 - time.sleep(0.01) - - def errors_from_file(self, file_p: Path, publish: bool = False): - logging.info(f"[{self.__tag}] processing from file") - hand_l_err, hand_r_err = [], [] - elbow_l_err, elbow_r_err = [], [] - hip_rot_errors = [] - - row_hist = [] - dat = pd.read_csv(file_p) - for i, row in dat.iterrows(): - ## PREDICTIONS - xx = row[self.__x_inputs.value].to_numpy() - row_hist.append(xx) - - # if not enough data is available yet, simply repeat the input as a first estimate - while len(row_hist) < self.__sequence_len: - row_hist.append(xx) - - # if the history is too long, delete old values - while len(row_hist) > self.__sequence_len: - del row_hist[0] - - # get message as numpy array - est = self.make_prediction_from_row_hist(row_hist) - - # estimate mean of rotations if we got multiple MC predictions - if est.shape[0] > 1: - # Calculate the mean of all predictions mean - p_larm_quat_rh = ts.average_quaternions(est[:, 9:13]) - p_uarm_quat_rh = ts.average_quaternions(est[:, 13:17]) - p_hips_quat_rh = ts.average_quaternions(est[:, 17:]) - - # get the transition from upper arm origin to lower arm origin - p_uarm_orig_rh = ts.quat_rotate_vector(p_hips_quat_rh, self.__uarm_orig) - p_larm_orig_rh = ts.quat_rotate_vector(p_uarm_quat_rh, self.__uarm_vec) + p_uarm_orig_rh - p_hand_orig_rh = ts.quat_rotate_vector(p_larm_quat_rh, self.__larm_vec) + p_larm_orig_rh - else: - p_hand_orig_rh = est[0, 0:3] - p_larm_orig_rh = est[0, 3:6] - p_uarm_orig_rh = est[0, 6:9] - p_larm_quat_rh = est[0, 9:13] - p_uarm_quat_rh = est[0, 13:17] - p_hips_quat_rh = est[0, 17:] - - # publish the estimation for our unity visualization - if publish: - msg = np.hstack([ - p_larm_quat_rh, - p_hand_orig_rh, - p_larm_quat_rh, - p_larm_orig_rh, - p_uarm_quat_rh, - p_uarm_orig_rh, - p_hips_quat_rh - ]) - if est.shape[0] > 1: - if est.shape[0] > 1: - msg = np.hstack([msg, est[:, :9].flatten()]) - - # five-secondly updates to log message frequency - now = datetime.now() - if (now - self.__start).seconds >= 5: - self.__start = now - logging.info(f"[{self.__tag}] {self.__dat / 5} Hz") - self.__dat = 0 - - # send message to Unity - self.send_np_msg(msg=msg) - self.__dat += 1 - - # GROUND TRUTH - yy = row[self.__y_targets.value].to_numpy()[np.newaxis, :] - est = estimate_joints.arm_pose_from_nn_targets( - preds=yy, - body_measurements=self.__body_measurements, - y_targets=self.__y_targets - ) - gt_hand_orig_rh = est[0, 0:3] - gt_larm_orig_rh = est[0, 3:6] - gt_larm_quat_g = est[0, 9:13] - gt_uarm_quat_g = est[0, 13:17] - gt_hips_quat_g = est[0, 17:] - - hand_l_err.append(np.linalg.norm(gt_hand_orig_rh - p_hand_orig_rh) * 100) - hre = ts.quat_to_euler(gt_larm_quat_g) - ts.quat_to_euler(p_larm_quat_rh) - hree = np.sum(np.abs(np.degrees(np.arctan2(np.sin(hre), np.cos(hre))))) - hand_r_err.append(hree) - - elbow_l_err.append(np.linalg.norm(gt_larm_orig_rh - p_larm_orig_rh) * 100) - ere = ts.quat_to_euler(gt_uarm_quat_g) - ts.quat_to_euler(p_uarm_quat_rh) - eree = np.sum(np.abs(np.degrees(np.arctan2(np.sin(ere), np.cos(ere))))) - elbow_r_err.append(eree) - - ydiff = ts.quat_to_euler(gt_hips_quat_g)[1] - ts.quat_to_euler(p_hips_quat_rh)[1] - err = np.abs(np.degrees(np.arctan2(np.sin(ydiff), np.cos(ydiff)))) - hip_rot_errors.append(err) - - if (int(i) + 1) % 100 == 0: - logging.info(f"[{self.__tag}] processed {i} rows") - - return hand_l_err, hand_r_err, elbow_l_err, elbow_r_err, hip_rot_errors - - def send_np_msg(self, msg: np.array) -> int: - # craft UDP message and send - msg = struct.pack('f' * len(msg), *msg) - return self.__udp_socket.sendto(msg, (self.__ip, self.__port)) - - def make_prediction_from_row_hist(self, row_hist): - # stack rows to one big array - seq = np.vstack(row_hist.copy()) - - if self.__normalize: - # normalize measurements with pre-calculated mean and std - xx = (seq - self.__xx_m) / self.__xx_s - - # finally, cast to a torch tensor with batch size 1 - xx = torch.tensor(xx[None, :, :]) - with torch.no_grad(): - # make mote carlo predictions if the model makes use of dropout - t_preds = self.__nn_model.monte_carlo_predictions(x=xx, n_samples=self.__mc_samples) - - # if on GPU copy the tensor to host memory first - if self.__device.type == 'cuda': - t_preds = t_preds.cpu() - t_preds = t_preds.numpy() - - # we are only interested in the last prediction of the sequence - t_preds = t_preds[:, -1, :] - - if self.__normalize: - # transform predictions back from normalized labels - t_preds = t_preds * self.__yy_s + self.__yy_m - - est = estimate_joints.arm_pose_from_nn_targets( - preds=t_preds, - body_measurements=self.__body_measurements, - y_targets=self.__y_targets - ) - - # store t_preds in history if smoothing is required - if self.__smooth > 1: - self.__smooth_hist.append(est) - while len(self.__smooth_hist) < self.__smooth: - self.__smooth_hist.append(est) - while len(self.__smooth_hist) > self.__smooth: - del self.__smooth_hist[0] - est = np.vstack(self.__smooth_hist) - - return est - - -def run_free_hips_pocket_udp(ip: str, - model_hash: str = deploy_models.LSTM.WATCH_PHONE_POCKET.value, - stream_monte_carlo: bool = False, - smooth: int = 10) -> FreeHipsPocketUDP: - """ - convenience function to create a default running instance - """ - - # data for left-hand mode - left_q = queue.Queue() - - # listen for imu data from phone and watch - imu_l = ImuListener( - ip=ip, - msg_size=messaging.watch_phone_imu_msg_len, - port=config.PORT_LISTEN_WATCH_PHONE_IMU_LEFT, - tag="LISTEN IMU LEFT" - ) - l_thread = threading.Thread( - target=imu_l.listen, - args=(left_q,) - ) - l_thread.start() - - # process into arm pose and body orientation - fhp = FreeHipsPocketUDP(ip=ip, - model_hash=model_hash, - smooth=smooth, - port=config.PORT_PUB_LEFT_ARM, - monte_carlo_samples=25, - stream_monte_carlo=stream_monte_carlo) - p_thread = threading.Thread( - target=fhp.stream_loop, - args=(left_q,) - ) - p_thread.start() - - return fhp diff --git a/src/wear_mocap_ape/estimate/watch_phone_pocket.py b/src/wear_mocap_ape/estimate/watch_phone_pocket.py index 8f96b1d..4be3275 100644 --- a/src/wear_mocap_ape/estimate/watch_phone_pocket.py +++ b/src/wear_mocap_ape/estimate/watch_phone_pocket.py @@ -57,7 +57,7 @@ def __init__(self, self.win_size = window_size self.mode = "Test" - self.model = kalman_models.new_smartwatch_model( + self.model = kalman_models.KalmanSmartwatchModel( self.num_ensemble, self.win_size, self.dim_x, @@ -65,7 +65,7 @@ def __init__(self, self.input_size_1 ) - self.utils_ = kalman_models.utils(self.num_ensemble, self.dim_x, self.dim_z) + self.utils_ = kalman_models.Utils(self.num_ensemble, self.dim_x, self.dim_z) # Check model type if not isinstance(self.model, torch.nn.Module):