From 8eeca6a3b9dcc76156e8373aefb8c12b5f726891 Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Fri, 16 May 2025 15:57:17 +0200 Subject: [PATCH] Final demo code --- examples/reachy2-remote/boot_reachy.sh | 7 + examples/reachy2-remote/dataflow_reachy.yml | 26 +- examples/reachy2-remote/no_torque copy.yml | 45 ++++ examples/reachy2-remote/no_torque.yml | 45 ++++ examples/reachy2-remote/no_torque_remote.yml | 45 ++++ examples/reachy2-remote/parse_bbox.py | 2 +- examples/reachy2-remote/parse_pose.py | 241 +++++++++++++----- examples/reachy2-remote/parse_whisper.py | 79 +++++- .../dora-kokoro-tts/dora_kokoro_tts/main.py | 4 +- node-hub/dora-object-to-pose/src/lib.rs | 2 +- node-hub/dora-reachy2/dora_reachy2/head.py | 3 +- .../dora-reachy2/dora_reachy2/left_arm.py | 30 ++- .../dora-reachy2/dora_reachy2/mobile_base.py | 5 +- .../dora-reachy2/dora_reachy2/right_arm.py | 33 +-- 14 files changed, 451 insertions(+), 116 deletions(-) create mode 100644 examples/reachy2-remote/boot_reachy.sh create mode 100644 examples/reachy2-remote/no_torque copy.yml create mode 100644 examples/reachy2-remote/no_torque.yml create mode 100644 examples/reachy2-remote/no_torque_remote.yml diff --git a/examples/reachy2-remote/boot_reachy.sh b/examples/reachy2-remote/boot_reachy.sh new file mode 100644 index 00000000..0e422233 --- /dev/null +++ b/examples/reachy2-remote/boot_reachy.sh @@ -0,0 +1,7 @@ +ssh bedrock@reachy2-pvt02.local + + +docker exec -it core bash + + +ros2 service call /SetZuuuSafety zuuu_interfaces/srv/SetZuuuSafety "{safety_on: False}" diff --git a/examples/reachy2-remote/dataflow_reachy.yml b/examples/reachy2-remote/dataflow_reachy.yml index 26b42c42..d0a189e0 100644 --- a/examples/reachy2-remote/dataflow_reachy.yml +++ b/examples/reachy2-remote/dataflow_reachy.yml @@ -108,6 +108,8 @@ nodes: - follow_pose - raise_arm_pose - look_ahead + - emotion + - speech env: IMAGE_RESIZE_RATIO: "1.0" @@ -133,7 +135,7 @@ nodes: points: parse_whisper/points outputs: - bbox_track - - bbox_grab + - bbox_pick env: IMAGE_RESIZE_RATIO: "1.0" @@ -142,7 +144,7 @@ nodes: path: dora-sam2 inputs: image_depth: camera_depth/image_depth - boxes2d: parse_bbox/bbox_grab + boxes2d: parse_bbox/bbox_pick outputs: - masks @@ -179,6 +181,7 @@ nodes: raise_arm_pose: parse_whisper/raise_arm_pose look_ahead: parse_whisper/look_ahead translate_base: reachy-mobile-base/response_base + emotion: parse_whisper/emotion outputs: - action_r_arm - action_l_arm @@ -206,9 +209,10 @@ nodes: head/image: camera/image_left torso/image: camera_depth/image_depth torso/depth: camera_depth/depth - torso/boxes2d: parse_bbox/bbox_grab + torso/boxes2d: parse_bbox/bbox_pick torso/masks: sam2/masks original_text: dora-distil-whisper/text + emotion_text: parse_whisper/emotion parsed_text: parse_whisper/text qwenvl_text: dora-qwenvl/text cotracker/tracked_image: tracker/tracked_image @@ -216,3 +220,19 @@ nodes: env: RERUN_MEMORY_LIMIT: 25% CAMERA_PITCH: 2.40 + + - id: dora-kokoro-tts + build: pip install -e ../../node-hub/dora-kokoro-tts + path: dora-kokoro-tts + inputs: + text: parse_whisper/speech + outputs: + - audio + env: + VOICE: bm_fable + + - id: dora-pyaudio + build: pip install -e ../../node-hub/dora-pyaudio + path: dora-pyaudio + inputs: + audio: dora-kokoro-tts/audio diff --git a/examples/reachy2-remote/no_torque copy.yml b/examples/reachy2-remote/no_torque copy.yml new file mode 100644 index 00000000..5f7da238 --- /dev/null +++ b/examples/reachy2-remote/no_torque copy.yml @@ -0,0 +1,45 @@ +nodes: + - id: camera + build: pip install -e ../../node-hub/dora-reachy2 + path: dora-reachy2-camera + _unstable_deploy: + machine: macbook + inputs: + image_left: dora/timer/millis/20 + outputs: + #- image_depth + #- depth + - image_left + env: + # IMAGE_WIDTH: 640 + # IMAGE_HEIGHT: 480 + ROBOT_IP: 10.42.0.242 + + - id: camera_depth + build: pip install -e ../../node-hub/dora-reachy2 + path: dora-reachy2-camera + _unstable_deploy: + machine: macbook + inputs: + depth: dora/timer/millis/20 + outputs: + - image_depth + - depth + env: + # IMAGE_WIDTH: 640 + # IMAGE_HEIGHT: 480 + ROBOT_IP: 10.42.0.242 + + - id: plot + build: pip install -e ../../node-hub/dora-rerun + path: dora-rerun + _unstable_deploy: + machine: macbook + inputs: + torso/depth: camera_depth/depth + torso/image: camera_depth/image_depth + head/image_left: camera/image_left + #torso/depth: camera/depth + env: + RERUN_MEMORY_LIMIT: 5% + CAMERA_PITCH: 2.47 diff --git a/examples/reachy2-remote/no_torque.yml b/examples/reachy2-remote/no_torque.yml new file mode 100644 index 00000000..510c4c2a --- /dev/null +++ b/examples/reachy2-remote/no_torque.yml @@ -0,0 +1,45 @@ +nodes: + - id: camera + build: pip install -e ../../node-hub/dora-reachy2 + path: dora-reachy2-camera + _unstable_deploy: + machine: macbook + inputs: + image_left: dora/timer/millis/20 + outputs: + #- image_depth + #- depth + - image_left + env: + # IMAGE_WIDTH: 640 + # IMAGE_HEIGHT: 480 + ROBOT_IP: 172.18.131.66 + + - id: camera_depth + build: pip install -e ../../node-hub/dora-reachy2 + path: dora-reachy2-camera + _unstable_deploy: + machine: macbook + inputs: + depth: dora/timer/millis/20 + outputs: + - image_depth + - depth + env: + # IMAGE_WIDTH: 640 + # IMAGE_HEIGHT: 480 + ROBOT_IP: 172.18.131.66 + + - id: plot + build: pip install -e ../../node-hub/dora-rerun + path: dora-rerun + _unstable_deploy: + machine: macbook + inputs: + torso/depth: camera_depth/depth + torso/image: camera_depth/image_depth + head/image_left: camera/image_left + #torso/depth: camera/depth + env: + RERUN_MEMORY_LIMIT: 5% + CAMERA_PITCH: 2.40 diff --git a/examples/reachy2-remote/no_torque_remote.yml b/examples/reachy2-remote/no_torque_remote.yml new file mode 100644 index 00000000..5f7da238 --- /dev/null +++ b/examples/reachy2-remote/no_torque_remote.yml @@ -0,0 +1,45 @@ +nodes: + - id: camera + build: pip install -e ../../node-hub/dora-reachy2 + path: dora-reachy2-camera + _unstable_deploy: + machine: macbook + inputs: + image_left: dora/timer/millis/20 + outputs: + #- image_depth + #- depth + - image_left + env: + # IMAGE_WIDTH: 640 + # IMAGE_HEIGHT: 480 + ROBOT_IP: 10.42.0.242 + + - id: camera_depth + build: pip install -e ../../node-hub/dora-reachy2 + path: dora-reachy2-camera + _unstable_deploy: + machine: macbook + inputs: + depth: dora/timer/millis/20 + outputs: + - image_depth + - depth + env: + # IMAGE_WIDTH: 640 + # IMAGE_HEIGHT: 480 + ROBOT_IP: 10.42.0.242 + + - id: plot + build: pip install -e ../../node-hub/dora-rerun + path: dora-rerun + _unstable_deploy: + machine: macbook + inputs: + torso/depth: camera_depth/depth + torso/image: camera_depth/image_depth + head/image_left: camera/image_left + #torso/depth: camera/depth + env: + RERUN_MEMORY_LIMIT: 5% + CAMERA_PITCH: 2.47 diff --git a/examples/reachy2-remote/parse_bbox.py b/examples/reachy2-remote/parse_bbox.py index c1a992a4..ae621e55 100644 --- a/examples/reachy2-remote/parse_bbox.py +++ b/examples/reachy2-remote/parse_bbox.py @@ -70,7 +70,7 @@ for event in node: ) elif image_id == "image_depth": node.send_output( - "bbox_grab", + "bbox_pick", pa.array(bboxes.ravel()), metadata, ) diff --git a/examples/reachy2-remote/parse_pose.py b/examples/reachy2-remote/parse_pose.py index c9cc2c38..ab64d6c5 100644 --- a/examples/reachy2-remote/parse_pose.py +++ b/examples/reachy2-remote/parse_pose.py @@ -1,8 +1,6 @@ """TODO: Add docstring.""" -import json import os - import numpy as np import pyarrow as pa from dora import Node @@ -34,7 +32,6 @@ r_init_pose = [ ] - def wait_for_event(id, timeout=None, cache={}): """TODO: Add docstring.""" while True: @@ -74,30 +71,24 @@ for event in node: node.send_output("look", pa.array([x, y, z])) match action: - case "grab": + case "pick": if len(values) == 0: continue x = x + 0.05 z = z + 0.03 - ## Clip the Maximum and minim values for the height of the arm to avoid collision or weird movement. - trajectory = np.array( - [ - [x, y, z + 0.1, 0, 0, 0, 100], - [x, y, z, 0, 0, 0, 0], - [x, y, z + 0.1, 0, 0, 0, 0], - ], - ).ravel() - - if y < 0: - + if y < 0: # right arm + node.send_output("look", pa.array([x, y, z])) + # Send a mobile base command to move slightly left to facilitate the grasp node.send_output( "translate_base", pa.array([0.0, y + 0.3, 0, 0, 0, 0]), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) event = wait_for_event(id="translate_base") + node.send_output("look", pa.array([x, -0.3, z])) + trajectory = np.array( [ [x, -0.3, z + 0.1, 0, 0, 0, 100], @@ -106,55 +97,167 @@ for event in node: [0.3, -0.3, -0.16, 0, 0, 0, 0], ], ).ravel() + node.send_output( + "action_r_arm", + pa.array(trajectory), + metadata={"encoding": "xyzrpy", "duration": "0.3"}, + ) + event = wait_for_event(id="response_r_arm") + node.send_output("look", pa.array([x, -0.3, z])) node.send_output( - "action_r_arm", + "translate_base", + pa.array([0.0, -(y + 0.3), 0, 0, 0, 0]), + metadata={"encoding": "xyzrpy", "duration": "0.3"}, + ) + event = wait_for_event(id="translate_base") + arm_holding_object = "right" + node.send_output("look", pa.array([0.4, 0, 0])) + node.send_output("success", pa.array([True])) + + else: # left arm + # Send a mobile base command to move slightly left to facilitate the grasp + node.send_output("look", pa.array([x, y, z])) + + ## Move the base + node.send_output( + "translate_base", + pa.array([0.0, y - 0.3, 0, 0, 0, 0]), + metadata={"encoding": "xyzrpy", "duration": "0.3"}, + ) + event = wait_for_event(id="translate_base") + + node.send_output("look", pa.array([x, 0.3, z])) + trajectory = np.array( + [ + [x, 0.3, z + 0.1, 0, 0, 0, 100], + [x, 0.3, z, 0, 0, 0, 0], + [x, 0.3, z + 0.1, 0, 0, 0, 0], + [0.3, 0.3, -0.16, 0, 0, 0, 0], + ], + ).ravel() + node.send_output("look", pa.array([x, 0.3, z])) + + node.send_output( + "action_l_arm", + pa.array(trajectory), + metadata={"encoding": "xyzrpy", "duration": "0.3"}, + ) + + event = wait_for_event(id="response_l_arm") + if not ( + event is not None + and event[0] is not None + and event[0][0].as_py() + ): + node.send_output( + "action_l_arm", pa.array(trajectory), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, + ) + + event = wait_for_event(id="response_l_arm") + node.send_output( + "translate_base", + pa.array([0.0, -(y - 0.3), 0, 0, 0, 0]), + metadata={"encoding": "xyzrpy", "duration": "0.3"}, + ) + event = wait_for_event(id="translate_base") + node.send_output("look", pa.array([0.4, 0.0, 0.0])) + + arm_holding_object = "left" + node.send_output("success", pa.array([True])) + case "flip": + if len(values) == 0: + continue + x = x + 0.05 + z = z + 0.03 + + if y < 0: # right arm + node.send_output("look", pa.array([x, y, z])) + + # Send a mobile base command to move slightly left to facilitate the grasp + node.send_output( + "translate_base", + pa.array([0.0, y + 0.3, 0, 0, 0, 0]), + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) + event = wait_for_event(id="translate_base") + node.send_output("look", pa.array([x, -0.3, z])) + trajectory = np.array( + [ + [x, -0.3, z + 0.1, 0, 0, 0, 100], + [x, -0.3, z, 0, 0, 0, 0], + [x, -0.3, z + 0.1, 0, 0, 0, 0], + [x, -0.3, z + 0.1, -np.pi, 0, 0, 0], + [x, -0.3, z, -np.pi, 0, 0, 100], + [x, -0.3, z + 0.1, -np.pi, 0, 0, 100], + [x, -0.3, z + 0.1, 0, 0, 0, 100], + ], + ).ravel() + node.send_output( + "action_r_arm", + pa.array(trajectory), + metadata={"encoding": "xyzrpy", "duration": "0.8"}, + ) event = wait_for_event(id="response_r_arm") + + node.send_output("look", pa.array([x, -0.3, z])) + node.send_output( "translate_base", pa.array([0.0, -(y + 0.3), 0, 0, 0, 0]), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) event = wait_for_event(id="translate_base") arm_holding_object = "right" + node.send_output("look", pa.array([0.4, 0, 0])) node.send_output("success", pa.array([True])) - else: + else: # left arm # Send a mobile base command to move slightly left to facilitate the grasp + node.send_output("look", pa.array([x, y, z])) + + ## Move the base node.send_output( "translate_base", pa.array([0.0, y - 0.3, 0, 0, 0, 0]), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) event = wait_for_event(id="translate_base") + node.send_output("look", pa.array([x, 0.3, z])) trajectory = np.array( [ [x, 0.3, z + 0.1, 0, 0, 0, 100], [x, 0.3, z, 0, 0, 0, 0], [x, 0.3, z + 0.1, 0, 0, 0, 0], - [0.3, 0.3, -0.16, 0, 0, 0, 0], + [x, 0.3, z + 0.1, -np.pi, 0, 0, 0], + [x, 0.3, z, -np.pi, 0, 0, 100], + [x, 0.3, z + 0.1, -np.pi, 0, 0, 100], + [x, 0.3, z + 0.1, 0, 0, 0, 100], ], ).ravel() + node.send_output("look", pa.array([x, 0.3, z])) node.send_output( "action_l_arm", pa.array(trajectory), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.8"}, ) event = wait_for_event(id="response_l_arm") + node.send_output( "translate_base", pa.array([0.0, -(y - 0.3), 0, 0, 0, 0]), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) event = wait_for_event(id="translate_base") + node.send_output("look", pa.array([0.4, 0.0, 0.0])) + arm_holding_object = "left" node.send_output("success", pa.array([True])) @@ -165,13 +268,6 @@ for event in node: z = z + 0.13 ## Clip the Maximum and minim values for the height of the arm to avoid collision or weird movement. - trajectory = np.array( - [ - [x, y, z + 0.1, 0, 0, 0, 0], - [x, y, z, 0, 0, 0, 100], - [x, y, z + 0.1, 0, 0, 0, 100], - ], - ).ravel() node.send_output("look", pa.array([x, y, z])) if abs(y) > 0.5: continue @@ -179,12 +275,11 @@ for event in node: print("No arm holding object!!!") continue elif arm_holding_object == "right": - # Send a mobile base command to move slightly left to facilitate the grasp node.send_output( "translate_base", pa.array([0.0, y + 0.3, 0, 0, 0, 0]), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) event = wait_for_event(id="translate_base", timeout=10) node.send_output("look", pa.array([x, -0.3, z])) @@ -199,29 +294,48 @@ for event in node: node.send_output( "action_r_arm", pa.array(trajectory), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) event = wait_for_event(id="response_r_arm", timeout=10) + if not ( + event is not None + and event[0] is not None + and event[0][0].as_py() + ): + trajectory = np.array( + [ + [x, 0.3, -0.2, 0, 0, 0, 0], + [x, 0.3, -0.2, 0, 0, 0, 100], + [x, 0.3, -0.2, 0, 0, 0, 100], + ], + ).ravel() + + node.send_output( + "action_l_arm", + pa.array(trajectory), + metadata={"encoding": "xyzrpy", "duration": "0.3"}, + ) node.send_output( "action_r_arm", pa.array(r_init_pose), - metadata={"encoding": "jointstate", "duration": "0.6"}, + metadata={"encoding": "jointstate", "duration": "0.7"}, ) event = wait_for_event(id="response_r_arm", timeout=10) node.send_output( "translate_base", pa.array([0.0, -(y + 0.3), 0, 0, 0, 0]), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) event = wait_for_event(id="translate_base", timeout=10) node.send_output("success", pa.array([True])) + node.send_output("look", pa.array([0.4, 0.0, 0.0])) else: node.send_output( "translate_base", pa.array([0.0, y - 0.3, 0, 0, 0, 0]), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) event = wait_for_event(id="translate_base") trajectory = np.array( @@ -236,25 +350,43 @@ for event in node: node.send_output( "action_l_arm", pa.array(trajectory), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) event = wait_for_event(id="response_l_arm") + if not ( + event is not None + and event[0] is not None + and event[0][0].as_py() + ): + trajectory = np.array( + [ + [x, 0.3, -0.2, 0, 0, 0, 0], + [x, 0.3, -0.2, 0, 0, 0, 100], + [x, 0.3, -0.2, 0, 0, 0, 100], + ], + ).ravel() + + node.send_output( + "action_l_arm", + pa.array(trajectory), + metadata={"encoding": "xyzrpy", "duration": "0.3"}, + ) node.send_output( "action_l_arm", pa.array(l_init_pose), - metadata={"encoding": "jointstate", "duration": "0.6"}, + metadata={"encoding": "jointstate", "duration": "0.7"}, ) event = wait_for_event(id="response_l_arm") node.send_output( "translate_base", pa.array([0.0, -(y - 0.3), 0, 0, 0, 0]), - metadata={"encoding": "xyzrpy", "duration": "0.6"}, + metadata={"encoding": "xyzrpy", "duration": "0.3"}, ) event = wait_for_event(id="translate_base") node.send_output("success", pa.array([True])) - + node.send_output("look", pa.array([0.4, 0.0, 0.0])) elif event["id"] == "release_right": node.send_output( @@ -279,29 +411,6 @@ for event in node: pa.array(r_init_pose), metadata={"encoding": "jointstate", "duration": 1}, ) - elif event["id"] == "release_left": - node.send_output( - "action_l_arm", - pa.array( - [ - 0.4, - 0, - -0.16, - 0, - 0, - 0, - 100, - ], - ), - metadata={"encoding": "xyzrpy", "duration": "0.75"}, - ) - event, cache = wait_for_event(id="response_l_arm", cache=cache) - if event is not None and event[0]: - node.send_output( - "action_l_arm", - pa.array(l_init_pose), - metadata={"encoding": "jointstate", "duration": 1}, - ) elif event["id"] == "follow_pose": node.send_output( @@ -330,11 +439,13 @@ for event in node: metadata={"encoding": "jointstate", "duration": 2}, ) event = wait_for_event(id="response_r_arm", timeout=10) + node.send_output( "action_l_arm", pa.array(l_init_pose), metadata={"encoding": "jointstate", "duration": 2}, ) event = wait_for_event(id="response_l_arm", timeout=10) + elif event["id"] == "look_ahead": - node.send_output("look", pa.array([0.7, 0, 0])) \ No newline at end of file + node.send_output("look", pa.array([0.7, 0, 0])) diff --git a/examples/reachy2-remote/parse_whisper.py b/examples/reachy2-remote/parse_whisper.py index cecbdfd3..aa1609f3 100644 --- a/examples/reachy2-remote/parse_whisper.py +++ b/examples/reachy2-remote/parse_whisper.py @@ -1,14 +1,13 @@ """TODO: Add docstring.""" -import json import os import time +import re import numpy as np import pyarrow as pa from dora import Node -import re node = Node() @@ -18,6 +17,7 @@ queue = [] last_prompt = "" + def handle_event(text: str): global queue if "stop" in text: @@ -31,31 +31,87 @@ def handle_event(text: str): elif "raise your arms" in text: node.send_output("raise_arm_pose", pa.array([1.0])) - elif ("grab" in text and "drop" in text) or ("make a hot dog" in text): + elif ( + ("pick" in text and "place" in text) + or ("make a hot dog" in text) + or ("cook" in text) + ): if "make a hot dog" in text: - text = "grab the sausage, drop it on the black grill, grab the salad, drop it on the bread, grab the sausage, drop it on the bread" + text = "pick the sausage, place it on the black grill, wait, flip the sausage, wait, pick the sausage, place it on the bread, speak it's ready!" + elif "cook" in text: + # Match 'grill' followed by two words + match = re.search(r"\bcook\b\s+(\w+)\s+(\w+)", text) + + if match: + word1 = match.group(1) + word2 = match.group(2) + grilled_item = word1 + " " + word2 + text = f"pick {grilled_item}, place it on the black grill, wait, flip {grilled_item}, wait, pick {grilled_item} again, place it on the white plate, speak {grilled_item} is ready!" + if "," or "." in text: - prompts = re.split(r"[,.]",text) + prompts = re.split(r"[,.]", text) queue = prompts first_prompt = queue[0] queue = queue[1:] handle_event(first_prompt) - - elif "grab " in text: - text = f"Given the prompt: {text}. Output the bounding boxes for the given grabbed object" + + elif "pick " in text: + text = text.replace("can you", "") + text = text.replace("please", "") + text = text.replace("reachy", "") + + node.send_output("speech", pa.array(["I'm going to " + text])) + + text = f"Given the prompt: {text}. Output the bounding boxes for the given picked object" + node.send_output( "text", pa.array([text]), - {"image_id": "image_depth", "action": "grab"}, + {"image_id": "image_depth", "action": "pick"}, ) - elif "drop " in text: - text = f"Given the prompt: {text}. Output the bounding boxes for the place to drop the object" + elif "place " in text: + text = text.replace("can you", "") + text = text.replace("please", "") + text = text.replace("reachy", "") + + node.send_output("speech", pa.array(["I'm going to " + text])) + + text = f"Given the prompt: {text}. Output the bounding boxes for the place to place the object" node.send_output( "text", pa.array([text]), {"image_id": "image_depth", "action": "release"}, ) + elif " wait" in text: + node.send_output("speech", pa.array(["I'm going to wait for 5 seconds."])) + time.sleep(5) + if len(queue) > 0: + first_prompt = queue[0] + queue = queue[1:] + handle_event(first_prompt) + elif " speak" in text: + node.send_output("speech", pa.array([text.replace("speak ", "")])) + + if len(queue) > 0: + first_prompt = queue[0] + queue = queue[1:] + handle_event(first_prompt) + elif " flip" in text: + text = text.replace("can you", "") + text = text.replace("please", "") + text = text.replace("reachy", "") + + node.send_output("speech", pa.array(["I'm going to " + text])) + + text = f"Given the prompt: {text}. Output the bounding boxes for the flipped object" + node.send_output( + "text", + pa.array([text]), + {"image_id": "image_depth", "action": "flip"}, + ) + # elif "flip " in text: + # node.send_output("flip", pa.array([True])) elif "release left" in text: node.send_output("action_release_left", pa.array([1.0])) elif "release right" in text: @@ -90,7 +146,6 @@ def handle_event(text: str): node.send_output("points", pa.array([])) - for event in node: if event["type"] == "INPUT": if event["id"] == "text": diff --git a/node-hub/dora-kokoro-tts/dora_kokoro_tts/main.py b/node-hub/dora-kokoro-tts/dora_kokoro_tts/main.py index 7762cfca..2aefd319 100644 --- a/node-hub/dora-kokoro-tts/dora_kokoro_tts/main.py +++ b/node-hub/dora-kokoro-tts/dora_kokoro_tts/main.py @@ -7,7 +7,7 @@ from dora import Node from kokoro import KPipeline LANGUAGE = os.getenv("LANGUAGE", "en") - +VOICE = os.getenv("VOICE", "af_heart") def main(): """TODO: Add docstring.""" if LANGUAGE in ["en", "english"]: @@ -31,7 +31,7 @@ def main(): generator = pipeline( text, - voice="af_heart", # <= change voice here + voice=VOICE, # <= change voice here speed=1.2, split_pattern=r"\n+", ) diff --git a/node-hub/dora-object-to-pose/src/lib.rs b/node-hub/dora-object-to-pose/src/lib.rs index 1d322496..a39eb653 100644 --- a/node-hub/dora-object-to-pose/src/lib.rs +++ b/node-hub/dora-object-to-pose/src/lib.rs @@ -82,7 +82,7 @@ fn points_to_pose(points: &[(f32, f32, f32)]) -> Vec { _z_max, ) = points .iter() - .filter(|(_x, _y, z)| (z - mean_z).abs() < 0.07) + .filter(|(_x, _y, z)| (z - mean_z).abs() < 0.05) .fold( ( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, -10.0, 10.0, -10.0, 10., -10.0, diff --git a/node-hub/dora-reachy2/dora_reachy2/head.py b/node-hub/dora-reachy2/dora_reachy2/head.py index 7374a697..4a70b815 100644 --- a/node-hub/dora-reachy2/dora_reachy2/head.py +++ b/node-hub/dora-reachy2/dora_reachy2/head.py @@ -50,8 +50,9 @@ def main(): [roll, _pitch, yaw] = reachy.head.get_current_positions() if "look" in event["id"]: [x, y, z] = event["value"].to_numpy() + duration = event["metadata"].get("duration", 0.5) reachy.head.cancel_all_goto() - reachy.head.look_at(x, y, z, duration=0.5) + reachy.head.look_at(x, y, z, duration=float(duration)) if reachy.head is not None: reachy.head.turn_off() diff --git a/node-hub/dora-reachy2/dora_reachy2/left_arm.py b/node-hub/dora-reachy2/dora_reachy2/left_arm.py index e255d4be..3920f746 100644 --- a/node-hub/dora-reachy2/dora_reachy2/left_arm.py +++ b/node-hub/dora-reachy2/dora_reachy2/left_arm.py @@ -12,44 +12,46 @@ from scipy.spatial.transform import Rotation ROBOT_IP = os.getenv("ROBOT_IP", "127.0.0.1") -def l_arm_go_to_mixed_angles(reachy, x, y, z): +def l_arm_go_to_mixed_angles(reachy, x, y, z, roll): """TODO: Add docstring.""" for theta in range(-46, -46, 10): + + theta = -60 r = Rotation.from_euler("zyx", [0, theta, 0], degrees=True) transform = np.eye(4) transform[:3, :3] = r.as_matrix() transform[:3, 3] = [x, y, z] try: - return reachy.l_arm.inverse_kinematics(transform) - + joint=reachy.l_arm.inverse_kinematics(transform) + if roll: + joint[-1] = joint[-1] - 180 + return joint except ValueError: continue for yaw in range(0, 30, 10): ## First try turning left - pitch = -46 + pitch = -60 + r = Rotation.from_euler("ZYX", (-yaw, 0, 0), degrees=True) * Rotation.from_euler( "ZYX", (0, pitch, 0), degrees=True, - ) + ) transform = np.eye(4) transform[:3, :3] = r.as_matrix() transform[:3, 3] = [x, y, z] try: - return reachy.l_arm.inverse_kinematics(transform) + joint=reachy.l_arm.inverse_kinematics(transform) + if roll: + joint[-1] = joint[-1] - 180 + return joint except ValueError: pass - try: - return reachy.l_arm.inverse_kinematics(transform) - - except ValueError: - continue - print("Left arm: No solution found for x, y, z: ", x, y, z) return [] @@ -103,11 +105,11 @@ def main(): x = value[0] y = value[1] z = value[2] - _r = value[3] + r = value[3] _p = value[4] _y = value[5] gripper = value[6] - joints = l_arm_go_to_mixed_angles(reachy, x, y, z) + joints = l_arm_go_to_mixed_angles(reachy, x, y, z, np.rad2deg(r)) response_ik = len(joints) > 0 if response_ik: joint_values.append((joints, gripper)) diff --git a/node-hub/dora-reachy2/dora_reachy2/mobile_base.py b/node-hub/dora-reachy2/dora_reachy2/mobile_base.py index 3f604972..8d73cd72 100644 --- a/node-hub/dora-reachy2/dora_reachy2/mobile_base.py +++ b/node-hub/dora-reachy2/dora_reachy2/mobile_base.py @@ -27,7 +27,10 @@ def main(): reachy.mobile_base.send_speed_command() elif event["id"] == "translate_base": [x, y, _z, _rx, _ry, rz] = event["value"].to_numpy() - reachy.mobile_base.translate_by(x, y, wait=True) + duration = float(event["metadata"].get("duration", 1)) + + if abs(y) <1: + reachy.mobile_base.translate_by(x, y, wait=True, ) node.send_output("response_base", pa.array([True])) elif event["id"] == "action_whisper": diff --git a/node-hub/dora-reachy2/dora_reachy2/right_arm.py b/node-hub/dora-reachy2/dora_reachy2/right_arm.py index b7750a21..9b935d64 100644 --- a/node-hub/dora-reachy2/dora_reachy2/right_arm.py +++ b/node-hub/dora-reachy2/dora_reachy2/right_arm.py @@ -9,47 +9,48 @@ from dora import Node from reachy2_sdk import ReachySDK from scipy.spatial.transform import Rotation -ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.242") +ROBOT_IP = os.getenv("ROBOT_IP", "127.0.0.1") -def r_arm_go_to_mixed_angles(reachy, x, y, z): +def r_arm_go_to_mixed_angles(reachy, x, y, z, roll): """TODO: Add docstring.""" for theta in range(-46, -46, 10): + theta = -60 + r = Rotation.from_euler("zyx", [0, theta, 0], degrees=True) transform = np.eye(4) transform[:3, :3] = r.as_matrix() transform[:3, 3] = [x, y, z] try: - return reachy.r_arm.inverse_kinematics(transform) - + joint=reachy.r_arm.inverse_kinematics(transform) + if roll: + joint[-1] = joint[-1] + 180 + return joint except ValueError: continue for yaw in range(0, 30, 10): ## First try turning left - pitch = -46 + pitch = -60 r = Rotation.from_euler("ZYX", (yaw, 0, 0), degrees=True) * Rotation.from_euler( "ZYX", (0, pitch, 0), degrees=True, - ) + ) transform = np.eye(4) transform[:3, :3] = r.as_matrix() transform[:3, 3] = [x, y, z] try: - return reachy.r_arm.inverse_kinematics(transform) - except ValueError: - pass - - try: - return reachy.r_arm.inverse_kinematics(transform) + joint=reachy.r_arm.inverse_kinematics(transform) + if roll: + joint[-1] = joint[-1] + 180 + return joint except ValueError: - continue - + pass print("Right arm: No solution found for x, y, z: ", x, y, z) return [] @@ -104,11 +105,11 @@ def main(): x = value[0] y = value[1] z = value[2] - _r = value[3] + r = value[3] _p = value[4] _y = value[5] gripper = value[6] - joints = r_arm_go_to_mixed_angles(reachy, x, y, z) + joints = r_arm_go_to_mixed_angles(reachy, x, y, z, np.rad2deg(r)) response_ik = len(joints) > 0 if response_ik: joint_values.append((joints, gripper))