Merge pull request #19 from wearable-motion-capture/parrot_control

Parrot control

example_scripts/watch_phone_pocket_stream.py

@@ -0,0 +1,69 @@
+import argparse
+import atexit
+import logging
+import queue
+import signal
+import threading
+
+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
+
+if __name__ == "__main__":
+    logging.basicConfig(level=logging.INFO)
+    # Instantiate the parser
+    parser = argparse.ArgumentParser(description='')
+
+    # 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')
+    args = parser.parse_args()
+
+    ip_arg = args.ip
+    smooth_arg = args.smooth
+    stream_mc_arg = args.stream_mc
+
+    # data for left-hand mode
+    left_q = queue.Queue()
+
+    # listen for imu data from phone and watch
+    imu_l = ImuListener(
+        ip=ip_arg,
+        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,)
+    )
+
+    # 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")
+    p_thread = threading.Thread(
+        target=kpp.stream_wearable_devices,
+        args=(left_q, True,)
+    )
+
+    l_thread.start()
+    p_thread.start()
+
+
+    def terminate_all(*args):
+        imu_l.terminate()
+        kpp.terminate()
+
+
+    # make sure all handler exit on termination
+    atexit.register(terminate_all)
+    signal.signal(signal.SIGTERM, terminate_all)
+    signal.signal(signal.SIGINT, terminate_all)
+

example_scripts/stream_watch_phone.py → example_scripts/watch_phone_uarm_stream.py

@@ -91,6 +91,7 @@ def terminate_all(*args):
     imu_r.terminate()
     wp2ul.terminate()
 
+
 # make sure all handler exit on termination
 atexit.register(terminate_all)
 signal.signal(signal.SIGTERM, terminate_all)

setup.cfg

@@ -1,6 +1,6 @@
 [metadata]
 name = wear_mocap_ape
-version = 1.0.2
+version = 1.1.0
 author = Fabian Weigend
 author_email = fweigend@asu.edu
 description =
@@ -25,6 +25,8 @@ install_requires =
     pandas
     matplotlib
     pynput
+    einops
+    bayesian_torch
 
 packages = find:
 include_package_data = True

src/wear_mocap_ape/config.py

@@ -1,10 +1,10 @@
-import os
+from pathlib import Path
 
-proj_path = os.path.dirname(os.path.abspath(__file__))
+proj_path = Path(__file__).parent.absolute()
 
 PATHS = {
-    "deploy": f"{proj_path}/data_deploy/",
-    "skeleton": f"{proj_path}/data_deploy/"
+    "deploy": proj_path / "data_deploy",
+    "skeleton": proj_path / "data_deploy"
 }
 
 # ports for publishing to other services

src/wear_mocap_ape/data_deploy/nn/04d2d059ee8980e13e5e16fe048b6bd0f8265c9a/results.json

@@ -0,0 +1,60 @@
+{
+    "model": "ImuPoseLSTM",
+    "hidden_layer_count": 2,
+    "hidden_layer_size": 128,
+    "epochs": 200,
+    "batch_size": 64,
+    "learning_rate": 0.001,
+    "dropout": 0.4,
+    "sequence_len": 5,
+    "normalize": true,
+    "seq_overlap": true,
+    "create_norm_stats": true,
+    "hash": "04d2d059ee8980e13e5e16fe048b6bd0f8265c9a",
+    "y_targets_n": "ORI_CAL_LARM_UARM_HIPS",
+    "x_inputs_n": "WATCH_PH_HIP",
+    "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",
+        "gt_hips_yrot_cal_sin",
+        "gt_hips_yrot_cal_cos"
+    ],
+    "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_hips_yrot_cal_sin",
+        "ph_hips_yrot_cal_cos"
+    ],
+    "Loss/train": 0.13687036271612407,
+    "Loss/test": 0.28687360928033256,
+    "Loss/b_train": 0.1611349323569118,
+    "Loss/b_test": 0.2804641976381203
+}

src/wear_mocap_ape/data_deploy/nn/2b4e48b366d717b035751c40f977d9ae6c26d6b2/results.json

@@ -0,0 +1,50 @@
+{
+    "model": "DropoutFF",
+    "hidden_layer_count": 4,
+    "hidden_layer_size": 128,
+    "epochs": 200,
+    "batch_size": 64,
+    "learning_rate": 0.0001,
+    "dropout": 0.2,
+    "sequence_len": 5,
+    "normalize": true,
+    "seq_overlap": true,
+    "create_norm_stats": true,
+    "hash": "2b4e48b366d717b035751c40f977d9ae6c26d6b2",
+    "y_targets_n": "POS_RH_LARM_HAND",
+    "x_inputs_n": "WATCH_ONLY",
+    "y_targets_v": [
+        "gt_hand_orig_rua_x",
+        "gt_hand_orig_rua_y",
+        "gt_hand_orig_rua_z",
+        "gt_larm_orig_rua_x",
+        "gt_larm_orig_rua_y",
+        "gt_larm_orig_rua_z"
+    ],
+    "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"
+    ],
+    "Loss/train": 0.24004665516297385,
+    "Loss/test": 0.377081323361239,
+    "Loss/b_train": 0.2682442721176126,
+    "Loss/b_test": 0.3734141369143522
+}

src/wear_mocap_ape/data_deploy/nn/deploy_models.py

@@ -1,5 +1,11 @@
 from enum import Enum
 
 
+class FF(Enum):
+    WATCH_ONLY = "2b4e48b366d717b035751c40f977d9ae6c26d6b2"
+
+
 class LSTM(Enum):
     WATCH_ONLY = "2c700a1ca1af084eedbae5bdd86a5194e42ded4d"
+    IMU_EXMP = "04d2d059ee8980e13e5e16fe048b6bd0f8265c9a"
+    WATCH_PHONE_POCKET = "ec70862ec17828320ca64a738b2a90cf5dcadced"

src/wear_mocap_ape/data_deploy/nn/ec70862ec17828320ca64a738b2a90cf5dcadced/results.json

@@ -0,0 +1,60 @@
+{
+    "model": "DropoutLSTM",
+    "hidden_layer_count": 2,
+    "hidden_layer_size": 128,
+    "epochs": 200,
+    "batch_size": 64,
+    "learning_rate": 0.001,
+    "dropout": 0.4,
+    "sequence_len": 5,
+    "normalize": true,
+    "seq_overlap": true,
+    "create_norm_stats": true,
+    "hash": "ec70862ec17828320ca64a738b2a90cf5dcadced",
+    "y_targets_n": "ORI_CAL_LARM_UARM_HIPS",
+    "x_inputs_n": "WATCH_PH_HIP",
+    "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",
+        "gt_hips_yrot_cal_sin", 
+        "gt_hips_yrot_cal_cos" 
+    ],
+    "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_hips_yrot_cal_sin",
+        "ph_hips_yrot_cal_cos"
+    ],
+    "Loss/train": 0.19960083461272307,
+    "Loss/test": 0.2753936388019971,
+    "Loss/b_train":  0.19960083461272307,
+    "Loss/b_test": 0.2753936388019971
+}

src/wear_mocap_ape/data_types/bone_map.py

@@ -39,7 +39,7 @@ class BoneMap:
     """
 
     # available without initialization
-    DEFAULT_LARM_LEN = 0.22
+    DEFAULT_LARM_LEN = 0.22  # 0.22
     DEFAULT_UARM_LEN = 0.30
     # default left shoulder origin relative to hip
     DEFAULT_L_SHOU_ORIG_RH = np.array([-0.1704612, 0.4309841, -0.00670862])
@@ -143,4 +143,3 @@ def left_upper_arm_origin_g(self):
     @property
     def hip_origin_g(self):
         return self.__bonemap[1].default_pos  # hips
-

src/wear_mocap_ape/data_types/messaging.py

@@ -56,14 +56,14 @@
     "sw_grav_z": 22,
 
     # calibration measurements
-    # pressure at chest height
-    "sw_init_pres": 23,
-
     # forward facing direction
-    "sw_forward_w": 24,
-    "sw_forward_x": 25,
-    "sw_forward_y": 26,
-    "sw_forward_z": 27
+    "sw_forward_w": 23,
+    "sw_forward_x": 24,
+    "sw_forward_y": 25,
+    "sw_forward_z": 26,
+
+    # pressure at chest height
+    "sw_init_pres": 27
 }
 watch_only_imu_msg_len = len(WATCH_ONLY_IMU_LOOKUP) * 4
 

src/wear_mocap_ape/data_types/voice_commands.py

@@ -5,16 +5,24 @@
 """
 KEY_PHRASES = {
     # dual manipulation task
-    "blue sky": 0,  # left
-    "red apple": 1,  # right
-
-    # ROS experiment commands
+    # "blue": 0,  # left
+    # "red": 1,  # right
+    #
+    # # ROS experiment commands
     "stop": 3,  # stop current task
-    "move": 4,  # carry on with original task
-    "continue": 4,  # carry on with original task
+    # "move": 4,  # carry on with original task
+    # "continue": 4,  # same as "move"
     "follow": 5,  # move EE to hand
     "open": 6,  # open the gripper
+    "close": 7,
+
+    # "handover": 6,  # same as "open"
+    # "over": 6,
+    # "give": 6,
+    # "cube": 6,
+    "home": 10,
+    "record": 11
 
-    # simple test
-    "test": 99
+    # # simple test
+    # "test": 99
 }