dora-piper 0.3.6__py3-none-any.whl → 0.3.8__py3-none-any.whl

dora_piper/main.py

@@ -1,13 +1,11 @@
 from piper_sdk import C_PiperInterface
 from dora import Node
 import pyarrow as pa
+import numpy as np
 import os
 import time
 
-TEACH_MODE = (
-    os.getenv("TEACH_MODE", "False") == "True"
-    or os.getenv("TEACH_MODE", "False") == "true"
-)
+TEACH_MODE = os.getenv("TEACH_MODE", "False") in ["True", "true"]
 
 
 def enable_fun(piper: C_PiperInterface):
@@ -32,8 +30,6 @@ def enable_fun(piper: C_PiperInterface):
             and piper.GetArmLowSpdInfoMsgs().motor_6.foc_status.driver_enable_status
         )
         print("使能状态:", enable_flag)
-        piper.EnableArm(7)
-        piper.GripperCtrl(0, 1000, 0x01, 0)
         print("--------------------")
         # 检查是否超过超时时间
         if elapsed_time > timeout:
@@ -44,8 +40,7 @@ def enable_fun(piper: C_PiperInterface):
         time.sleep(1)
     if elapsed_time_flag:
         print("程序自动使能超时,退出程序")
-        print("If you have this issue, you should probably restart your computer")
-        exit(0)
+        raise ConnectionError("程序自动使能超时,退出程序")
 
 
 def main():
@@ -54,35 +49,25 @@ def main():
     piper = C_PiperInterface(CAN_BUS)
     piper.ConnectPort()
 
-    if TEACH_MODE is False:
+    if not TEACH_MODE:
         # piper.MotionCtrl_3(0, 0, 0, 0x00)#位置速度模式
         piper.EnableArm(7)
         enable_fun(piper=piper)
-        piper.GripperCtrl(0, 1000, 0x01, 0)
+        piper.MotionCtrl_2(0x01, 0x01, 50, 0x00)
+        piper.JointCtrl(0, 0, 0, 0, 0, 0)
+        piper.GripperCtrl(abs(0), 1000, 0x01, 0)
+        piper.MotionCtrl_2(0x01, 0x01, 50, 0x00)
+        time.sleep(5)
+
     factor = 57324.840764  # 1000*180/3.14
-    time.sleep(2)
     node = Node()
 
     for event in node:
         if event["type"] == "INPUT":
-            if event["id"] != "action":
-                joint = piper.GetArmJointMsgs()
-                gripper = piper.GetArmGripperMsgs()
-
-                joint_value = []
-                joint_value += [joint.joint_state.joint_1.real / factor]
-                joint_value += [joint.joint_state.joint_2.real / factor]
-                joint_value += [joint.joint_state.joint_3.real / factor]
-                joint_value += [joint.joint_state.joint_4.real / factor]
-                joint_value += [joint.joint_state.joint_5.real / factor]
-                joint_value += [joint.joint_state.joint_6.real / factor]
-                joint_value += [gripper.gripper_state.grippers_angle / 1000 / 1000 / 4]
-
-                node.send_output("jointstate", pa.array(joint_value, type=pa.float32()))
-            else:
-
+            if event["id"] == "joint_action":
+                if TEACH_MODE:
+                    continue
                 # Do not push to many commands to fast. Limiting it to 20Hz
-                # This is due to writing on a moving arm might fail the can bus.
                 if time.time() - elapsed_time > 0.05:
                     elapsed_time = time.time()
                 else:
@@ -95,15 +80,76 @@ def main():
                 joint_3 = round(position[3] * factor)
                 joint_4 = round(position[4] * factor)
                 joint_5 = round(position[5] * factor)
-                joint_6 = round(position[6] * 1000 * 1000 * 12)
+                joint_6 = round(position[6] * 1000 * 100)
 
                 piper.MotionCtrl_2(0x01, 0x01, 50, 0x00)
                 piper.JointCtrl(joint_0, joint_1, joint_2, joint_3, joint_4, joint_5)
                 piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0)
                 piper.MotionCtrl_2(0x01, 0x01, 50, 0x00)
+
+            elif event["id"] == "eef_action":
+                if TEACH_MODE:
+                    continue
+                # Do not push to many commands to fast. Limiting it to 20Hz
+                if time.time() - elapsed_time > 0.05:
+                    elapsed_time = time.time()
+                else:
+                    continue
+
+                position = event["value"].to_numpy()
+                piper.MotionCtrl_2(0x01, 0x01, 50, 0x00)
+                piper.EndPoseCtrl(
+                    position[0] * 1000 * 1000,
+                    position[1] * 1000 * 1000,
+                    position[2] * 1000 * 1000,
+                    position[3] * 1000 / (2 * np.pi) * 360,
+                    position[4] * 1000 / (2 * np.pi) * 360,
+                    position[5] * 1000 / (2 * np.pi) * 360,
+                )
+                piper.GripperCtrl(abs(position[6] * 1000 * 100), 1000, 0x01, 0)
+                piper.MotionCtrl_2(0x01, 0x01, 50, 0x00)
+
+            else:
+                joint = piper.GetArmJointMsgs()
+
+                joint_value = []
+                joint_value += [joint.joint_state.joint_1.real / factor]
+                joint_value += [joint.joint_state.joint_2.real / factor]
+                joint_value += [joint.joint_state.joint_3.real / factor]
+                joint_value += [joint.joint_state.joint_4.real / factor]
+                joint_value += [joint.joint_state.joint_5.real / factor]
+                joint_value += [joint.joint_state.joint_6.real / factor]
+
+                gripper = piper.GetArmGripperMsgs()
+                joint_value += [gripper.gripper_state.grippers_angle / 1000 / 100]
+
+                node.send_output("jointstate", pa.array(joint_value, type=pa.float32()))
+
+                position = piper.GetArmEndPoseMsgs()
+                position_value = []
+                position_value += [position.end_pose.X_axis * 0.001 * 0.001]
+                position_value += [position.end_pose.Y_axis * 0.001 * 0.001]
+                position_value += [position.end_pose.Z_axis * 0.001 * 0.001]
+                position_value += [position.end_pose.RX_axis * 0.001 / 360 * 2 * np.pi]
+                position_value += [position.end_pose.RY_axis * 0.001 / 360 * 2 * np.pi]
+                position_value += [position.end_pose.RZ_axis * 0.001 / 360 * 2 * np.pi]
+
+                node.send_output("pose", pa.array(position_value, type=pa.float32()))
+                node.send_output(
+                    "gripper",
+                    pa.array(
+                        [gripper.gripper_state.grippers_angle / 1000 / 100],
+                        type=pa.float32(),
+                    ),
+                )
+
         elif event["type"] == "STOP":
 
-            # Waiting for the arm to stop moving before stopping the node
+            if not TEACH_MODE:
+                piper.MotionCtrl_2(0x01, 0x01, 50, 0x00)
+                piper.JointCtrl(0, 0, 0, 0, 0, 0)
+                piper.GripperCtrl(abs(0), 1000, 0x01, 0)
+                piper.MotionCtrl_2(0x01, 0x01, 50, 0x00)
             time.sleep(5)
             break
 

{dora_piper-0.3.6.dist-info → dora_piper-0.3.8.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: dora-piper
-Version: 0.3.6
+Version: 0.3.8
 Summary: Dora Node for using Agilex piper
 Author: Haixuan Xavier Tao
 Author-email: tao.xavier@outlook.com
@@ -14,7 +14,8 @@ Classifier: Programming Language :: Python :: 3.11
 Classifier: Programming Language :: Python :: 3.12
 Classifier: Programming Language :: Python :: 3.13
 Requires-Dist: dora-rs (>=0.3.6,<0.4.0)
-Requires-Dist: piper_sdk (>=0.0.7,<0.0.8)
+Requires-Dist: numpy (<2.0.0)
+Requires-Dist: piper_sdk (>=0.0.8,<0.0.9)
 Description-Content-Type: text/markdown
 
 # Dora Node (Experimental) to communicate with Agilex Piper SDK

dora_piper-0.3.8.dist-info/RECORD

@@ -0,0 +1,6 @@
+dora_piper/__init__.py,sha256=Gy4qL4vCeTyA5HR1Yp3ioL4-ClJyW8oi_38CzMuMsBM,358
+dora_piper/main.py,sha256=K_wKIIgVEtPoef2FLiVz5HRCKMJK05F-Y0JdgMoY42M,6294
+dora_piper-0.3.8.dist-info/METADATA,sha256=jgeo4eeMgOd7GqTOe_9gHOLVUSNjPzpoD_MGBBSyy_c,1079
+dora_piper-0.3.8.dist-info/WHEEL,sha256=Nq82e9rUAnEjt98J6MlVmMCZb-t9cYE2Ir1kpBmnWfs,88
+dora_piper-0.3.8.dist-info/entry_points.txt,sha256=PuWCNzfJelov0oX-TmquNxlsUikVwvAE9hjlQKyC2T8,51
+dora_piper-0.3.8.dist-info/RECORD,,

dora_piper-0.3.6.dist-info/RECORD

@@ -1,6 +0,0 @@
-dora_piper/__init__.py,sha256=Gy4qL4vCeTyA5HR1Yp3ioL4-ClJyW8oi_38CzMuMsBM,358
-dora_piper/main.py,sha256=29bsNt0LPScEbnjshekP_gxj2DIZg1cddbCQvs6TfB4,4238
-dora_piper-0.3.6.dist-info/METADATA,sha256=VPnYEZ1R2TQ0oKmIDGJMnq7LC-tJ92WU2BlqAWBc5io,1049
-dora_piper-0.3.6.dist-info/WHEEL,sha256=Nq82e9rUAnEjt98J6MlVmMCZb-t9cYE2Ir1kpBmnWfs,88
-dora_piper-0.3.6.dist-info/entry_points.txt,sha256=PuWCNzfJelov0oX-TmquNxlsUikVwvAE9hjlQKyC2T8,51
-dora_piper-0.3.6.dist-info/RECORD,,