-
Notifications
You must be signed in to change notification settings - Fork 5
/
Copy patharm_interpolation.py
91 lines (83 loc) · 3.36 KB
/
arm_interpolation.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
from dora import Node
import pyarrow as pa
from enum import Enum
node = Node()
class Action(Enum):
"""
Action abstraction
"""
# YAW_RIGHT = ("yaw right", "movej", [0, 0, 0, 0, -0.1, 0, 0.1])
# YAW_LEFT = ("yaw left", "movej", [0, 0, 0, 0, 0.1, 0, 0.1])
# PITCH_UP = ("pitch up", "movej", [0, 0, 0, -0.1, 0, 0, 0.1])
# PITCH_DOWN = ("pitch down", "movej", [0, 0, 0, 0.1, 0, 0, 0.1])
# ROLL_LEFT = ("roll left", "movej", [0, 0, 0, 0, 0, 0, 0.1, 0.1])
# ROLL_RIGHT = ("roll right", "movej", [0, 0,0, 0, 0, 0, -0.1, 0.1])
# YAW_SHOULDER_RIGHT = (
# "yaw shoulder right",
# "movej",
# [-0.1, 0, 0, 0, 0, 0, 0.1],
# )
# YAW_SHOULDER_LEFT = (
# "yaw shoulder left",
# "movej",
# [0.1, 0, 0, 0, 0, 0, 0.1],
# )
# YAW_RIGHT = ("yaw right", "movej", [0, 0, 0, 0, -2, 0, 0])
# YAW_LEFT = ("yaw left", "movej", [0, 0, 0, 0, 2, 0, 0])
# PITCH_UP = ("pitch up", "movej", [0, 0, 0, 0, 0, -2, 0])
# PITCH_DOWN = ("pitch down", "movej", [0, 0, 0, 0, 0, 2, 0])
# ROLL_LEFT = ("roll left", "movej", [0, 0, 0, 0, 0, 0, 2])
# ROLL_RIGHT = ("roll right", "movej", [0, 0, 0, 0, 0, 0, -2])
# YAW_SHOULDER_RIGHT = (
# "yaw shoulder right",
# "movej",
# [-2, 0, 0, 0, 0, 0, 0],
# )
# YAW_SHOULDER_LEFT = (
# "yaw shoulder left",
# "movej",
# [2, 0, 0, 0, 0, 0, 0],
# )
FORWARD = ("arm forward", "movec", [0.04, 0, 0, 0, 0, 0, 0.1])
BACK = ("arm backward", "movec", [-0.04, 0, 0, 0, 0, 0, 0.1])
LEFT = ("arm left", "movec", [0, 0.04, 0, 0, 0, 0, 0.1])
RIGHT = ("arm right", "movec", [0, -0.04, 0, 0, 0, 0, 0.1])
UP = ("arm up", "movec", [0, 0, 0.04, 0, 0, 0, 0.1])
DOWN = ("arm down", "movec", [0, 0, -0.04, 0, 0, 0, 0.1])
CLOSE = ("close", "claw", [0])
OPEN = ("open", "claw", [100])
# STOP = ("stop", "stop", [])
SAVE = ("save", "save", [])
GO_TO = ("go", "go_to", [])
# END_TEACH = ("end of teach", "end_teach", [])
# TEACH = ("teach", "teach", [])
go_to_box = False
for event in node:
if event["type"] == "INPUT":
text = event["value"][0].as_py().lower()
text = text.replace(".", "")
text = text.replace(".", "")
if "save" in text:
node.send_output("save", pa.array([text.replace("save ", "")]))
elif "go" in text:
node.send_output("go_to", pa.array([text.replace("go ", "")]))
elif "go to " in text:
node.send_output("go_to", pa.array([text.replace("go to ", "")]))
else:
for action in Action:
if action.value[0] in text:
node.send_output(action.value[1], pa.array(action.value[2]))
# if action.value[0] == "close" and not go_to_box:
# already_close = True
# node.send_output(
# "prompt",
# pa.array(
# [
# "Respond with left, right, forward, backward, open, close to drop the bottle in the box"
# ]
# ),
# ) # TODO: Remove this
# # node.send_output(
# # "go_to", pa.array(["home"])
# # ) # TODO: Remove this
# break