From a1c17aa766d071879dfb868386775cc2edc023f0 Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Tue, 18 Feb 2025 17:45:11 +0100 Subject: [PATCH] Minor fix and improvement --- examples/reachy2/demo-dev.yml | 5 +-- examples/reachy2/state_machine.py | 12 ++++--- .../dora-qwen2-5-vl/dora_qwen2_5_vl/main.py | 26 +++++++--------- .../dora-reachy2/dora_reachy2/left_arm.py | 31 ++++++++----------- .../dora-reachy2/dora_reachy2/right_arm.py | 30 ++++++++---------- 5 files changed, 49 insertions(+), 55 deletions(-) diff --git a/examples/reachy2/demo-dev.yml b/examples/reachy2/demo-dev.yml index b98de107..cac272ae 100755 --- a/examples/reachy2/demo-dev.yml +++ b/examples/reachy2/demo-dev.yml @@ -98,11 +98,12 @@ nodes: image_depth: reachy-camera/image_depth image_left: reachy-camera/image_left text_1: dora/timer/millis/500 - text_2: state_machine/text_vlm + text_2: + source: state_machine/text_vlm + queue_size: 10 outputs: - text env: - #ADAPTER_PATH: /home/peter/Documents/work/LLaMA-Factory/saves/qwen2.5_vl-felix/lora/sft/checkpoint-558 DEFAULT_QUESTION: grab human. IMAGE_RESIZE_RATIO: "0.5" # ACTIVATION_WORDS: grab pick give output take catch grabs picks gives output takes catches have diff --git a/examples/reachy2/state_machine.py b/examples/reachy2/state_machine.py index 869aa605..2146a889 100644 --- a/examples/reachy2/state_machine.py +++ b/examples/reachy2/state_machine.py @@ -195,7 +195,9 @@ while True: if y < 0: node.send_output( - "action_r_arm", pa.array(trajectory), metadata={"encoding": "xyzrpy"} + "action_r_arm", + pa.array(trajectory), + metadata={"encoding": "xyzrpy", "duration": "0.75"}, ) event = wait_for_event(id="response_r_arm")[0].as_py() if event: @@ -207,13 +209,15 @@ while True: node.send_output( "action_r_arm", pa.array(r_default_pose), - metadata={"encoding": "jointstate"}, + metadata={"encoding": "jointstate", "duration": "0.75"}, ) event = wait_for_event(id="response_r_arm") else: y += 0.03 node.send_output( - "action_l_arm", pa.array(trajectory), metadata={"encoding": "xyzrpy"} + "action_l_arm", + pa.array(trajectory), + metadata={"encoding": "xyzrpy", "duration": "0.75"}, ) event = wait_for_event(id="response_l_arm")[0].as_py() if event: @@ -225,7 +229,7 @@ while True: node.send_output( "action_l_arm", pa.array(l_default_pose), - metadata={"encoding": "jointstate"}, + metadata={"encoding": "jointstate", "duration": "0.75"}, ) event = wait_for_event(id="response_l_arm") diff --git a/node-hub/dora-qwen2-5-vl/dora_qwen2_5_vl/main.py b/node-hub/dora-qwen2-5-vl/dora_qwen2_5_vl/main.py index 42eab52a..c5a9cdb1 100644 --- a/node-hub/dora-qwen2-5-vl/dora_qwen2_5_vl/main.py +++ b/node-hub/dora-qwen2-5-vl/dora_qwen2_5_vl/main.py @@ -140,20 +140,18 @@ def main(): node = Node() frames = {} - history = [ - { - "role": "system", - "content": [ - {"type": "text", "text": SYSTEM_PROMPT}, - ], - }, - { - "role": "user", - "content": [ - {"type": "text", "text": DEFAULT_QUESTION}, - ], - }, - ] + if SYSTEM_PROMPT: + history = [ + { + "role": "system", + "content": [ + {"type": "text", "text": SYSTEM_PROMPT}, + ], + }, + ] + else: + history = [] + cached_text = DEFAULT_QUESTION select_image = None past_key_values = None diff --git a/node-hub/dora-reachy2/dora_reachy2/left_arm.py b/node-hub/dora-reachy2/dora_reachy2/left_arm.py index ac9978a2..442f35a6 100644 --- a/node-hub/dora-reachy2/dora_reachy2/left_arm.py +++ b/node-hub/dora-reachy2/dora_reachy2/left_arm.py @@ -64,12 +64,18 @@ def l_arm_go_to_mixed_angles(reachy, x, y, z): return [] -def manage_gripper(reachy, gripper, grasp, initial_opening=0.0): - if initial_opening == gripper: +def manage_gripper(reachy, gripper, grasp): + if gripper == 100 and reachy.r_arm.gripper.get_current_opening() == 100: return True ## If the gripper is already half opened and we're trying to close it, it's probably already closed - elif gripper == 0.0 and (initial_opening < 98) and grasp: - print("Gripper already in grasp position") + elif ( + gripper == 0.0 + and ( + reachy.r_arm.gripper.get_current_opening() < 98 + and reachy.r_arm.gripper.get_current_opening() > 2 + ) + and grasp + ): return True if gripper == 0.0: reachy.l_arm.gripper.close() @@ -99,11 +105,10 @@ def main(): values: np.array = event["value"].to_numpy(zero_copy_only=False) encoding = event["metadata"]["encoding"] wait = event["metadata"].get("wait", True) - duration = event["metadata"].get("duration", 1) + duration = float(event["metadata"].get("duration", 1)) grasp = event["metadata"].get("grasp", True) if encoding == "xyzrpy": values = values.reshape((-1, 7)) - print(values) joint_values = [] for value in values: x = value[0] @@ -130,12 +135,7 @@ def main(): else: for joint, gripper in joint_values: reachy.l_arm.goto(joint, duration=duration, wait=wait) - response_gripper = manage_gripper( - reachy, - gripper, - grasp, - reachy.l_arm.gripper.get_current_opening(), - ) + response_gripper = manage_gripper(reachy, gripper, grasp) if not response_gripper: node.send_output( "response_l_arm", @@ -153,12 +153,7 @@ def main(): gripper = value[7] reachy.l_arm.goto(joints, duration=duration, wait=wait) - manage_gripper( - reachy, - gripper, - grasp, - reachy.l_arm.gripper.get_current_opening(), - ) + manage_gripper(reachy, gripper, grasp) node.send_output("response_l_arm", pa.array([True])) diff --git a/node-hub/dora-reachy2/dora_reachy2/right_arm.py b/node-hub/dora-reachy2/dora_reachy2/right_arm.py index 5f010bbc..b20b58e8 100644 --- a/node-hub/dora-reachy2/dora_reachy2/right_arm.py +++ b/node-hub/dora-reachy2/dora_reachy2/right_arm.py @@ -63,11 +63,18 @@ def r_arm_go_to_mixed_angles(reachy, x, y, z): return [] -def manage_gripper(reachy, gripper, grasp, initial_opening=0.0): - if initial_opening == gripper: +def manage_gripper(reachy, gripper, grasp): + if gripper == 100 and reachy.r_arm.gripper.get_current_opening() == 100: return True ## If the gripper is already half opened and we're trying to close it, it's probably already closed - elif gripper == 0.0 and (initial_opening < 98) and grasp: + elif ( + gripper == 0.0 + and ( + reachy.r_arm.gripper.get_current_opening() < 98 + and reachy.r_arm.gripper.get_current_opening() > 2 + ) + and grasp + ): print("Gripper already in grasp position") return True if gripper == 0.0: @@ -98,11 +105,10 @@ def main(): values: np.array = event["value"].to_numpy(zero_copy_only=False) encoding = event["metadata"]["encoding"] wait = event["metadata"].get("wait", True) - duration = event["metadata"].get("duration", 1) + duration = float(event["metadata"].get("duration", 1)) grasp = event["metadata"].get("grasp", True) if encoding == "xyzrpy": values = values.reshape((-1, 7)) - print(values) joint_values = [] for value in values: x = value[0] @@ -129,12 +135,7 @@ def main(): else: for joint, gripper in joint_values: reachy.r_arm.goto(joint, duration=duration, wait=wait) - response_gripper = manage_gripper( - reachy, - gripper, - grasp, - reachy.r_arm.gripper.get_current_opening(), - ) + response_gripper = manage_gripper(reachy, gripper, grasp) if not response_gripper: node.send_output( "response_r_arm", @@ -152,12 +153,7 @@ def main(): gripper = value[7] reachy.r_arm.goto(joints, duration=duration, wait=wait) - manage_gripper( - reachy, - gripper, - grasp, - reachy.r_arm.gripper.get_current_opening(), - ) + manage_gripper(reachy, gripper, grasp) node.send_output("response_r_arm", pa.array([True]))