diff --git a/README.md b/README.md index 299330b..ece7df5 100644 --- a/README.md +++ b/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/ -python imu_test.py +python3 mini_bdx_runtime/mini_bdx_runtime/raw_imu.py ``` +You can also run `python3 scripts/imu_server.py` on the robot and `python3 scripts/imu_client.py --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` diff --git a/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py b/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py index e3a446d..6758e08 100644 --- a/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py +++ b/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__": diff --git a/scripts/imu_client.py b/scripts/imu_client.py index 31e8b0f..6cc367b 100644 --- a/scripts/imu_client.py +++ b/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 - -from mini_bdx_runtime.rl_utils import quat_rotate_inverse +import argparse 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") - - # 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) + fv.pushFrame(pose, "pose") 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() diff --git a/scripts/imu_server.py b/scripts/imu_server.py index cde27be..e537eec 100644 --- a/scripts/imu_server.py +++ b/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) - diff --git a/scripts/imu_test.py b/scripts/imu_test.py deleted file mode 100644 index 4691df1..0000000 --- a/scripts/imu_test.py +++ /dev/null @@ -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)