cleaning up, fixing readme, adding infos

README.md

@@ -85,7 +85,7 @@ The led on the controller should stop blinking and stay on.
 You can test that it's working by running
 
 ```bash
-python3 scripts/test_xbox_controller.py
+python3 mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py
 ```
 
 ## Speaker wiring and configuration
@@ -118,10 +118,13 @@ pip install -e .
 ## Test the IMU
 
 ```bash
-cd scripts/
+python3 mini_bdx_runtime/mini_bdx_runtime/raw_imu.py
-python imu_test.py
 ```
 
+You can also run `python3 scripts/imu_server.py` on the robot and `python3 scripts/imu_client.py --ip <robot_ip>` on your computer to check that the frame is oriented correctly. 
+
+> To find the ip address of the robot, run `ifconfig` on the robot
+
 ## Find the joints offsets
 
 This script will guide you through finding the joints offsets of your robot, that you can then write in `rustypot_position_hwi.py` in `self.joints_offsets`

mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py

@@ -84,7 +84,7 @@ class XBoxController:
             last_commands[0] = 0.0
             last_commands[1] = 0.0
             last_commands[2] = 0.0
-            last_commands[3] = 0.0 # neck pitch 0 for now
+            last_commands[3] = 0.0  # neck pitch 0 for now
 
             head_yaw = l_x
             head_pitch = l_y
@@ -123,21 +123,40 @@ class XBoxController:
             if self.p1.get_button(4):  # Y button
                 self.head_control_mode = not self.head_control_mode
 
-
         pygame.event.pump()  # process event queue
 
-        return np.around(last_commands, 3), A_pressed, X_pressed, left_trigger, right_trigger
+        return (
+            np.around(last_commands, 3),
+            A_pressed,
+            X_pressed,
+            left_trigger,
+            right_trigger,
+        )
 
     def get_last_command(self):
         A_pressed = False
         X_pressed = False
 
         try:
-            self.last_commands, A_pressed, X_pressed, self.last_left_trigger, self.last_right_trigger = self.cmd_queue.get(False)  # non blocking
+            (
+                self.last_commands,
+                A_pressed,
+                X_pressed,
+                self.last_left_trigger,
+                self.last_right_trigger,
+            ) = self.cmd_queue.get(
+                False
+            )  # non blocking
         except Exception:
             pass
 
-        return self.last_commands, A_pressed, X_pressed, self.last_left_trigger, self.last_right_trigger
+        return (
+            self.last_commands,
+            A_pressed,
+            X_pressed,
+            self.last_left_trigger,
+            self.last_right_trigger,
+        )
 
 
 if __name__ == "__main__":

scripts/imu_client.py

@@ -6,8 +6,7 @@ from queue import Queue
 from threading import Thread
 from scipy.spatial.transform import Rotation as R
 from FramesViewer.viewer import Viewer
-
+import argparse
-from mini_bdx_runtime.rl_utils import quat_rotate_inverse
 
 
 class IMUClient:
@@ -52,37 +51,26 @@ class IMUClient:
 
 if __name__ == "__main__":
 
-    client = IMUClient("192.168.248.253")
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--ip", type=str, required=True, help="IP address of the robot")
+    args = parser.parse_args()
+
+    client = IMUClient(args.ip)
+
     fv = Viewer()
     fv.start()
     pose = np.eye(4)
     pose[:3, 3] = [0.1, 0.1, 0.1]
-    projected_gravities = []
     try:
         while True:
             quat = client.get_imu()
             try:
                 rot_mat = R.from_quat(quat).as_matrix()
                 pose[:3, :3] = rot_mat
-                fv.pushFrame(pose, "aze")
+                fv.pushFrame(pose, "pose")
-
-                # euler = R.from_quat(quat).as_euler("xyz")
-                # euler[2] = 0
-                # quat = R.from_euler("xyz", euler).as_quat()
-
-                projected_gravity = quat_rotate_inverse(quat, [0, 0, -1])
-                projected_gravities.append(projected_gravity)
             except Exception as e:
                 print("error", e)
                 pass
             time.sleep(1 / 30)
     except KeyboardInterrupt:
         pass
-
-    # plot projected_gravities [[x, y, z], [x, y, z], ...]
-
-    # from matplotlib import pyplot as plt
-
-    # projected_gravities = np.array(projected_gravities)
-    # plt.plot(projected_gravities)
-    # plt.show()

scripts/imu_server.py

@@ -8,14 +8,15 @@ import time
 import argparse
 
 
-
 class IMUServer:
     def __init__(self, imu=None):
         self.host = "0.0.0.0"
         self.port = 1234
 
         self.server_socket = socket.socket()
-        self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # enable address reuse
+        self.server_socket.setsockopt(
+            socket.SOL_SOCKET, socket.SO_REUSEADDR, 1
+        )  # enable address reuse
 
         self.server_socket.bind((self.host, self.port))
 
@@ -27,7 +28,6 @@ class IMUServer:
 
         Thread(target=self.run, daemon=True).start()
 
-
     def run(self):
         while not self.stop:
             self.server_socket.listen(1)
@@ -41,8 +41,7 @@ class IMUServer:
                     time.sleep(1 / 30)
             except:
                 pass
-        
+
-    
         self.server_socket.close()
         print("thread closed")
         time.sleep(1)
@@ -61,4 +60,3 @@ if __name__ == "__main__":
         imu_server.stop = True
 
     time.sleep(2)
-

scripts/imu_test.py

@@ -1,10 +0,0 @@
-from mini_bdx_runtime.imu import Imu
-import time
-
-imu = Imu(sampling_freq=30)
-while True:
-    orientation_quat = imu.get_data()
-
-    print(orientation_quat)
-
-    time.sleep(1 / 30)