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

Parrot control

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)
-

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,)

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",

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

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
+        self.default_rot = default_rot  # rotation as quaternion

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
+        return output