From 571f24c2c3b8765ef9796dc17e1c221c0527c935 Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 8 Apr 2025 10:36:31 +0200 Subject: [PATCH] working ! --- mini_bdx_runtime/mini_bdx_runtime/camera.py | 30 +++++++++++++ scripts/cam_test.py | 14 +++++- scripts/fc_test.py | 47 +++++++++------------ scripts/test2.py | 9 ++-- scripts/v2_rl_walk_mujoco.py | 18 ++++---- 5 files changed, 79 insertions(+), 39 deletions(-) create mode 100644 mini_bdx_runtime/mini_bdx_runtime/camera.py diff --git a/mini_bdx_runtime/mini_bdx_runtime/camera.py b/mini_bdx_runtime/mini_bdx_runtime/camera.py new file mode 100644 index 0000000..68b59b7 --- /dev/null +++ b/mini_bdx_runtime/mini_bdx_runtime/camera.py @@ -0,0 +1,30 @@ +from picamzero import Camera +import cv2 +import base64 +import os + +class Cam: + def __init__(self): + self.cam = Camera() + + def get_encoded_image(self): + im = self.cam.capture_array() + im = cv2.resize(im, (512, 512)) + im = cv2.cvtColor(im, cv2.COLOR_BGR2RGB) + im = cv2.rotate(im, cv2.ROTATE_90_CLOCKWISE) + + cv2.imwrite("/home/bdxv2/aze.jpg", im) + + return self.encode_image("/home/bdxv2/aze.jpg") + + + + # def encode_image(self, image): + # return base64.b64encode(image).decode("utf-8") + + def encode_image(self, image_path: str): + # check if the image exists + if not os.path.exists(image_path): + raise FileNotFoundError(f"Image file not found: {image_path}") + with open(image_path, "rb") as image_file: + return base64.b64encode(image_file.read()).decode("utf-8") diff --git a/scripts/cam_test.py b/scripts/cam_test.py index 22f1e36..b75f311 100644 --- a/scripts/cam_test.py +++ b/scripts/cam_test.py @@ -1,6 +1,18 @@ from picamzero import Camera +import cv2 +print("Initializing camera ...") cam = Camera() +# cam.still_size = (512, 512) +print("Camera initialized") +im = cam.capture_array() +im = cv2.resize(im, (512, 512)) +im = cv2.cvtColor(im, cv2.COLOR_BGR2RGB) -cam.take_photo("/home/bdxv2/aze.jpg") +cv2.imwrite("/home/bdxv2/aze.jpg", im) +# cv2.imshow("Image", im) +# cv2.waitKey(0) +# cv2.destroyAllWindows() + +# cam.take_photo("/home/bdxv2/aze.jpg") diff --git a/scripts/fc_test.py b/scripts/fc_test.py index 07fd6e4..4155841 100644 --- a/scripts/fc_test.py +++ b/scripts/fc_test.py @@ -8,7 +8,7 @@ import base64 from v2_rl_walk_mujoco import RLWalk from threading import Thread import cv2 -from picamzero import Camera +from mini_bdx_runtime.camera import Cam # TODO mission : find an object ? @@ -16,7 +16,7 @@ from picamzero import Camera # Your Tools class class Tools: def __init__(self): - self.cam = Camera() + self.cam = Cam() self.rl_walk = RLWalk( "/home/bdxv2/BEST_WALK_ONNX_2.onnx", cutoff_frequency=40, @@ -40,16 +40,9 @@ class Tools: # os.system(command) # return url - # Function to encode the image as base64 - def encode_image(self, image_path: str): - # check if the image exists - if not os.path.exists(image_path): - raise FileNotFoundError(f"Image file not found: {image_path}") - with open(image_path, "rb") as image_file: - return base64.b64encode(image_file.read()).decode("utf-8") - def move_forward(self, seconds=1): - seconds = min(seconds, 3) + def move_forward(self, seconds=2): + seconds = max(2, min(seconds, 5)) print(f"Moving forward for {seconds} seconds") self.rl_walk.last_commands[0] = 0.15 time.sleep(seconds) @@ -57,8 +50,8 @@ class Tools: print("Stopped moving forward") return f"Moved forward for {seconds} seconds successfully" - def turn_left(self, seconds=1): - seconds = min(seconds, 3) + def turn_left(self, seconds=2): + seconds = max(2, min(seconds, 5)) print(f"Turning left for {seconds} seconds") self.rl_walk.last_commands[2] = 1.0 time.sleep(seconds) @@ -66,8 +59,8 @@ class Tools: print("Stopped turning left") return f"Turned left for {seconds} seconds successfully" - def turn_right(self, seconds=1): - seconds = min(seconds, 3) + def turn_right(self, seconds=2): + seconds = max(2, min(seconds, 5)) print(f"Turning right for {seconds} seconds") self.rl_walk.last_commands[2] = -1.0 time.sleep(seconds) @@ -75,8 +68,8 @@ class Tools: print("Stopped turning right") return f"Turned right for {seconds} seconds successfully" - def move_backward(self, seconds=1): - seconds = min(seconds, 3) + def move_backward(self, seconds=2): + seconds = max(2, min(seconds, 5)) print(f"Moving backward for {seconds} seconds") self.rl_walk.last_commands[0] = -0.15 time.sleep(seconds) @@ -86,11 +79,8 @@ class Tools: def take_picture(self): # https://projects.raspberrypi.org/en/projects/getting-started-with-picamera/5 - self.cam.take_photo("/home/bdxv2/aze.jpg") - path = None # TODO cam.take_photo(f"{home_dir}/Desktop/new_image.jpg") - # path = "/home/antoine/aze.jpg" print("Taking a picture...") - image64 = self.encode_image(path) + image64 = self.cam.get_encoded_image() url = ("data:image/jpeg;base64," + image64,) time.sleep(1) print("Picture taken") @@ -110,7 +100,7 @@ openai_tools = [ "properties": { "seconds": { "type": "integer", - "description": "Number of seconds to move forward (max 3)", + "description": "Number of seconds to move forward (min 2, max 5)", } }, "required": ["seconds"], @@ -126,7 +116,7 @@ openai_tools = [ "properties": { "seconds": { "type": "integer", - "description": "Number of seconds to move backward (max 3)", + "description": "Number of seconds to move backward (min 2, max 5)", } }, "required": ["seconds"], @@ -142,7 +132,7 @@ openai_tools = [ "properties": { "seconds": { "type": "integer", - "description": "Number of seconds to turn left (max 3)", + "description": "Number of seconds to turn left (min 2, max 5)", } }, "required": ["seconds"], @@ -158,7 +148,7 @@ openai_tools = [ "properties": { "seconds": { "type": "integer", - "description": "Number of seconds to turn right (max 3)", + "description": "Number of seconds to turn right (min 2, max 5)", } }, "required": ["seconds"], @@ -192,15 +182,20 @@ messages = [ "content": ( "You are a cute little biped robot that can move around using the following tools: " "`move_forward`, `move_backward`, `turn_left`, `turn_right` and 'take_picture'. " + "moving forward for 1 second will make you move forward by about 15 centimeters" + "turning for 1 second will make you turn about 45 degrees" "You can only perform one action at a time. If multiple actions are needed, call them step by step." "Explain what you are doing along the way" "Always start by taking a picture of the environment so you can see where you are. " "When you take a picture, describe what you see in the image. " + "make sure not to hit any walls or objects. Take pictures regularly to know where you are." + "When given a goal to find something, if you found it, navigate to be in front of it, facing it" + "Maybe it's a good idea to turn 360 degrees to check all directions" ), }, { "role": "user", - "content": "Explore your environment, make sure not to hit any walls or objects. Take pictures regularly to know where you are.", + "content": "Find the yellow vaccum cleaner !", }, ] diff --git a/scripts/test2.py b/scripts/test2.py index bfedfa7..b9f28f3 100644 --- a/scripts/test2.py +++ b/scripts/test2.py @@ -1,6 +1,9 @@ from openai import OpenAI import os import base64 +from mini_bdx_runtime.camera import Cam + +cam = Cam() client = OpenAI() @@ -25,8 +28,8 @@ def encode_image(image_path: str): # }], # ) -image64 = encode_image("/home/antoine/aze.jpg") - +# image64 = encode_image("/home/antoine/aze.jpg") +image64 = cam.get_encoded_image() response = client.responses.create( model="gpt-4o-mini", input=[{ @@ -41,4 +44,4 @@ response = client.responses.create( }], ) -print(response.output_text) \ No newline at end of file +print(response.output_text) diff --git a/scripts/v2_rl_walk_mujoco.py b/scripts/v2_rl_walk_mujoco.py index 6a6b93f..4d5e08c 100644 --- a/scripts/v2_rl_walk_mujoco.py +++ b/scripts/v2_rl_walk_mujoco.py @@ -12,7 +12,7 @@ from mini_bdx_runtime.xbox_controller import XBoxController from mini_bdx_runtime.feet_contacts import FeetContacts from mini_bdx_runtime.eyes import Eyes from mini_bdx_runtime.sounds import Sounds -from mini_bdx_runtime.antennas import Antennas +# from mini_bdx_runtime.antennas import Antennas from mini_bdx_runtime.projector import Projector from mini_bdx_runtime.rl_utils import make_action_dict, LowPassActionFilter @@ -123,7 +123,7 @@ class RLWalk: self.paused = False self.sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/") - self.antennas = Antennas() + # self.antennas = Antennas() self.command_freq = 20 # hz if self.commands: @@ -225,8 +225,8 @@ class RLWalk: self.sounds.play_random_sound() self.projector.switch() - self.antennas.set_position_left(right_trigger) - self.antennas.set_position_right(left_trigger) + # self.antennas.set_position_left(right_trigger) + # self.antennas.set_position_right(left_trigger) if A_pressed and not self.paused: self.paused = True @@ -305,11 +305,11 @@ class RLWalk: took = time.time() - t # print("Full loop took", took, "fps : ", np.around(1 / took, 2)) - if (1 / self.control_freq - took) < 0: - print( - "Policy control budget exceeded by", - np.around(took - 1 / self.control_freq, 3), - ) + # if (1 / self.control_freq - took) < 0: + # print( + # "Policy control budget exceeded by", + # np.around(took - 1 / self.control_freq, 3), + # ) time.sleep(max(0, 1 / self.control_freq - took)) except KeyboardInterrupt: