Minor fix and improvement

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

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")
 

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

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]))
 
 

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]))