新增demo和函数

piper_sdk/demo/V2/piper_read_instruction_response.py

@@ -0,0 +1,19 @@
+#!/usr/bin/env python3
+# -*-coding:utf8-*-
+# 注意demo无法直接运行，需要pip安装sdk后才能运行
+# 读取机械臂消息并打印,需要先安装piper_sdk
+import time
+from piper_sdk import *
+
+# 测试代码
+if __name__ == "__main__":
+    piper = C_PiperInterface_V2()
+    piper.ConnectPort()
+    while True:
+        print(piper.GetInstructionResponse())
+        if piper.GetInstructionResponse().instruction_response.instruction_index == 0x71:
+            # 捕获到设置指令0x471的应答时，等待3s后清除SDK保存的应答信息
+            time.sleep(3)
+            piper.ClearInstructionResponse()
+        time.sleep(0.005)
+

piper_sdk/demo/V2/piper_read_tcp_pose.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python3
+# -*-coding:utf8-*-
+# 测试TCP偏移
+import math
+import time
+from piper_sdk import *
+
+
+def apply_tool_offset(pose, tool_offset):
+    '''
+    计算TCP在基座系下的位置
+
+    Args:
+        pose: (X, Y, Z, RX, RY, RZ)
+            X,Y,Z: 单位0.001mm
+            RX,RY,RZ: 单位0.001度 (XYZ旋转顺序)
+        tool_offset: (tool_x, tool_y, tool_z) 单位米
+            在J6坐标系下工具中心点的坐标值 
+
+    Returns:
+        TCP_XYZ: (X_tcp, Y_tcp, Z_tcp) 单位0.001mm
+    '''
+    '''
+    Calculate TCP position in base coordinate system
+
+    Args:
+        pose: Tuple (X, Y, Z, RX, RY, RZ)
+            X,Y,Z: Position in 0.001mm units
+            RX,RY,RZ: Rotation in 0.001 degrees (XYZ rotation order)
+        tool_offset: Tuple (tool_x, tool_y, tool_z) in meters
+            TCP coordinates in J6 coordinate system
+
+    Returns:
+        TCP_XYZ: Tuple (X_tcp, Y_tcp, Z_tcp) in 0.001mm units
+    '''
+    X, Y, Z, RX, RY, RZ = pose
+
+    # Convert angles: 0.001 degrees → radians
+    rx_rad = math.radians(RX / 1000.0)
+    ry_rad = math.radians(RY / 1000.0)
+    rz_rad = math.radians(RZ / 1000.0)
+
+    # Precompute trig values
+    cx, sx = math.cos(rx_rad), math.sin(rx_rad)
+    cy, sy = math.cos(ry_rad), math.sin(ry_rad)
+    cz, sz = math.cos(rz_rad), math.sin(rz_rad)
+
+    # Compute rotation matrix (XYZ order: R = Rz * Ry * Rx)
+    r00 = cy * cz
+    r01 = sx * sy * cz - cx * sz
+    r02 = cx * sy * cz + sx * sz
+
+    r10 = cy * sz
+    r11 = sx * sy * sz + cx * cz
+    r12 = cx * sy * sz - sx * cz
+
+    r20 = -sy
+    r21 = sx * cy
+    r22 = cx * cy
+
+    # Convert tool offset: meters → 0.001mm
+    tool_x_mm = tool_offset[0] * 1000000.0
+    tool_y_mm = tool_offset[1] * 1000000.0
+    tool_z_mm = tool_offset[2] * 1000000.0
+
+    # Apply rotation to tool offset
+    offset_x = r00 * tool_x_mm + r01 * tool_y_mm + r02 * tool_z_mm
+    offset_y = r10 * tool_x_mm + r11 * tool_y_mm + r12 * tool_z_mm
+    offset_z = r20 * tool_x_mm + r21 * tool_y_mm + r22 * tool_z_mm
+
+    # Calculate TCP position with rounding
+    return (
+        round(X + offset_x),
+        round(Y + offset_y),
+        round(Z + offset_z)
+    )
+
+
+if __name__ == "__main__":
+    # 初始化机械臂连接
+    piper = C_PiperInterface_V2()
+    piper.ConnectPort()
+
+    # 等待机械臂启用
+    while not piper.EnablePiper():
+        time.sleep(0.01)
+
+    # 设置初始位置
+    piper.MotionCtrl_2(0x01, 0x00, 100, 0x00)
+    piper.EndPoseCtrl(0, 0, 400000, 0, 89999, 0)
+
+    # 获取末端位姿并设置工具偏移
+    end_pose = piper.GetArmEndPoseMsgs().end_pose
+    tool_offset = (0, 0, 0.145)  # Z轴偏移145mm
+
+    # 实时计算并显示TCP位置
+    while True:
+        pose = (
+            end_pose.X_axis, 
+            end_pose.Y_axis, 
+            end_pose.Z_axis, 
+            end_pose.RX_axis, 
+            end_pose.RY_axis, 
+            end_pose.RZ_axis
+        )
+
+        x_tcp, y_tcp, z_tcp = apply_tool_offset(pose, tool_offset)
+
+        print("="*50)
+        print("J6位置 (0.001mm):")
+        print(f" X: {pose[0]}, Y: {pose[1]}, Z: {pose[2]}")
+
+        print("\nTCP位置 (0.001mm):")
+        print(f" X: {x_tcp}, Y: {y_tcp}, Z: {z_tcp}\n")
+
+        time.sleep(0.005)

piper_sdk/demo/V2/piper_set_joint_zero.py

@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+# -*-coding:utf8-*-
+# 注意demo无法直接运行，需要pip安装sdk后才能运行
+# 设置关节电机零点位置
+import time
+from piper_sdk import *
+
+# 测试代码
+if __name__ == "__main__":
+    piper = C_PiperInterface_V2()
+    piper.ConnectPort()
+    time.sleep(0.1)
+    print("设置过程输入'q'可以退出程序")
+    print("During setup, enter 'q' to exit the program")
+    print("设置零点前会对指定的电机失能，请保护好机械臂")
+    print("Before setting zero position, the specified motor will be disabled. Please protect the robotic arm.")
+    while( not piper.EnablePiper()):
+        time.sleep(0.01)
+    piper.ModeCtrl(0x01, 0x01, 30, 0x00)
+    piper.JointCtrl(0, 0, 0, 0, 0, 0)
+    piper.GripperCtrl(0, 1000, 0x01, 0)
+    mode = -1
+    while True:
+        # 模式选择
+        if mode == -1:
+            print("\nStep 1: 请选择设置模式(0: 指定电机; 1: 顺序设置): ")
+            print("Step 1: Select setting mode (0: Single motor; 1: Sequential setting): ")
+            mode = input("> ")
+            if mode == '0':
+                mode = 0
+            elif mode == '1':
+                mode = 1
+            elif mode == 'q':
+                break
+            else:
+                mode = -1
+
+        # 单电机设置
+        elif mode == 0:
+            print("\nStep 2: 输入需要设置零点的电机序号(1~7), 7代表所有电机: ")
+            print("Step 2: Enter motor number to set zero (1~7), 7 represents all motors: ")
+            motor_num = input("> ")
+            if motor_num == 'q':
+                mode = -1
+                continue
+            try:
+                motor_num = int(motor_num)
+                if motor_num < 1 or motor_num > 7:
+                    print("Tip: 输入超出范围")
+                    print("Tip: Input out of range")
+                    continue
+            except:
+                print("Tip: 请输入整数")
+                print("Tip: Please enter an integer")
+                continue
+            piper.DisableArm(motor_num)
+            print(f"\nInfo: 第{motor_num}号电机失能成功，请手动纠正电机的零点位置")
+            print(f"Info: Motor {motor_num} disabled successfully. Please manually adjust to zero position")
+
+            print(f"\nStep 3: 回车设置第{motor_num}号电机零点: ")
+            print(f"Step 3: Press Enter to set zero for motor {motor_num}: ")
+            if input("(按回车继续/Press Enter) ") == 'q':
+                mode = -1
+                continue
+            piper.JointConfig(motor_num, 0xAE)
+            piper.EnableArm(motor_num)
+            print(f"\nInfo: 第{motor_num}号电机零点设置成功")
+            print(f"Info: Motor {motor_num} zero position set successfully")
+
+        # 顺序设置
+        elif mode == 1:
+            print("\nStep 2: 输入从第几号电机开始设置(1~6): ")
+            print("Step 2: Enter starting motor number (1~6): ")
+            motor_num = input("> ")
+            if motor_num == 'q':
+                mode = -1
+                continue
+            try:
+                motor_num = int(motor_num)
+                if motor_num < 1 or motor_num > 6:
+                    print("Tip: 输入超出范围")
+                    print("Tip: Input out of range")
+                    continue
+            except:
+                print("Tip: 请输入整数")
+                print("Tip: Please enter an integer")
+                continue
+            for i in range(motor_num, 7):
+                piper.DisableArm(i)
+                print(f"\nInfo: 第{i}号电机失能成功，请手动纠正电机的零点位置")
+                print(f"Info: Motor {i} disabled successfully. Please manually adjust to zero position")
+
+                print(f"\nStep 3: 回车设置第{i}号电机零点: ")
+                print(f"Step 3: Press Enter to set zero for motor {i}: ")
+                if input("(按回车继续/Press Enter) ") == 'q':
+                    mode = -1
+                    break
+                piper.JointConfig(i, 0xAE)
+                piper.EnableArm(i)
+                print(f"\nInfo: 第{i}号电机零点设置成功")
+                print(f"Info: Motor {i} zero position set successfully")

piper_sdk/interface/piper_interface.py

@@ -2881,45 +2881,6 @@ def JointMaxAccConfig(self, motor_num: Literal[1, 2, 3, 4, 5, 6] = 6, max_joint_
         '''
         self.JointConfig(motor_num, 0, 0xAE, max_joint_acc, 0)
 
-    def SetInstructionResponse(self, instruction_index: int, zero_config_success_flag: Literal[0, 1] = 0):
-        '''
-        设置指令应答
-
-        CAN ID:
-            0x476
-
-        Args:
-            instruction_index: 应答指令索引
-                取设置指令 id 最后一个字节
-                例如,应答 0x471 设置指令时此位填充0x71
-            zero_config_success_flag: 零点是否设置成功
-                零点成功设置-0x01
-                设置失败/未设置-0x00
-                仅在关节设置指令--成功设置 N 号电机当前位置为零点时应答-0x01
-        '''
-        '''
-        Sets the response for the instruction.
-
-        CAN ID: 0x476
-
-        Args:
-            instruction_index (int): The response instruction index.
-                This is derived from the last byte of the set instruction ID.
-                For example, when responding to the 0x471 set instruction, this would be 0x71.
-
-            zero_config_success_flag (int): Flag indicating whether the zero point was successfully set.
-                0x01: Zero point successfully set.
-                0x00: Zero point set failed/not set.
-                This is only applicable when responding to a joint setting instruction that successfully sets motor N's current position as the zero point.
-        '''
-        tx_can = Message()
-        set_resp = ArmMsgInstructionResponseConfig(instruction_index, zero_config_success_flag)
-        msg = PiperMessage(type_=ArmMsgType.PiperMsgInstructionResponseConfig, arm_set_instruction_response=set_resp)
-        self.__parser.EncodeMessage(msg, tx_can)
-        feedback = self.__arm_can.SendCanMessage(tx_can.arbitration_id, tx_can.data)
-        if feedback is not self.__arm_can.CAN_STATUS.SEND_MESSAGE_SUCCESS:
-            self.logger.error("SetInstructionResponse send failed: SendCanMessage(%s)", feedback)
-
     def ArmParamEnquiryAndConfig(self, 
                                  param_enquiry: Literal[0x00, 0x01, 0x02, 0x03, 0x04] = 0x00, 
                                  param_setting: Literal[0x00, 0x01, 0x02] = 0x00,