From 61f4f3a8856541a2c37e80f7df10edb28af5259f Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Wed, 13 Nov 2024 04:13:31 +0100 Subject: [PATCH] Add piper sdk into dora node hub --- node-hub/dora-piper/README.md | 9 ++ node-hub/dora-piper/dora_piper/__init__.py | 11 +++ node-hub/dora-piper/dora_piper/main.py | 109 +++++++++++++++++++++ node-hub/dora-piper/pyproject.toml | 21 ++++ 4 files changed, 150 insertions(+) create mode 100644 node-hub/dora-piper/README.md create mode 100644 node-hub/dora-piper/dora_piper/__init__.py create mode 100644 node-hub/dora-piper/dora_piper/main.py create mode 100644 node-hub/dora-piper/pyproject.toml diff --git a/node-hub/dora-piper/README.md b/node-hub/dora-piper/README.md new file mode 100644 index 00000000..0c562d42 --- /dev/null +++ b/node-hub/dora-piper/README.md @@ -0,0 +1,9 @@ +# Dora Node (Experimental) to communicate with Agilex Piper SDK + +This node is used to communicate with piper. + +Make sure to follow both the setup script and installation script from https://github.com/agilexrobotics/piper_sdk + +# To setup the arm + +Make sure that the can bus is activated for both arm and leader arms are not connected. diff --git a/node-hub/dora-piper/dora_piper/__init__.py b/node-hub/dora-piper/dora_piper/__init__.py new file mode 100644 index 00000000..ac3cbef9 --- /dev/null +++ b/node-hub/dora-piper/dora_piper/__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, "r", encoding="utf-8") as f: + __doc__ = f.read() +except FileNotFoundError: + __doc__ = "README file not found." diff --git a/node-hub/dora-piper/dora_piper/main.py b/node-hub/dora-piper/dora_piper/main.py new file mode 100644 index 00000000..c7e74dc0 --- /dev/null +++ b/node-hub/dora-piper/dora_piper/main.py @@ -0,0 +1,109 @@ +from piper_sdk import * +from dora import Node +import pyarrow as pa +import os +import time + +TEACH_MODE = False + + +def enable_fun(piper: C_PiperInterface): + """ + 使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序 + """ + enable_flag = False + # 设置超时时间（秒） + timeout = 5 + # 记录进入循环前的时间 + start_time = time.time() + elapsed_time_flag = False + while not (enable_flag): + elapsed_time = time.time() - start_time + print("--------------------") + enable_flag = ( + piper.GetArmLowSpdInfoMsgs().motor_1.foc_status.driver_enable_status + and piper.GetArmLowSpdInfoMsgs().motor_2.foc_status.driver_enable_status + and piper.GetArmLowSpdInfoMsgs().motor_3.foc_status.driver_enable_status + and piper.GetArmLowSpdInfoMsgs().motor_4.foc_status.driver_enable_status + and piper.GetArmLowSpdInfoMsgs().motor_5.foc_status.driver_enable_status + 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: + print("超时....") + elapsed_time_flag = True + enable_flag = True + break + time.sleep(1) + pass + if elapsed_time_flag: + print("程序自动使能超时,退出程序") + exit(0) + + +def main(): + elapsed_time = time.time() + CAN_BUS = os.getenv("CAN_BUS", "") + piper = C_PiperInterface(CAN_BUS) + piper.ConnectPort() + + if TEACH_MODE is False: + # piper.MotionCtrl_3(0, 0, 0, 0x00)#位置速度模式 + piper.EnableArm(7) + enable_fun(piper=piper) + piper.GripperCtrl(0, 1000, 0x01, 0) + 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: + + # 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: + continue + + position = event["value"].to_numpy() + joint_0 = round(position[0] * factor) + joint_1 = round(position[1] * factor) + joint_2 = round(position[2] * factor) + 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) + + 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["type"] == "STOP": + + # Waiting for the arm to stop moving before stopping the node + time.sleep(5) + break + + +if __name__ == "__main__": + main() diff --git a/node-hub/dora-piper/pyproject.toml b/node-hub/dora-piper/pyproject.toml new file mode 100644 index 00000000..cf678d49 --- /dev/null +++ b/node-hub/dora-piper/pyproject.toml @@ -0,0 +1,21 @@ +[tool.poetry] +name = "dora-piper" +version = "0.3.6" +authors = ["Haixuan Xavier Tao "] +description = "Dora Node for using Agilex piper" +readme = "README.md" + +packages = [{ include = "dora_piper" }] + +[tool.poetry.dependencies] +dora-rs = "^0.3.6" +python = "^3.7" +piper_sdk = "^0.0.7" + + +[tool.poetry.scripts] +dora-piper = "dora_piper" + +[build-system] +requires = ["poetry-core>=1.8.0"] +build-backend = "poetry.core.masonry.api"