dora-reachy2 0.0.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,11 @@
1
+ import os
2
+
3
+ # Define the path to the README file relative to the package directory
4
+ readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.md")
5
+
6
+ # Read the content of the README file
7
+ try:
8
+ with open(readme_path, encoding="utf-8") as f:
9
+ __doc__ = f.read()
10
+ except FileNotFoundError:
11
+ __doc__ = "README file not found."
dora_reachy2/camera.py ADDED
@@ -0,0 +1,97 @@
1
+ import os
2
+
3
+ import numpy as np
4
+ import pyarrow as pa
5
+ from dora import Node
6
+ from reachy2_sdk import ReachySDK
7
+ from reachy2_sdk.media.camera import CameraView
8
+
9
+
10
+ def main():
11
+ ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80")
12
+
13
+ for _ in range(5):
14
+ reachy = ReachySDK(ROBOT_IP)
15
+ try:
16
+ reachy.cameras.teleop.get_frame(view=CameraView.LEFT)
17
+ params = reachy.cameras.depth.get_parameters(view=CameraView.DEPTH)
18
+ if params is not None:
19
+ break
20
+ except Exception as e:
21
+ print(e)
22
+ import time
23
+
24
+ time.sleep(1)
25
+
26
+ reachy.cameras.teleop.get_frame(view=CameraView.LEFT)
27
+ params = reachy.cameras.depth.get_parameters(view=CameraView.DEPTH)
28
+ height, width, _distortion_model, _D, K, _R, _P = params
29
+
30
+ node = Node()
31
+
32
+ for event in node:
33
+ if event["type"] == "INPUT":
34
+ if event["id"] == "tick":
35
+ (image_left, _) = reachy.cameras.teleop.get_frame(view=CameraView.LEFT)
36
+
37
+ if image_left is int:
38
+ continue
39
+
40
+ node.send_output(
41
+ "image_left",
42
+ pa.array(image_left.ravel()),
43
+ metadata={
44
+ "encoding": "bgr8",
45
+ "width": image_left.shape[1],
46
+ "height": image_left.shape[0],
47
+ },
48
+ )
49
+
50
+ (image_right, _) = reachy.cameras.teleop.get_frame(
51
+ view=CameraView.RIGHT,
52
+ )
53
+
54
+ if image_right is int:
55
+ continue
56
+
57
+ node.send_output(
58
+ "image_right",
59
+ pa.array(image_right.ravel()),
60
+ metadata={
61
+ "encoding": "bgr8",
62
+ "width": image_right.shape[1],
63
+ "height": image_right.shape[0],
64
+ },
65
+ )
66
+
67
+ (depth_image, _) = reachy.cameras.depth.get_frame()
68
+
69
+ node.send_output(
70
+ "image_depth",
71
+ pa.array(depth_image.ravel()),
72
+ metadata={
73
+ "encoding": "bgr8",
74
+ "width": depth_image.shape[1],
75
+ "height": depth_image.shape[0],
76
+ },
77
+ )
78
+
79
+ (depth_frame, _) = reachy.cameras.depth.get_depth_frame()
80
+
81
+ if params is not None and depth_frame is not None:
82
+ depth_frame = depth_frame.ravel().astype(np.float64) / 1_000.0
83
+
84
+ node.send_output(
85
+ "depth",
86
+ pa.array(depth_frame),
87
+ metadata={
88
+ "width": width,
89
+ "height": height,
90
+ "focal": [int(K[0, 0]), int(K[1, 1])],
91
+ "resolution": [int(K[0, 2]), int(K[1, 2])],
92
+ },
93
+ )
94
+
95
+
96
+ if __name__ == "__main__":
97
+ main()
dora_reachy2/head.py ADDED
@@ -0,0 +1,58 @@
1
+ import os
2
+
3
+ import numpy as np
4
+ from dora import Node
5
+ from reachy2_sdk import ReachySDK
6
+
7
+
8
+ def main():
9
+ ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80")
10
+
11
+ for _i in range(5):
12
+ reachy = ReachySDK(ROBOT_IP)
13
+
14
+ if reachy.head is not None:
15
+ reachy.head.turn_on()
16
+ reachy.head.goto([0, 0, 0])
17
+ break
18
+ FOV_H = 107
19
+ FOV_V = 91
20
+ resolution = [720, 960]
21
+
22
+ roll, _pitch, yaw = reachy.head.get_current_positions()
23
+ node = Node()
24
+ for event in node:
25
+ if event["type"] == "INPUT":
26
+ if event["id"] == "boxes2d":
27
+ boxes = event["value"].to_numpy()
28
+ # [x, y, z, _rx, ry, rz] = event["value"].to_numpy()
29
+ x_min = boxes[0]
30
+ y_min = boxes[1]
31
+ x_max = boxes[2]
32
+ y_max = boxes[3]
33
+ x = (
34
+ x_min + x_max
35
+ ) / 2 - 10 # Deviate a bit to take into account the off centered camera
36
+ y = (3 * y_min + y_max) / 4
37
+ ry = (x - resolution[1] / 2) * FOV_H / 2 / resolution[1]
38
+ rz = (y - resolution[0] / 2) * FOV_V / 2 / resolution[0]
39
+ if np.abs(yaw) > 45 and yaw * -ry > 0:
40
+ reachy.head.cancel_all_goto()
41
+ roll, _pitch, yaw = reachy.head.get_current_positions()
42
+ reachy.head.rotate_by(pitch=0, yaw=0, roll=-roll, duration=1.5)
43
+ else:
44
+ reachy.head.cancel_all_goto()
45
+ roll, _pitch, yaw = reachy.head.get_current_positions()
46
+ reachy.head.rotate_by(pitch=rz, yaw=-ry, roll=-roll, duration=1.5)
47
+ [roll, _pitch, yaw] = reachy.head.get_current_positions()
48
+ if "look" in event["id"]:
49
+ [x, y, z] = event["value"].to_numpy()
50
+ reachy.head.cancel_all_goto()
51
+ reachy.head.look_at(x, y, z, duration=0.5)
52
+
53
+ if reachy.head is not None:
54
+ reachy.head.turn_off()
55
+
56
+
57
+ if __name__ == "__main__":
58
+ main()
@@ -0,0 +1,160 @@
1
+ import os
2
+ import time
3
+
4
+ import numpy as np
5
+ import pyarrow as pa
6
+ from dora import Node
7
+ from reachy2_sdk import ReachySDK
8
+ from scipy.spatial.transform import Rotation as R
9
+
10
+ ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80")
11
+
12
+
13
+ l_default_pose = [
14
+ 42.212611297240635,
15
+ -16.95827541661092,
16
+ 15.241872783848812,
17
+ -131.11770715700908,
18
+ 0.1682905250638251,
19
+ -1.6613469324618695,
20
+ 2.1666679127563904,
21
+ ]
22
+
23
+
24
+ def l_arm_go_to_mixed_angles(reachy, x, y, z):
25
+ for theta in range(-80, -60, 10):
26
+ r = R.from_euler("zyx", [0, theta, 0], degrees=True)
27
+ transform = np.eye(4)
28
+ transform[:3, :3] = r.as_matrix()
29
+ transform[:3, 3] = [x, y, z]
30
+
31
+ try:
32
+ return reachy.l_arm.inverse_kinematics(transform)
33
+
34
+ except ValueError:
35
+ continue
36
+
37
+ for yaw in range(0, 90, 30):
38
+
39
+ ## First try turning left
40
+ pitch = -90
41
+ r = R.from_euler("ZYX", (-yaw, 0, 0), degrees=True) * R.from_euler(
42
+ "ZYX",
43
+ (0, pitch, 0),
44
+ degrees=True,
45
+ )
46
+ transform = np.eye(4)
47
+ transform[:3, :3] = r.as_matrix()
48
+ transform[:3, 3] = [x, y, z]
49
+
50
+ try:
51
+ return reachy.l_arm.inverse_kinematics(transform)
52
+ except ValueError:
53
+ pass
54
+
55
+ try:
56
+ return reachy.l_arm.inverse_kinematics(transform)
57
+
58
+ except ValueError:
59
+ continue
60
+
61
+ # Fallback to default pose if we need the arm to be within x and z limits.
62
+ if x < 0.3 and z > -0.2:
63
+ return l_default_pose
64
+
65
+ print("Left arm: No solution found for x, y, z: ", x, y, z)
66
+ return []
67
+
68
+
69
+ def manage_gripper(reachy, gripper, grasp):
70
+ if (gripper == 100 and reachy.r_arm.gripper.get_current_opening() == 100) or (
71
+ gripper == 0.0
72
+ and (
73
+ reachy.r_arm.gripper.get_current_opening() < 98
74
+ and reachy.r_arm.gripper.get_current_opening() > 2
75
+ )
76
+ and grasp
77
+ ):
78
+ return True
79
+ if gripper == 0.0:
80
+ reachy.l_arm.gripper.close()
81
+ time.sleep(0.5)
82
+ if grasp:
83
+ half_open = reachy.l_arm.gripper.get_current_opening() > 2
84
+ if not half_open:
85
+ return False
86
+ elif gripper == 100.0:
87
+ reachy.l_arm.gripper.open()
88
+ time.sleep(0.5)
89
+ return True
90
+
91
+
92
+ def main():
93
+ reachy = ReachySDK(ROBOT_IP)
94
+
95
+ if reachy.l_arm is not None:
96
+ reachy.l_arm.turn_on()
97
+ reachy.l_arm.gripper.turn_on()
98
+ node = Node()
99
+
100
+ for event in node:
101
+ if event["type"] == "INPUT":
102
+ if event["id"] == "pose":
103
+
104
+ values: np.array = event["value"].to_numpy(zero_copy_only=False)
105
+ encoding = event["metadata"]["encoding"]
106
+ wait = event["metadata"].get("wait", True)
107
+ duration = float(event["metadata"].get("duration", 1))
108
+ grasp = event["metadata"].get("grasp", True)
109
+ if encoding == "xyzrpy":
110
+ values = values.reshape((-1, 7))
111
+ joint_values = []
112
+ for value in values:
113
+ x = value[0]
114
+ y = value[1]
115
+ z = value[2]
116
+ _r = value[3]
117
+ _p = value[4]
118
+ _y = value[5]
119
+ gripper = value[6]
120
+ joints = l_arm_go_to_mixed_angles(reachy, x, y, z)
121
+ response_ik = len(joints) > 0
122
+ if response_ik:
123
+ joint_values.append((joints, gripper))
124
+ else:
125
+
126
+ break
127
+
128
+ if not response_ik:
129
+ node.send_output(
130
+ "response_l_arm",
131
+ pa.array([False]),
132
+ metadata={"error": f"IK Failed for x: {x}, y: {y}, z: {z}"},
133
+ )
134
+ else:
135
+ for joint, gripper in joint_values:
136
+ reachy.l_arm.goto(joint, duration=duration, wait=wait)
137
+ response_gripper = manage_gripper(reachy, gripper, grasp)
138
+ if not response_gripper:
139
+ node.send_output(
140
+ "response_l_arm",
141
+ pa.array([False]),
142
+ metadata={"error": "Failed to grasp"},
143
+ )
144
+ break
145
+ if response_gripper:
146
+ node.send_output("response_l_arm", pa.array([True]))
147
+
148
+ elif encoding == "jointstate":
149
+ values = values.reshape((-1, 8))
150
+ for value in values:
151
+ joints = value[:7].tolist()
152
+ gripper = value[7]
153
+
154
+ reachy.l_arm.goto(joints, duration=duration, wait=wait)
155
+ manage_gripper(reachy, gripper, grasp)
156
+ node.send_output("response_l_arm", pa.array([True]))
157
+
158
+
159
+ if __name__ == "__main__":
160
+ main()
@@ -0,0 +1,33 @@
1
+ import os
2
+ import time
3
+
4
+ import numpy as np
5
+ import pyarrow as pa
6
+ from dora import Node
7
+ from reachy2_sdk import ReachySDK
8
+
9
+
10
+ def main():
11
+ ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80")
12
+
13
+ reachy = ReachySDK(ROBOT_IP)
14
+
15
+ if reachy.mobile_base is not None:
16
+ reachy.mobile_base.turn_on()
17
+ reachy.mobile_base.reset_odometry()
18
+
19
+ node = Node()
20
+ for event in node:
21
+ if event["type"] == "INPUT":
22
+ if event["id"] == "action_base":
23
+ [x, y, _z, _rx, _ry, rz] = event["value"].to_numpy()
24
+ reachy.mobile_base.rotate_by(np.rad2deg(rz))
25
+ reachy.mobile_base.translate_by(x, y)
26
+ time.sleep(0.5)
27
+ node.send_output("response_base", pa.array([True]))
28
+ if reachy.mobile_base is not None:
29
+ reachy.mobile_base.turn_off()
30
+
31
+
32
+ if __name__ == "__main__":
33
+ main()
@@ -0,0 +1,159 @@
1
+ import os
2
+ import time
3
+
4
+ import numpy as np
5
+ import pyarrow as pa
6
+ from dora import Node
7
+ from reachy2_sdk import ReachySDK
8
+ from scipy.spatial.transform import Rotation as R
9
+
10
+ ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80")
11
+
12
+ r_default_pose = [
13
+ 38.058172640242475,
14
+ 0.07798708660299236,
15
+ 2.0084781702579564,
16
+ -129.76629958820868,
17
+ 4.428130313456095,
18
+ -9.272674208719419,
19
+ 354.280491569214,
20
+ ]
21
+
22
+
23
+ def r_arm_go_to_mixed_angles(reachy, x, y, z):
24
+ for theta in range(-80, -60, 10):
25
+ r = R.from_euler("zyx", [0, theta, 0], degrees=True)
26
+ transform = np.eye(4)
27
+ transform[:3, :3] = r.as_matrix()
28
+ transform[:3, 3] = [x, y, z]
29
+
30
+ try:
31
+ return reachy.r_arm.inverse_kinematics(transform)
32
+
33
+ except ValueError:
34
+ continue
35
+
36
+ for yaw in range(0, 90, 30):
37
+
38
+ ## First try turning left
39
+ pitch = -90
40
+ r = R.from_euler("ZYX", (yaw, 0, 0), degrees=True) * R.from_euler(
41
+ "ZYX",
42
+ (0, pitch, 0),
43
+ degrees=True,
44
+ )
45
+ transform = np.eye(4)
46
+ transform[:3, :3] = r.as_matrix()
47
+ transform[:3, 3] = [x, y, z]
48
+
49
+ try:
50
+ return reachy.r_arm.inverse_kinematics(transform)
51
+ except ValueError:
52
+ pass
53
+
54
+ try:
55
+ return reachy.r_arm.inverse_kinematics(transform)
56
+
57
+ except ValueError:
58
+ continue
59
+
60
+ # Fallback to default pose if we need the arm to be within x and z limits.
61
+ if x < 0.3 and z > -0.2:
62
+ return r_default_pose
63
+
64
+ print("Right arm: No solution found for x, y, z: ", x, y, z)
65
+ return []
66
+
67
+
68
+ def manage_gripper(reachy, gripper, grasp):
69
+ if (gripper == 100 and reachy.r_arm.gripper.get_current_opening() == 100) or (
70
+ gripper == 0.0
71
+ and (
72
+ reachy.r_arm.gripper.get_current_opening() < 98
73
+ and reachy.r_arm.gripper.get_current_opening() > 2
74
+ )
75
+ and grasp
76
+ ):
77
+ return True
78
+ if gripper == 0.0:
79
+ reachy.r_arm.gripper.close()
80
+ time.sleep(0.5)
81
+ if grasp:
82
+ half_open = reachy.r_arm.gripper.get_current_opening() > 2
83
+ if not half_open:
84
+ return False
85
+ elif gripper == 100.0:
86
+ reachy.r_arm.gripper.open()
87
+ time.sleep(0.5)
88
+ return True
89
+
90
+
91
+ def main():
92
+ reachy = ReachySDK(ROBOT_IP)
93
+
94
+ if reachy.r_arm is not None:
95
+ reachy.r_arm.turn_on()
96
+ reachy.r_arm.gripper.turn_on()
97
+ node = Node()
98
+
99
+ for event in node:
100
+ if event["type"] == "INPUT":
101
+ if event["id"] == "pose":
102
+
103
+ values: np.array = event["value"].to_numpy(zero_copy_only=False)
104
+ encoding = event["metadata"]["encoding"]
105
+ wait = event["metadata"].get("wait", True)
106
+ duration = float(event["metadata"].get("duration", 1))
107
+ grasp = event["metadata"].get("grasp", True)
108
+ if encoding == "xyzrpy":
109
+ values = values.reshape((-1, 7))
110
+ joint_values = []
111
+ for value in values:
112
+ x = value[0]
113
+ y = value[1]
114
+ z = value[2]
115
+ _r = value[3]
116
+ _p = value[4]
117
+ _y = value[5]
118
+ gripper = value[6]
119
+ joints = r_arm_go_to_mixed_angles(reachy, x, y, z)
120
+ response_ik = len(joints) > 0
121
+ if response_ik:
122
+ joint_values.append((joints, gripper))
123
+ else:
124
+
125
+ break
126
+
127
+ if not response_ik:
128
+ node.send_output(
129
+ "response_r_arm",
130
+ pa.array([False]),
131
+ metadata={"error": f"IK Failed for x: {x}, y: {y}, z: {z}"},
132
+ )
133
+ else:
134
+ for joint, gripper in joint_values:
135
+ reachy.r_arm.goto(joint, duration=duration, wait=wait)
136
+ response_gripper = manage_gripper(reachy, gripper, grasp)
137
+ if not response_gripper:
138
+ node.send_output(
139
+ "response_r_arm",
140
+ pa.array([False]),
141
+ metadata={"error": "Failed to grasp"},
142
+ )
143
+ break
144
+ if response_gripper:
145
+ node.send_output("response_r_arm", pa.array([True]))
146
+
147
+ elif encoding == "jointstate":
148
+ values = values.reshape((-1, 8))
149
+ for value in values:
150
+ joints = value[:7].tolist()
151
+ gripper = value[7]
152
+
153
+ reachy.r_arm.goto(joints, duration=duration, wait=wait)
154
+ manage_gripper(reachy, gripper, grasp)
155
+ node.send_output("response_r_arm", pa.array([True]))
156
+
157
+
158
+ if __name__ == "__main__":
159
+ main()
@@ -0,0 +1,50 @@
1
+ Metadata-Version: 2.2
2
+ Name: dora-reachy2
3
+ Version: 0.0.0
4
+ Summary: dora-reachy2
5
+ Author-email: Your Name <email@email.com>
6
+ License: MIT
7
+ Requires-Python: >=3.10
8
+ Description-Content-Type: text/markdown
9
+ Requires-Dist: dora-rs>=0.3.6
10
+ Requires-Dist: reachy2-sdk==1.0.7
11
+ Requires-Dist: reachy2-sdk-api==1.0.15
12
+ Requires-Dist: scipy
13
+
14
+ # dora-reachy2
15
+
16
+ ## Getting started
17
+
18
+ - Install it with pip:
19
+
20
+ ```bash
21
+ pip install -e .
22
+ ```
23
+
24
+ ## Contribution Guide
25
+
26
+ - Format with [ruff](https://docs.astral.sh/ruff/):
27
+
28
+ ```bash
29
+ ruff check . --fix
30
+ ```
31
+
32
+ - Lint with ruff:
33
+
34
+ ```bash
35
+ ruff check .
36
+ ```
37
+
38
+ - Test with [pytest](https://github.com/pytest-dev/pytest)
39
+
40
+ ```bash
41
+ pytest . # Test
42
+ ```
43
+
44
+ ## YAML Specification
45
+
46
+ ## Examples
47
+
48
+ ## License
49
+
50
+ dora-reachy2's code are released under the MIT License
@@ -0,0 +1,11 @@
1
+ dora_reachy2/__init__.py,sha256=HuSK3dnyI9Pb5QAuaKFwQQ3J5SIZnLcKHPJO0norGzc,353
2
+ dora_reachy2/camera.py,sha256=qGQsmEVWbXNP_6mSiaIHqTlPOHOvHfkbE4CLfvubWdc,3064
3
+ dora_reachy2/head.py,sha256=P2PmtaHLX0dSNX2NNlmpMb2m1cCAvAFIygWFk2reSXQ,2042
4
+ dora_reachy2/left_arm.py,sha256=B7Yby0mBDGYl8V4dBYuhkv9W5VyjAMXwgz_RnjRIyi4,5155
5
+ dora_reachy2/mobile_base.py,sha256=a7Kj1Zsj-MIvOcDBXMj9gRqLq6NU4xkx5hU4bnWWdfo,860
6
+ dora_reachy2/right_arm.py,sha256=KZz8MMK-zkPkdBURC9v903-iaddcRENm-ITsJQBSiG4,5151
7
+ dora_reachy2-0.0.0.dist-info/METADATA,sha256=98z3gsfUBpLQjlfvAY8rqumzj2Ph7PNMngOFm6qGOx8,757
8
+ dora_reachy2-0.0.0.dist-info/WHEEL,sha256=In9FTNxeP60KnTkGw7wk6mJPYd_dQSjEZmXdBdMCI-8,91
9
+ dora_reachy2-0.0.0.dist-info/entry_points.txt,sha256=1wj96u7QDcTPy9VN2xxV0HD2Og8GRzyT7f8wmMWD9Jc,269
10
+ dora_reachy2-0.0.0.dist-info/top_level.txt,sha256=KytY6Rdfj03RnwbYoeaA4xV1QirpxuJD66BodLTs_pM,13
11
+ dora_reachy2-0.0.0.dist-info/RECORD,,
@@ -0,0 +1,5 @@
1
+ Wheel-Version: 1.0
2
+ Generator: setuptools (75.8.0)
3
+ Root-Is-Purelib: true
4
+ Tag: py3-none-any
5
+
@@ -0,0 +1,6 @@
1
+ [console_scripts]
2
+ dora-reachy2-camera = dora_reachy2.camera:main
3
+ dora-reachy2-head = dora_reachy2.head:main
4
+ dora-reachy2-left-arm = dora_reachy2.left_arm:main
5
+ dora-reachy2-mobile-base = dora_reachy2.mobile_base:main
6
+ dora-reachy2-right-arm = dora_reachy2.right_arm:main
@@ -0,0 +1 @@
1
+ dora_reachy2