Minor adjustement

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

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)

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

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