integrate agilex piper arm into lerobot

piper_scripts/piper_disable.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+# -*-coding:utf8-*-
+# 注意demo无法直接运行，需要pip安装sdk后才能运行
+# 使能机械臂
+from typing import (
+    Optional,
+)
+import time
+from piper_sdk import *
+
+def enable_fun(piper:C_PiperInterface_V2, enable:bool):
+    '''
+    使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
+    '''
+    enable_flag = False
+    loop_flag = False
+    # 设置超时时间（秒）
+    timeout = 5
+    # 记录进入循环前的时间
+    start_time = time.time()
+    elapsed_time_flag = False
+    while not (loop_flag):
+        elapsed_time = time.time() - start_time
+        print(f"--------------------")
+        enable_list = []
+        enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_1.foc_status.driver_enable_status)
+        enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_2.foc_status.driver_enable_status)
+        enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_3.foc_status.driver_enable_status)
+        enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_4.foc_status.driver_enable_status)
+        enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_5.foc_status.driver_enable_status)
+        enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_6.foc_status.driver_enable_status)
+        if(enable):
+            enable_flag = all(enable_list)
+            piper.EnableArm(7)
+            piper.GripperCtrl(0,1000,0x01, 0)
+        else:
+            enable_flag = any(enable_list)
+            piper.DisableArm(7)
+            piper.GripperCtrl(0,1000,0x02, 0)
+        print(f"使能状态: {enable_flag}")
+        print(f"--------------------")
+        if(enable_flag == enable):
+            loop_flag = True
+            enable_flag = True
+        else: 
+            loop_flag = False
+            enable_flag = False
+        # 检查是否超过超时时间
+        if elapsed_time > timeout:
+            print(f"超时....")
+            elapsed_time_flag = True
+            enable_flag = False
+            loop_flag = True
+            break
+        time.sleep(0.5)
+    resp = enable_flag
+    print(f"Returning response: {resp}")
+    return resp
+
+# 测试代码
+if __name__ == "__main__":
+    piper = C_PiperInterface_V2()
+    piper.ConnectPort()
+    import time
+    flag = enable_fun(piper=piper, enable=False)
+    if(flag == True):
+        print("失能成功!!!!")
+        exit(0)
+    pass