dora-reachy2 0.0.0__py3-none-any.whl

dora_reachy2/__init__.py

@@ -0,0 +1,11 @@
+import os
+
+# Define the path to the README file relative to the package directory
+readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.md")
+
+# Read the content of the README file
+try:
+    with open(readme_path, encoding="utf-8") as f:
+        __doc__ = f.read()
+except FileNotFoundError:
+    __doc__ = "README file not found."

dora_reachy2/camera.py

@@ -0,0 +1,97 @@
+import os
+
+import numpy as np
+import pyarrow as pa
+from dora import Node
+from reachy2_sdk import ReachySDK
+from reachy2_sdk.media.camera import CameraView
+
+
+def main():
+    ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80")
+
+    for _ in range(5):
+        reachy = ReachySDK(ROBOT_IP)
+        try:
+            reachy.cameras.teleop.get_frame(view=CameraView.LEFT)
+            params = reachy.cameras.depth.get_parameters(view=CameraView.DEPTH)
+            if params is not None:
+                break
+        except Exception as e:
+            print(e)
+        import time
+
+        time.sleep(1)
+
+    reachy.cameras.teleop.get_frame(view=CameraView.LEFT)
+    params = reachy.cameras.depth.get_parameters(view=CameraView.DEPTH)
+    height, width, _distortion_model, _D, K, _R, _P = params
+
+    node = Node()
+
+    for event in node:
+        if event["type"] == "INPUT":
+            if event["id"] == "tick":
+                (image_left, _) = reachy.cameras.teleop.get_frame(view=CameraView.LEFT)
+
+                if image_left is int:
+                    continue
+
+                node.send_output(
+                    "image_left",
+                    pa.array(image_left.ravel()),
+                    metadata={
+                        "encoding": "bgr8",
+                        "width": image_left.shape[1],
+                        "height": image_left.shape[0],
+                    },
+                )
+
+                (image_right, _) = reachy.cameras.teleop.get_frame(
+                    view=CameraView.RIGHT,
+                )
+
+                if image_right is int:
+                    continue
+
+                node.send_output(
+                    "image_right",
+                    pa.array(image_right.ravel()),
+                    metadata={
+                        "encoding": "bgr8",
+                        "width": image_right.shape[1],
+                        "height": image_right.shape[0],
+                    },
+                )
+
+                (depth_image, _) = reachy.cameras.depth.get_frame()
+
+                node.send_output(
+                    "image_depth",
+                    pa.array(depth_image.ravel()),
+                    metadata={
+                        "encoding": "bgr8",
+                        "width": depth_image.shape[1],
+                        "height": depth_image.shape[0],
+                    },
+                )
+
+                (depth_frame, _) = reachy.cameras.depth.get_depth_frame()
+
+                if params is not None and depth_frame is not None:
+                    depth_frame = depth_frame.ravel().astype(np.float64) / 1_000.0
+
+                    node.send_output(
+                        "depth",
+                        pa.array(depth_frame),
+                        metadata={
+                            "width": width,
+                            "height": height,
+                            "focal": [int(K[0, 0]), int(K[1, 1])],
+                            "resolution": [int(K[0, 2]), int(K[1, 2])],
+                        },
+                    )
+
+
+if __name__ == "__main__":
+    main()

dora_reachy2/head.py

@@ -0,0 +1,58 @@
+import os
+
+import numpy as np
+from dora import Node
+from reachy2_sdk import ReachySDK
+
+
+def main():
+    ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80")
+
+    for _i in range(5):
+        reachy = ReachySDK(ROBOT_IP)
+
+        if reachy.head is not None:
+            reachy.head.turn_on()
+            reachy.head.goto([0, 0, 0])
+            break
+    FOV_H = 107
+    FOV_V = 91
+    resolution = [720, 960]
+
+    roll, _pitch, yaw = reachy.head.get_current_positions()
+    node = Node()
+    for event in node:
+        if event["type"] == "INPUT":
+            if event["id"] == "boxes2d":
+                boxes = event["value"].to_numpy()
+                # [x, y, z, _rx, ry, rz] = event["value"].to_numpy()
+                x_min = boxes[0]
+                y_min = boxes[1]
+                x_max = boxes[2]
+                y_max = boxes[3]
+                x = (
+                    x_min + x_max
+                ) / 2 - 10  # Deviate a bit to take into account the off centered camera
+                y = (3 * y_min + y_max) / 4
+                ry = (x - resolution[1] / 2) * FOV_H / 2 / resolution[1]
+                rz = (y - resolution[0] / 2) * FOV_V / 2 / resolution[0]
+                if np.abs(yaw) > 45 and yaw * -ry > 0:
+                    reachy.head.cancel_all_goto()
+                    roll, _pitch, yaw = reachy.head.get_current_positions()
+                    reachy.head.rotate_by(pitch=0, yaw=0, roll=-roll, duration=1.5)
+                else:
+                    reachy.head.cancel_all_goto()
+                    roll, _pitch, yaw = reachy.head.get_current_positions()
+                    reachy.head.rotate_by(pitch=rz, yaw=-ry, roll=-roll, duration=1.5)
+                [roll, _pitch, yaw] = reachy.head.get_current_positions()
+            if "look" in event["id"]:
+                [x, y, z] = event["value"].to_numpy()
+                reachy.head.cancel_all_goto()
+                reachy.head.look_at(x, y, z, duration=0.5)
+
+    if reachy.head is not None:
+        reachy.head.turn_off()
+
+
+if __name__ == "__main__":
+    main()

dora_reachy2/left_arm.py

@@ -0,0 +1,160 @@
+import os
+import time
+
+import numpy as np
+import pyarrow as pa
+from dora import Node
+from reachy2_sdk import ReachySDK
+from scipy.spatial.transform import Rotation as R
+
+ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80")
+
+
+l_default_pose = [
+    42.212611297240635,
+    -16.95827541661092,
+    15.241872783848812,
+    -131.11770715700908,
+    0.1682905250638251,
+    -1.6613469324618695,
+    2.1666679127563904,
+]
+
+
+def l_arm_go_to_mixed_angles(reachy, x, y, z):
+    for theta in range(-80, -60, 10):
+        r = R.from_euler("zyx", [0, theta, 0], degrees=True)
+        transform = np.eye(4)
+        transform[:3, :3] = r.as_matrix()
+        transform[:3, 3] = [x, y, z]
+
+        try:
+            return reachy.l_arm.inverse_kinematics(transform)
+
+        except ValueError:
+            continue
+
+    for yaw in range(0, 90, 30):
+
+        ## First try turning left
+        pitch = -90
+        r = R.from_euler("ZYX", (-yaw, 0, 0), degrees=True) * R.from_euler(
+            "ZYX",
+            (0, pitch, 0),
+            degrees=True,
+        )
+        transform = np.eye(4)
+        transform[:3, :3] = r.as_matrix()
+        transform[:3, 3] = [x, y, z]
+
+        try:
+            return reachy.l_arm.inverse_kinematics(transform)
+        except ValueError:
+            pass
+
+        try:
+            return reachy.l_arm.inverse_kinematics(transform)
+
+        except ValueError:
+            continue
+
+    # Fallback to default pose if we need the arm to be within x and z limits.
+    if x < 0.3 and z > -0.2:
+        return l_default_pose
+
+    print("Left arm: No solution found for x, y, z: ", x, y, z)
+    return []
+
+
+def manage_gripper(reachy, gripper, grasp):
+    if (gripper == 100 and reachy.r_arm.gripper.get_current_opening() == 100) or (
+        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()
+        time.sleep(0.5)
+        if grasp:
+            half_open = reachy.l_arm.gripper.get_current_opening() > 2
+            if not half_open:
+                return False
+    elif gripper == 100.0:
+        reachy.l_arm.gripper.open()
+        time.sleep(0.5)
+    return True
+
+
+def main():
+    reachy = ReachySDK(ROBOT_IP)
+
+    if reachy.l_arm is not None:
+        reachy.l_arm.turn_on()
+        reachy.l_arm.gripper.turn_on()
+    node = Node()
+
+    for event in node:
+        if event["type"] == "INPUT":
+            if event["id"] == "pose":
+
+                values: np.array = event["value"].to_numpy(zero_copy_only=False)
+                encoding = event["metadata"]["encoding"]
+                wait = event["metadata"].get("wait", True)
+                duration = float(event["metadata"].get("duration", 1))
+                grasp = event["metadata"].get("grasp", True)
+                if encoding == "xyzrpy":
+                    values = values.reshape((-1, 7))
+                    joint_values = []
+                    for value in values:
+                        x = value[0]
+                        y = value[1]
+                        z = value[2]
+                        _r = value[3]
+                        _p = value[4]
+                        _y = value[5]
+                        gripper = value[6]
+                        joints = l_arm_go_to_mixed_angles(reachy, x, y, z)
+                        response_ik = len(joints) > 0
+                        if response_ik:
+                            joint_values.append((joints, gripper))
+                        else:
+
+                            break
+
+                    if not response_ik:
+                        node.send_output(
+                            "response_l_arm",
+                            pa.array([False]),
+                            metadata={"error": f"IK Failed for x: {x}, y: {y}, z: {z}"},
+                        )
+                    else:
+                        for joint, gripper in joint_values:
+                            reachy.l_arm.goto(joint, duration=duration, wait=wait)
+                            response_gripper = manage_gripper(reachy, gripper, grasp)
+                            if not response_gripper:
+                                node.send_output(
+                                    "response_l_arm",
+                                    pa.array([False]),
+                                    metadata={"error": "Failed to grasp"},
+                                )
+                                break
+                        if response_gripper:
+                            node.send_output("response_l_arm", pa.array([True]))
+
+                elif encoding == "jointstate":
+                    values = values.reshape((-1, 8))
+                    for value in values:
+                        joints = value[:7].tolist()
+                        gripper = value[7]
+
+                        reachy.l_arm.goto(joints, duration=duration, wait=wait)
+                        manage_gripper(reachy, gripper, grasp)
+                    node.send_output("response_l_arm", pa.array([True]))
+
+
+if __name__ == "__main__":
+    main()

dora_reachy2/mobile_base.py

@@ -0,0 +1,33 @@
+import os
+import time
+
+import numpy as np
+import pyarrow as pa
+from dora import Node
+from reachy2_sdk import ReachySDK
+
+
+def main():
+    ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80")
+
+    reachy = ReachySDK(ROBOT_IP)
+
+    if reachy.mobile_base is not None:
+        reachy.mobile_base.turn_on()
+    reachy.mobile_base.reset_odometry()
+
+    node = Node()
+    for event in node:
+        if event["type"] == "INPUT":
+            if event["id"] == "action_base":
+                [x, y, _z, _rx, _ry, rz] = event["value"].to_numpy()
+                reachy.mobile_base.rotate_by(np.rad2deg(rz))
+                reachy.mobile_base.translate_by(x, y)
+                time.sleep(0.5)
+                node.send_output("response_base", pa.array([True]))
+    if reachy.mobile_base is not None:
+        reachy.mobile_base.turn_off()
+
+
+if __name__ == "__main__":
+    main()

dora_reachy2/right_arm.py

@@ -0,0 +1,159 @@
+import os
+import time
+
+import numpy as np
+import pyarrow as pa
+from dora import Node
+from reachy2_sdk import ReachySDK
+from scipy.spatial.transform import Rotation as R
+
+ROBOT_IP = os.getenv("ROBOT_IP", "10.42.0.80")
+
+r_default_pose = [
+    38.058172640242475,
+    0.07798708660299236,
+    2.0084781702579564,
+    -129.76629958820868,
+    4.428130313456095,
+    -9.272674208719419,
+    354.280491569214,
+]
+
+
+def r_arm_go_to_mixed_angles(reachy, x, y, z):
+    for theta in range(-80, -60, 10):
+        r = R.from_euler("zyx", [0, theta, 0], degrees=True)
+        transform = np.eye(4)
+        transform[:3, :3] = r.as_matrix()
+        transform[:3, 3] = [x, y, z]
+
+        try:
+            return reachy.r_arm.inverse_kinematics(transform)
+
+        except ValueError:
+            continue
+
+    for yaw in range(0, 90, 30):
+
+        ## First try turning left
+        pitch = -90
+        r = R.from_euler("ZYX", (yaw, 0, 0), degrees=True) * R.from_euler(
+            "ZYX",
+            (0, pitch, 0),
+            degrees=True,
+        )
+        transform = np.eye(4)
+        transform[:3, :3] = r.as_matrix()
+        transform[:3, 3] = [x, y, z]
+
+        try:
+            return reachy.r_arm.inverse_kinematics(transform)
+        except ValueError:
+            pass
+
+        try:
+            return reachy.r_arm.inverse_kinematics(transform)
+
+        except ValueError:
+            continue
+
+    # Fallback to default pose if we need the arm to be within x and z limits.
+    if x < 0.3 and z > -0.2:
+        return r_default_pose
+
+    print("Right arm: No solution found for x, y, z: ", x, y, z)
+    return []
+
+
+def manage_gripper(reachy, gripper, grasp):
+    if (gripper == 100 and reachy.r_arm.gripper.get_current_opening() == 100) or (
+        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.r_arm.gripper.close()
+        time.sleep(0.5)
+        if grasp:
+            half_open = reachy.r_arm.gripper.get_current_opening() > 2
+            if not half_open:
+                return False
+    elif gripper == 100.0:
+        reachy.r_arm.gripper.open()
+        time.sleep(0.5)
+    return True
+
+
+def main():
+    reachy = ReachySDK(ROBOT_IP)
+
+    if reachy.r_arm is not None:
+        reachy.r_arm.turn_on()
+        reachy.r_arm.gripper.turn_on()
+    node = Node()
+
+    for event in node:
+        if event["type"] == "INPUT":
+            if event["id"] == "pose":
+
+                values: np.array = event["value"].to_numpy(zero_copy_only=False)
+                encoding = event["metadata"]["encoding"]
+                wait = event["metadata"].get("wait", True)
+                duration = float(event["metadata"].get("duration", 1))
+                grasp = event["metadata"].get("grasp", True)
+                if encoding == "xyzrpy":
+                    values = values.reshape((-1, 7))
+                    joint_values = []
+                    for value in values:
+                        x = value[0]
+                        y = value[1]
+                        z = value[2]
+                        _r = value[3]
+                        _p = value[4]
+                        _y = value[5]
+                        gripper = value[6]
+                        joints = r_arm_go_to_mixed_angles(reachy, x, y, z)
+                        response_ik = len(joints) > 0
+                        if response_ik:
+                            joint_values.append((joints, gripper))
+                        else:
+
+                            break
+
+                    if not response_ik:
+                        node.send_output(
+                            "response_r_arm",
+                            pa.array([False]),
+                            metadata={"error": f"IK Failed for x: {x}, y: {y}, z: {z}"},
+                        )
+                    else:
+                        for joint, gripper in joint_values:
+                            reachy.r_arm.goto(joint, duration=duration, wait=wait)
+                            response_gripper = manage_gripper(reachy, gripper, grasp)
+                            if not response_gripper:
+                                node.send_output(
+                                    "response_r_arm",
+                                    pa.array([False]),
+                                    metadata={"error": "Failed to grasp"},
+                                )
+                                break
+                        if response_gripper:
+                            node.send_output("response_r_arm", pa.array([True]))
+
+                elif encoding == "jointstate":
+                    values = values.reshape((-1, 8))
+                    for value in values:
+                        joints = value[:7].tolist()
+                        gripper = value[7]
+
+                        reachy.r_arm.goto(joints, duration=duration, wait=wait)
+                        manage_gripper(reachy, gripper, grasp)
+                    node.send_output("response_r_arm", pa.array([True]))
+
+
+if __name__ == "__main__":
+    main()

dora_reachy2-0.0.0.dist-info/METADATA

@@ -0,0 +1,50 @@
+Metadata-Version: 2.2
+Name: dora-reachy2
+Version: 0.0.0
+Summary: dora-reachy2
+Author-email: Your Name <email@email.com>
+License: MIT
+Requires-Python: >=3.10
+Description-Content-Type: text/markdown
+Requires-Dist: dora-rs>=0.3.6
+Requires-Dist: reachy2-sdk==1.0.7
+Requires-Dist: reachy2-sdk-api==1.0.15
+Requires-Dist: scipy
+
+# dora-reachy2
+
+## Getting started
+
+- Install it with pip:
+
+```bash
+pip install -e .
+```
+
+## Contribution Guide
+
+- Format with [ruff](https://docs.astral.sh/ruff/):
+
+```bash
+ruff check . --fix
+```
+
+- Lint with ruff:
+
+```bash
+ruff check .
+```
+
+- Test with [pytest](https://github.com/pytest-dev/pytest)
+
+```bash
+pytest . # Test
+```
+
+## YAML Specification
+
+## Examples
+
+## License
+
+dora-reachy2's code are released under the MIT License

dora_reachy2-0.0.0.dist-info/RECORD

@@ -0,0 +1,11 @@
+dora_reachy2/__init__.py,sha256=HuSK3dnyI9Pb5QAuaKFwQQ3J5SIZnLcKHPJO0norGzc,353
+dora_reachy2/camera.py,sha256=qGQsmEVWbXNP_6mSiaIHqTlPOHOvHfkbE4CLfvubWdc,3064
+dora_reachy2/head.py,sha256=P2PmtaHLX0dSNX2NNlmpMb2m1cCAvAFIygWFk2reSXQ,2042
+dora_reachy2/left_arm.py,sha256=B7Yby0mBDGYl8V4dBYuhkv9W5VyjAMXwgz_RnjRIyi4,5155
+dora_reachy2/mobile_base.py,sha256=a7Kj1Zsj-MIvOcDBXMj9gRqLq6NU4xkx5hU4bnWWdfo,860
+dora_reachy2/right_arm.py,sha256=KZz8MMK-zkPkdBURC9v903-iaddcRENm-ITsJQBSiG4,5151
+dora_reachy2-0.0.0.dist-info/METADATA,sha256=98z3gsfUBpLQjlfvAY8rqumzj2Ph7PNMngOFm6qGOx8,757
+dora_reachy2-0.0.0.dist-info/WHEEL,sha256=In9FTNxeP60KnTkGw7wk6mJPYd_dQSjEZmXdBdMCI-8,91
+dora_reachy2-0.0.0.dist-info/entry_points.txt,sha256=1wj96u7QDcTPy9VN2xxV0HD2Og8GRzyT7f8wmMWD9Jc,269
+dora_reachy2-0.0.0.dist-info/top_level.txt,sha256=KytY6Rdfj03RnwbYoeaA4xV1QirpxuJD66BodLTs_pM,13
+dora_reachy2-0.0.0.dist-info/RECORD,,

dora_reachy2-0.0.0.dist-info/WHEEL

@@ -0,0 +1,5 @@
+Wheel-Version: 1.0
+Generator: setuptools (75.8.0)
+Root-Is-Purelib: true
+Tag: py3-none-any
+

dora_reachy2-0.0.0.dist-info/entry_points.txt

@@ -0,0 +1,6 @@
+[console_scripts]
+dora-reachy2-camera = dora_reachy2.camera:main
+dora-reachy2-head = dora_reachy2.head:main
+dora-reachy2-left-arm = dora_reachy2.left_arm:main
+dora-reachy2-mobile-base = dora_reachy2.mobile_base:main
+dora-reachy2-right-arm = dora_reachy2.right_arm:main

dora_reachy2-0.0.0.dist-info/top_level.txt

@@ -0,0 +1 @@
+dora_reachy2