From 0494ef753b41abd1f0c914edcfefeb8dd9c8acd4 Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Sun, 11 May 2025 21:20:04 +0200 Subject: [PATCH] Minor adjustement --- examples/reachy2-remote/dataflow_reachy.yml | 2 ++ examples/reachy2-remote/parse_point.py | 12 ++++-------- examples/reachy2-remote/parse_pose.py | 19 +++++++++++-------- examples/reachy2-remote/parse_whisper.py | 18 ++++++++++++++---- 4 files changed, 31 insertions(+), 20 deletions(-) diff --git a/examples/reachy2-remote/dataflow_reachy.yml b/examples/reachy2-remote/dataflow_reachy.yml index 8f439581..5d31ece7 100644 --- a/examples/reachy2-remote/dataflow_reachy.yml +++ b/examples/reachy2-remote/dataflow_reachy.yml @@ -106,6 +106,7 @@ nodes: - action_release_right - follow_pose - raise_arm_pose + - look_ahead env: IMAGE_RESIZE_RATIO: "1.0" @@ -173,6 +174,7 @@ nodes: release_right: parse_whisper/action_release_right follow_pose: parse_whisper/follow_pose raise_arm_pose: parse_whisper/raise_arm_pose + look_ahead: parse_whisper/look_ahead translate_base: reachy-mobile-base/response_base outputs: - action_r_arm diff --git a/examples/reachy2-remote/parse_point.py b/examples/reachy2-remote/parse_point.py index 6b7ca4ec..d1593c53 100644 --- a/examples/reachy2-remote/parse_point.py +++ b/examples/reachy2-remote/parse_point.py @@ -13,6 +13,7 @@ node = Node() IMAGE_RESIZE_RATIO = float(os.getenv("IMAGE_RESIZE_RATIO", "1.0")) arrive_time = time.time() +arrived = False for event in node: if event["type"] == "INPUT": @@ -50,15 +51,10 @@ for event in node: x = 0.8 elif x_distance > (height * 0.25): x = 0.61 + elif x_distance > (height * 0.1): + x = 0.3 else: x = 0 - - # if x_distance < (height * 0.25): - # print("ARRIVED!") - # time.sleep(1.0) - # if time.time() - arrive_time > 4.0: - # node.send_output("arrived", pa.array([])) - # arrive_time = time.time() - # Action + node.send_output("arrived", pa.array([])) action = pa.array([x, y, 0, 0, 0, rz]) node.send_output("action", action) diff --git a/examples/reachy2-remote/parse_pose.py b/examples/reachy2-remote/parse_pose.py index daf59c14..11b23fd9 100644 --- a/examples/reachy2-remote/parse_pose.py +++ b/examples/reachy2-remote/parse_pose.py @@ -98,7 +98,7 @@ cache = {} ## ---- INIT --- -node.send_output("look", pa.array([4.0, 0, 0])) +node.send_output("look", pa.array([1.0, 0, 0])) for event in node: @@ -165,6 +165,7 @@ for event in node: [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])) @@ -218,6 +219,7 @@ for event in node: [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() @@ -237,7 +239,7 @@ for event in node: case "release": if len(values) == 0: continue - x = x + 0.01 + x = x - 0.02 z = z + 0.13 ## Clip the Maximum and minim values for the height of the arm to avoid collision or weird movement. @@ -281,7 +283,7 @@ for event in node: pa.array([0.0, y + 0.3, 0, 0, 0, 0]), metadata={"encoding": "xyzrpy", "duration": "1"}, ) - event = wait_for_event(id="translate_base") + event = wait_for_event(id="translate_base", timeout=10) node.send_output("look", pa.array([x, -0.3, z])) trajectory = np.array( [ @@ -296,22 +298,21 @@ for event in node: pa.array(trajectory), metadata={"encoding": "xyzrpy", "duration": "0.75"}, ) - event = wait_for_event(id="response_r_arm") + event = wait_for_event(id="response_r_arm", timeout=10) node.send_output( "action_r_arm", pa.array(r_init_pose), metadata={"encoding": "jointstate", "duration": 1}, ) - event = wait_for_event(id="response_r_arm") + 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": "1"}, ) - event = wait_for_event(id="translate_base") + event = wait_for_event(id="translate_base", timeout=10) else: - y += 0.03 node.send_output( "action_l_arm", pa.array(trajectory), @@ -332,7 +333,7 @@ for event in node: ) arm_holding_object = None else: - print("Failed") + print("----------------------Failed------------------") # Send a mobile base command to move slightly left to facilitate the grasp node.send_output( "translate_base", @@ -450,3 +451,5 @@ for event in node: 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 diff --git a/examples/reachy2-remote/parse_whisper.py b/examples/reachy2-remote/parse_whisper.py index 2eb338d6..dc9f238f 100644 --- a/examples/reachy2-remote/parse_whisper.py +++ b/examples/reachy2-remote/parse_whisper.py @@ -60,6 +60,7 @@ for event in node: text = f"Given the prompt: {text}. Output the bounding boxes for the given followed object" node.send_output("text", pa.array([text]), {"image_id": "image_left"}) node.send_output("follow_pose", pa.array([1.0])) + node.send_output("look_ahead", pa.array([1.0])) elif "raise your arms" in text: node.send_output("raise_arm_pose", pa.array([1.0])) elif "grab " in text: @@ -70,22 +71,24 @@ for event in node: {"image_id": "image_depth", "action": "grab"}, ) elif "get " in text: - continue + text = f"Given the prompt: {text}. Output the bounding boxes for the object" node.send_output( "text", pa.array([text]), {"image_id": "image_left", "action": "grab"}, ) + node.send_output("look_ahead", pa.array([1.0])) last_prompt = text elif "put " in text: - continue + text = f"Given the prompt: {text}. Output the bounding boxes for the place to put the object" node.send_output( "text", pa.array([text]), {"image_id": "image_left", "action": "release"}, ) + node.send_output("look_ahead", pa.array([1.0])) last_prompt = text elif "drop " in text: text = f"Given the prompt: {text}. Output the bounding boxes for the place to drop the object" @@ -106,6 +109,10 @@ for event in node: action = pa.array([0.0, 0, 0, 0, 0, -np.deg2rad(30)]) node.send_output("action", action) node.send_output("points", pa.array([])) + elif "turn around" in text: + action = pa.array([0.0, 0, 0, 0, 0, -np.deg2rad(180)]) + node.send_output("action", action) + node.send_output("points", pa.array([])) elif "move left" in text: action = pa.array([0.0, 0.2, 0, 0, 0, 0]) node.send_output("action", action) @@ -125,18 +132,21 @@ for event in node: elif event["id"] == "arrived": text = last_prompt print("received arrived message") - node.send_output("points", pa.array([])) + time.sleep(1.0) # Sync image if "get " in text: - text = f"Given the prompt: {text}. Output the bounding boxes for the place to put the object" + node.send_output("points", pa.array([])) + text = f"Given the prompt: {text}. Output the bounding boxes for the grabbed object" node.send_output( "text", pa.array([text]), {"image_id": "image_depth", "action": "grab"}, ) elif "put " in text: + node.send_output("points", pa.array([])) text = f"Given the prompt: {text}. Output the bounding boxes for the place to put the object" node.send_output( "text", pa.array([text]), {"image_id": "image_depth", "action": "release"}, ) + last_prompt = ""