setup

examples/test_reachability.py

@@ -0,0 +1,14 @@
+import time
+
+import FramesViewer.utils as fv_utils
+import numpy as np
+from reachy_sdk import ReachySDK
+
+from pollen_manipulation.reachy.reachability import is_pose_reachable, read_angle_limits
+
+reachy = ReachySDK("172.16.0.32")
+angle_limits = read_angle_limits(reachy)
+
+while True:
+    pose = reachy.r_arm.forward_kinematics()
+    print(is_pose_reachable(pose, reachy, angle_limits, is_left_arm=False))

setup.cfg

@@ -1,5 +1,5 @@
 [metadata]
-name = example
+name = pollen-manipulation
 version = 0.1.0
 author = Pollen Robotics
 author_email = [email protected]
@@ -17,6 +17,8 @@ package_dir=
     =src
 install_requires =
     numpy==1.25.1
+    reachy-sdk==0.7.0
+    PyYAML==6.0.1
 
 [options.packages.find]
 where=src

src/config_files/left_arm_advanced.yaml

@@ -0,0 +1,70 @@
+left_arm_advanced:
+  l_shoulder_fan:
+    fan:
+      id: 20
+  l_shoulder_pitch:
+    dxl_motor:
+      type: MX-106
+      id: 20
+      offset: 1.57
+      cw_angle_limit: -1.05
+      ccw_angle_limit: 3.14
+  l_shoulder_roll:
+    dxl_motor:
+      type: MX-106
+      id: 21
+      offset: -1.57
+      direct: False
+      cw_angle_limit: -1.57
+      ccw_angle_limit: 1.75
+  l_arm_yaw:
+    dxl_motor:
+      type: MX-64
+      id: 22
+      direct: False
+      cw_angle_limit: -1.57
+      ccw_angle_limit: 1.57
+  l_elbow_fan:
+    fan:
+      id: 23
+  l_elbow_pitch:
+    dxl_motor:
+      type: MX-106
+      id: 23
+      direct: False
+      cw_angle_limit: 0.0
+      ccw_angle_limit: 2.18
+  l_forearm_yaw:
+    dxl_motor:
+      type: MX-28
+      id: 24
+      direct: False
+      cw_angle_limit: -1.75
+      ccw_angle_limit: 1.75
+  l_wrist_fan:
+    fan:
+      id: 25
+  l_wrist_pitch:
+    dxl_motor:
+      type: MX-28
+      id: 25
+      direct: False
+      cw_angle_limit: -0.79
+      ccw_angle_limit: 0.79
+  l_wrist_roll:
+    dxl_motor:
+      type: AX-18
+      id: 26
+      direct: False
+      cw_angle_limit: -0.61
+      ccw_angle_limit: 0.95
+  l_gripper:
+    dxl_motor:
+      type: AX-18
+      id: 27
+      cw_angle_limit: -1.70
+      ccw_angle_limit: 0.90
+      reduction: -2
+  l_force_gripper:
+    force_sensor:
+      id: 20

src/config_files/right_arm_advanced.yaml

@@ -0,0 +1,71 @@
+right_arm_advanced:
+  r_shoulder_fan:
+    fan:
+      id: 10
+  r_shoulder_pitch:
+    dxl_motor:
+      type: MX-106
+      id: 10
+      offset: 1.57
+      direct: False
+      cw_angle_limit: -3.14
+      ccw_angle_limit: 1.05
+  r_shoulder_roll:
+    dxl_motor:
+      type: MX-106
+      id: 11
+      offset: 1.57
+      direct: False
+      cw_angle_limit: -1.75
+      ccw_angle_limit: 1.57
+  r_arm_yaw:
+    dxl_motor:
+      type: MX-64
+      id: 12
+      direct: False
+      cw_angle_limit: -1.57
+      ccw_angle_limit: 1.57
+  r_elbow_fan:
+    fan:
+      id: 13
+  r_elbow_pitch:
+    dxl_motor:
+      type: MX-106
+      id: 13
+      direct: False
+      cw_angle_limit: 0.0
+      ccw_angle_limit: 2.18
+  r_forearm_yaw:
+    dxl_motor:
+      type: MX-28
+      id: 14
+      direct: False
+      cw_angle_limit: -1.75
+      ccw_angle_limit: 1.75
+  r_wrist_fan:
+    fan:
+      id: 15
+  r_wrist_pitch:
+    dxl_motor:
+      type: MX-28
+      id: 15
+      direct: False
+      cw_angle_limit: -0.79
+      ccw_angle_limit: 0.79
+  r_wrist_roll:
+    dxl_motor:
+      type: AX-18
+      id: 16
+      direct: False
+      cw_angle_limit: -0.95
+      ccw_angle_limit: 0.61
+  r_gripper:
+    dxl_motor:
+      type: AX-18
+      id: 17
+      cw_angle_limit: -0.90
+      ccw_angle_limit: 1.70
+      reduction: -2
+  r_force_gripper:
+    force_sensor:
+      id: 10