dora-rs
/
dora
Not watched
1
0
Fork 0
Code
Releases 124 Wiki evaluate Activity
Issues 0 Pull Requests 0 Datasets Model Cloudbrain HPC
Browse Source

Minor adjustement

remote-reachy2-improvements
haixuanTao 8 months ago
parent
b1a3cd67f4
commit
0494ef753b
4 changed files with 31 additions and 20 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +2
    -0
      examples/reachy2-remote/dataflow_reachy.yml
  2. +4
    -8
      examples/reachy2-remote/parse_point.py
  3. +11
    -8
      examples/reachy2-remote/parse_pose.py
  4. +14
    -4
      examples/reachy2-remote/parse_whisper.py

+ 2
- 0
examples/reachy2-remote/dataflow_reachy.yml View File

@@ -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


+ 4
- 8
examples/reachy2-remote/parse_point.py View File

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

+ 11
- 8
examples/reachy2-remote/parse_pose.py View File

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

+ 14
- 4
examples/reachy2-remote/parse_whisper.py View File

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

Write Preview
Loading…
Cancel
Save