目标检测与跟踪（13）-- Jetson Xavier NX / Orin NX 松灵机械臂PiPer SDK、ROS功能包、官方文档解读

1. 机械臂简介

该六自由度机械臂是专为教育科研行业、消费级应用场景、工业自动化设计，具备1.5kg的负载能力，适用于人形机器人、自动装配、自动搬运等多种科研及工业应用场景。六个旋转关节提供了全方位的操作灵活性，确保高精度和重复性。机械臂采用模块化设计，便于维护和升级。提供直观的用户界面，简化编程和操作流程，即使是非专业人士也能快速上手。可以广泛应用于科学研究、教育教学、汽车制造、电子组装、食品加工、实验室自动化和医疗设备操作等领域。

1.1 性能参数

|------|--------|------------------------------------------------------------------------|
| 参数类型 | 项目 | 指标 |
| 结构参数 | 自由度 | 6 |
| 结构参数 | 有效负载 | 1.5KG |
| 结构参数 | 本体重量 | 4.2KG |
| 结构参数 | 重复定位精度 | ±.0.1mm |
| 结构参数 | 工作半径 | 626.75mm |
| 结构参数 | 标准供电电压 | DC24V 最小工作电压24V,最大工作电压26V |
| 结构参数 | 功耗 | 最大功耗≤120W 综合功耗≤40W |
| 结构参数 | 材质 | 铝合金骨架、塑料外壳 |
| 结构参数 | 控制器 | 集成 |
| 结构参数 | 通讯方式 | CAN |
| 结构参数 | 控制方式 | 拖动示教/离线轨迹/上位机 |
| 结构参数 | 外部接口 | 电源接口*1，CAN接口*1 |
| 结构参数 | 底座安装尺寸 | 70mm*70mm*M5*4 |
| 结构参数 | 工作环境 | 温度：-20-50℃，湿度：25%-85%，非冷凝 |
| 结构参数 | 噪音 | <60dB |
| 结构参数 | 安装 | 松灵全系机器人产品 |
| 运动参数 | 关节运动范围 | J1:±154° J2:0°~195° J3:-175°~0° J4:-102°~102° J5:-75°~75° J6:±170° |
| | 关节最大速度 | J1:180°/s J2:195°/s J3:180°/s J4:225°/s J5:225°/s J6:225°/s |
| 注意：上述数据为松灵机械臂在受控测试环境下的测试结果。在不同的环境、使用方式下，结果或有不同程度的差异，请以实际体验为准。 |||

|------------|---------------------------|
| 两指夹爪参数（选配） | |
| 本体重量 | 500g |
| 精度 | ±.0.5mm |
| 开合间距 | 0-100mm（历史版本为0-70mm） |
| 额定夹合力 | 40N |
| 最大夹合力 | 50N |
| 供电电压 | DC24V |
| 功耗 | 最大功耗≤50W 综合功耗≤30W |
| 接触面材质 | 橡胶 |
| 控制器 | 集成 |
| 通讯方式 | CAN |
| 控制方式 | 拖动示教/离线轨迹/API/上位机 |
| 外部接口 | 电源接口*1，CAN接口*1 |
| 安装方式 | 法兰安装 |
| 工作环境 | 温度：-20-50℃，湿度：25%-85%，非冷凝 |
| 噪音 | <60dB |
| 示教器参数（选配） | |
| 本体重量 | 550g |
| 精度 | ±.0.5mm |
| 开合间距 | 0-70mm |
| 额定夹合力 | 40N（力控，力反馈） |
| 最大夹合力 | 50N（力控，力反馈） |
| 供电电压 | DC24V |
| 功耗 | 最大功耗≤50W 综合功耗≤30W |
| 接触面材质 | 橡胶 |
| 控制器 | 集成 |
| 通讯方式 | CAN |
| 控制方式 | 拖动示教/离线轨迹/API/上位机 |
| 外部接口 | 电源接口*1，CAN接口*1 |
| 安装方式 | 法兰安装 |
| 工作环境 | 温度：-20-50℃，湿度：25%-85%，非冷凝 |
| 噪音 | <60dB |

1.2 M-DH参数表

该机械臂具备6自由度，1.5k末端负载，六个旋转关节提供了全方位的操作灵活性，确保高精度和重复性。采用了轻量化的设计，使得机械臂具备快速运动能力的同时兼具较高的负载能力，可以广泛应用于具身智能方向真实场景数据采集。

1.3 电气接口说明

机械臂另提供航空插头、USB转CAN模块、数据线、适配器以及末端电机预留的XT30 2+2接口，具体使用方式请按以下说明进行使用。

电器接口图示：

1: 供电通讯口 2: 状态指示灯 3: 主控-J2连接口

4：供电正 5：供电负 6：CAN-H 7：CAN-L

注意：红点朝向与下面线缆红点对应，航插带纹路部位为受力向后可收缩区域，在安装时红点向下对准凸点直接插入即可，拔出时在纹路部分按下后拔出即可，安装方式如下图所示：

电气面板2位置灯光指示说明：

|-------|--------------------------------|
| 指示灯颜色 | 状态说明 |
| 绿灯闪烁 | 机械臂运行正常 |
| 绿灯常亮 | 主控器上电异常，需重新拔插上电 |
| 红灯闪烁 | 红灯闪烁，机械臂异常或者机械臂进入升级模式，前者需要排查故障 |

机械臂末关节示教指示灯说明：

|---------|-------------------------|
| 示教指示灯颜色 | 状态说明 |
| 无灯光 | 机械臂拖动示教停止状态/机械臂拖动记录结束状态 |
| 绿灯常亮 | 机械臂进入拖动示教轨迹记录状态 |
| 绿灯闪烁 | 机械臂进入拖动示教轨迹复现状态 |

1.4 机械臂坐标系说明

1.4.1 原点坐标系

piper的原点（x:0，y:0, z:0）的位置

1.4.2 用户坐标系

用户坐标系为机械臂基座坐标系

1.4.3 奇异位置

1、腕部奇异位姿： 当J4 与 J6共轴时为腕部奇异。

2、肘部奇异位姿：当J1与J4共轴时为肘部奇异。

3、肩部奇异位姿：当J5与J6的轴交点在J1的轴线上时为肩部奇异

4、边界奇异位姿：当机械臂末端位置位于最大工作范围时为边界奇异位姿。

1.4.4 零点

机械臂关节零点为如图位置，J1零点校准刻度对齐为零点，J2、J3当机械臂失能后自然垂下放置即为零点，J6轴线与J4共线状态下J5为零点

零点坐标 X = 56.128 , Y = 0 , Z = 213.266 , RX = 0 ,RY = 85.0 , RZ = 0 。

1.5 机械臂工作空间及负载

机械臂末端负载曲线图：

1.6 关节零点说明

1.7 示教模式使用说明：

械臂拖动示教状态可通过 J5与J6 之间的按钮指示灯表现。

机械臂状态灯光显示有三种：

1. 无灯光显示： 机械臂拖动示教停止状态/机械臂拖动记录结束状态。

1. 绿色灯光常量：机械臂进入拖动示教轨迹记录状态。

3、绿色灯光闪烁：机械臂进入拖动示教轨迹复现状态。

切换拖动状态操作方法：

1、单击按钮：用于切换拖动示教轨迹记录状态和拖动记录结束状态。

2、双击按钮：拖动示教轨迹复现状态。

使用说明:

首先观察指示灯状态

1、如果指示灯为无灯光显示，可单击按钮，此时绿色灯光应常亮，用户可直接拖动机械臂，表示进入轨迹记录状态，开始轨迹记录。

2、如果指示灯为无灯光显示，并且在此之前有操作轨迹记录，可双击按钮，此时绿色灯光以500ms闪烁，表示进入轨迹复现状态，机械臂会复现一次记录的轨迹动作。

3、如果灯光为常亮显示，表示正在进行轨迹记录，如果需要结束记录，可单击按钮，此时灯光应为不亮状态，表示进入记录结束状态，如果想复现轨迹请按照2操作。

4、如果灯光为闪烁状态，表示正在进行轨迹复现。

1.8 can连接与准备

将插头CAN线引出，将CAN线中的CAN_H和CAN_L分别与CAN_TO_USB适配器相连；

将CAN_TO_USB连接至笔记本的usb口。连接示意如图所示。

此外，固定can线需要向有h ,l这一面，顺时针拧紧导线，确保导线不会脱落，如下图所示。导线往上压，不要往下！！！！

注意：如果使用的非标配充电器，电源输入不得超过26V，电流不小于10A。

末端连接电器说明：

末端关节预留XT30 2+2接口：电源和CAN定义下图。电压24V 电流2A MAX。末端CAN通讯只适配松灵自有设备，严禁私自接入其他CAN设备，否则可能造成不可控破坏。

上电使用说明：

机械臂第一次上电需要按照以下A-G顺序进行操作：

1、先把A的J2连接口插上；

2、把航空插头B的CAN线接好；

3、把C的XT30接头对插好；

4、把D航空插头红点向下对插好；

5、把适配器E的插头插好；

6、检查确认适配器E的AC头插好上电，待电器面板指示灯闪烁绿色后；

7、把USB线插进电脑进行使用。

注意：红点朝向与下面线缆红点对应，航插带纹路部位为受力向后可收缩区域，在安装时红点向下对准凸点直接插入即可，拔出时在纹路部分按下后拔出即可。

1.9 结构模型

https://agilexsupport.yuque.com/staff-hso6mo/alxgtf/km95una15ctk4b17?singleDoc# 《PiPER 模型文件》

---

2. 工程代码

2.1 二次开发链接

|-------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
| 类目 | 链接 |
| SDK | https://github.com/agilexrobotics/piper_sdk（interface参见readme） |
| SDK-DEMO | https://github.com/agilexrobotics/piper_sdk/tree/master/demo |
| SDK-UI | https://github.com/agilexrobotics/Piper_sdk_ui |
| ROS1-NOETIC | https://github.com/agilexrobotics/piper_ros/tree/noetic |
| ROS2-HUMBLE | https://github.com/agilexrobotics/piper_ros/tree/humble |
| URDF | https://github.com/agilexrobotics/piper_ros/tree/noetic/src/piper_description/urdf |
| MOVEIT2 | https://github.com/agilexrobotics/piper_ros/tree/humble/src/piper_moveit |
| MOVEIT | https://github.com/agilexrobotics/piper_ros/tree/noetic/src/piper_moveit |
| GAZEBO | https://github.com/agilexrobotics/piper_ros/tree/noetic/src/piper_sim/piper_gazebo |
| ISSAC SIM | https://github.com/agilexrobotics/piper_isaac_sim |
| MUJOCO | https://github.com/agilexrobotics/piper_ros/tree/noetic/src/piper_sim/piper_mujoco |
| 松灵开源技术贴 | https://github.com/agilexrobotics/Agilex-College |
| | https://github.com/agilexrobotics/piper_ros/tree/6cf947c58797775dba148861729a5e87c99564bf/src/piper/scripts/piper_pinocchio https://github.com/vanstrong12138/GraspGen |

2.2 接口示意

2.2.1 SDK(V1-V2版本)

版本说明：

piper_sdk/asserts/V1/CHANGELOG-V1.MD at 1_0_0_beta · agilexrobotics/piper_sdk

piper_sdk/asserts/V2/CHANGELOG-V2.MD at 1_0_0_beta · agilexrobotics/piper_sdk

最新的V2版本SDK接口文档中Update记录说明了接口的使用和更迭问题;

piper_sdk/asserts/V2/INTERFACE_V2.MD at 1_0_0_beta · agilexrobotics/piper_sdk

MotionCtrl_2()已经改为ModeCtrl()接口

机械臂控制相关的接口函数Control Methods：

2.2.2 ROS SDK

agilexrobotics/piper_ros: piper ros workspacehttps://github.com/agilexrobotics/piper_ros/tree/noetic

单机械臂

节点名piper_ctrl_single_node.py

param

can_port:要打开的can路由名字
auto_enable:是否自动使能，True则开启程序就自动使能
# 注意这个设置为False，中断程序后再启动节点，机械臂会保持上次启动程序的状态
# 若上次启动程序机械臂状态为使能，则中断程序后再启动节点，机械臂仍为使能
# 若上次启动程序机械臂状态为失能，则中断程序后再启动节点，机械臂仍为失能
girpper_exist:是否有末端夹爪，True则说明有末端夹爪，会开启夹爪控制
rviz_ctrl_flag:是否使用rviz来发送关节角消息，True则接收rviz发送的关节角消息
#由于rviz中的joint7范围是[0,0.04]，而真实夹爪行程为0.08m，打开rviz控制后会将rviz发出的joint7乘2倍

start_single_piper.launch如下:

<launch>
  <arg name="can_port" default="can0" />
  <arg name="auto_enable" default="true" />
  <arg name="gripper_val_mutiple" default="1" />
  <!-- <include file="$(find piper_description)/launch/display_xacro.launch"/> -->
  <!-- 启动机械臂节点 -->
  <node name="piper_ctrl_single_node" pkg="piper" type="piper_ctrl_single_node.py" output="screen">
    <param name="can_port" value="$(arg can_port)" />
    <param name="auto_enable" value="$(arg auto_enable)" />
    <param name="gripper_val_mutiple" value="$(arg gripper_val_mutiple)" />
    <!-- <param name="rviz_ctrl_flag" value="true" /> -->
    <param name="girpper_exist" value="true" />
    <remap from="joint_ctrl_single" to="/joint_states" />
  </node>
</launch>

start_single_piper_rviz.launch如下:

<launch>
  <arg name="can_port" default="can0" />
  <arg name="auto_enable" default="true" />
  <include file="$(find piper_description)/launch/piper_with_gripper/display_xacro.launch"/>
  <!-- 启动机械臂节点 -->
  <node name="piper_ctrl_single_node" pkg="piper" type="piper_ctrl_single_node.py" output="screen">
    <param name="can_port" value="$(arg can_port)" />
    <param name="auto_enable" value="$(arg auto_enable)" />
    <param name="gripper_val_mutiple" value="2" />
    <!-- <param name="rviz_ctrl_flag" value="true" /> -->
    <param name="girpper_exist" value="true" />
    <remap from="joint_ctrl_single" to="/joint_states" />
  </node>
</launch>

（1）启动控制节点

提供几种不同的启动方式，启动的都是相同节点

# 启动节点
roscore
rosrun piper piper_ctrl_single_node.py _can_port:=can0 _mode:=0
# 或，使用launch 启动节点
roslaunch piper start_single_piper.launch can_port:=can0 auto_enable:=true
# 或，直接运行launch，会以默认参数运行
roslaunch piper start_single_piper.launch
# 也可以用rviz开启控制,需要更改的参数如上述
roslaunch piper start_single_piper_rviz.launch

如果只是单独启动了控制节点，没有启动rviz

rostopic list

/arm_status #机械臂状态，详见下文
/enable_flag #使能标志位，发送给节点，发送true用来使能
/end_pose #机械臂末端位姿状态反馈，四元数
/end_pose_euler #机械臂末端位姿状态反馈，欧拉角(为自定义消息)
/joint_states #订阅关节消息，给这个消息发送关节位置能控制机械臂运动
/joint_states_single #机械臂关节状态反馈
/pos_cmd #末端控制消息

rosservice list

/enable_srv #机械臂使能服务端
/go_zero_srv #机械臂归零服务
/gripper_srv #机械臂夹爪控制服务
/reset_srv #机械臂重置服务
/stop_srv #机械臂停止服务

（2）使能机械臂

# call 服务端
rosservice call /enable_srv "enable_request: true"
# pub topic
rostopic pub /enable_flag std_msgs/Bool "data: true"

（3）失能机械臂

# call 服务端
rosservice call /enable_srv "enable_request: false"
# pub topic
rostopic pub /enable_flag std_msgs/Bool "data: false"

（4）发布关节消息

注意，机械臂会抬起，请确保机械臂工作范围内无障碍

rostopic pub /joint_states sensor_msgs/JointState "header:
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: ''
name: ['']
position: [0.2,0.2,-0.2,0.3,-0.2,0.5,0.01]
velocity: [0,0,0,0,0,0,10]
effort: [0,0,0,0,0,0,0.5]" 

（5）停止机械臂，注意机械臂会以一个恒定阻尼落下

rosservice call /stop_srv

（6）重置机械臂，注意机械臂会立刻掉电落下

rosservice call /reset_srv

（7）令机械臂归零

• 如果机械臂为mit模式，将is_mit_mode置为true
• 如果机械臂不为mit模式(位置速度控制模式)，将is_mit_mode置为false

rosservice call /go_zero_srv "is_mit_mode: false"
rosservice call /go_zero_srv "is_mit_mode: true"

关键代码包：src/piper/scripts/piper_ctrl_single_node.py

 def joint_callback(self, joint_data):
        """机械臂关节角回调函数

        Args:
            joint_data (): 
        """
        if not self.block_ctrl_flag:
            factor = 57324.840764 #1000*180/3.14
            factor = 1000 * 180 / np.pi
            # rospy.loginfo("Received Joint States:")
            # rospy.loginfo("joint_0: %f", joint_data.position[0])
            # rospy.loginfo("joint_1: %f", joint_data.position[1])
            # rospy.loginfo("joint_2: %f", joint_data.position[2])
            # rospy.loginfo("joint_3: %f", joint_data.position[3])
            # rospy.loginfo("joint_4: %f", joint_data.position[4])
            # rospy.loginfo("joint_5: %f", joint_data.position[5])
            # rospy.loginfo("joint_6: %f", joint_data.position[6])
            # print(joint_data.position)
            joint_0 = round(joint_data.position[0]*factor)
            joint_1 = round(joint_data.position[1]*factor)
            joint_2 = round(joint_data.position[2]*factor)
            joint_3 = round(joint_data.position[3]*factor)
            joint_4 = round(joint_data.position[4]*factor)
            joint_5 = round(joint_data.position[5]*factor)
            if(len(joint_data.position) >= 7):
                joint_6 = round(joint_data.position[6]*1000*1000)
                joint_6 = joint_6 * self.gripper_val_mutiple
                if(joint_6>80000): joint_6 = 80000
                if(joint_6<0): joint_6 = 0
            else: joint_6 = None
            if(self.GetEnableFlag()):
                # 设定电机速度
                if(joint_data.velocity != []):
                    all_zeros = all(v == 0 for v in joint_data.velocity)
                else: all_zeros = True
                if(not all_zeros):
                    lens = len(joint_data.velocity)
                    if(lens == 7):
                        vel_all = round(joint_data.velocity[6])
                        if (vel_all > 100): vel_all = 100
                        if (vel_all < 0): vel_all = 0
                        rospy.loginfo("vel_all: %d", vel_all)
                        self.piper.MotionCtrl_2(0x01, 0x01, vel_all)
                    # elif(lens == 7):
                    #     # 遍历速度列表
                    #     for i, velocity in enumerate(joint_data.velocity):
                    #         if velocity > 0:  # 如果速度是正数
                    #             # 设置指定位置的关节速度为这个正数速度
                    #             # self.piper.SearchMotorMaxAngleSpdAccLimit(i+1,0x01)
                    #             # self.piper.MotorAngleLimitMaxSpdSet(i+1)
                    else: self.piper.MotionCtrl_2(0x01, 0x01, 50,0)
                else: self.piper.MotionCtrl_2(0x01, 0x01, 50,0)
                
                # 给定关节角位置
                self.piper.JointCtrl(joint_0, joint_1, joint_2, 
                                        joint_3, joint_4, joint_5)
                # 如果末端夹爪存在，则发送末端夹爪控制
                if(self.gripper_exist and joint_6 is not None):
                    if abs(joint_6)<200:
                        joint_6=0
                    if(len(joint_data.effort) >= 7):
                        gripper_effort = joint_data.effort[6]
                        gripper_effort = max(0.5, min(gripper_effort, 3))
                        # rospy.loginfo("gripper_effort: %f", gripper_effort)
                        gripper_effort = round(gripper_effort*1000)
                        self.piper.GripperCtrl(abs(joint_6), gripper_effort, 0x01, 0)
                    # 默认1N
                    else: self.piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0)

在机械臂的SDK中由于兼容性问题，V2及V1版本的接口API会同时存在，例如：

self.piper.MotionCtrl_2(0x01, 0x01, 50,0)

self.piper.ModeCtrl(0x01, 0x01, 50,0)

# 给定关节角位置
self.piper.JointCtrl(joint_0, joint_1, joint_2,
joint_3, joint_4, joint_5)

回零操作：

def handle_go_zero_service(self,req):
        response = GoZeroResponse()
        response.status = False
        response.code = 151000
        rospy.loginfo(f"-----------------------GOZERO---------------------------")
        rospy.loginfo(f"piper go zero .")
        rospy.loginfo(f"-----------------------GOZERO---------------------------")
        if(req.is_mit_mode):
            self.piper.MotionCtrl_2(0x01, 0x01, 50, 0xAD)
        else:
            self.piper.MotionCtrl_2(0x01, 0x01, 50, 0)
        self.piper.JointCtrl(0, 0, 0, 0, 0, 0)
        response.status = True
        response.code = 151001
        rospy.loginfo(f"Returning GoZeroResponse: {response.status}, {response.code}")
        return response

        self.piper.MotionCtrl_2(0x01, 0x01, 10, 0) 设置动作模式为速度-位置控制，速度10
        self.piper.JointCtrl(0, 0, 0, 0, 0, 0)，给定关节角度为初始值00

2.2.3 示教再现模式

机械臂自身状态反馈消息，对应can协议中id=0x2A1的反馈消息 说明

PiperStatusMsg.msg

uint8 ctrl_mode
/*
0x00 待机模式  
0x01 CAN指令控制模式
0x02 示教模式
0x03 以太网控制模式
0x04 wifi控制模式
0x05 遥控器控制模式
0x06 联动示教输入模式
0x07 离线轨迹模式*/
uint8 arm_status
/*
0x00 正常
0x01 急停
0x02 无解
0x03 奇异点
0x04 目标角度超过限
0x05 关节通信异常
0x06 关节抱闸未打开 
0x07 机械臂发生碰撞
0x08 拖动示教时超速
0x09 关节状态异常
0x0A 其它异常  
0x0B 示教记录
0x0C 示教执行
0x0D 示教暂停
0x0E 主控NTC过温
0x0F 释放电阻NTC过温*/
uint8 mode_feedback
/*
0x00 MOVE P
0x01 MOVE J
0x02 MOVE L
0x03 MOVE C*/
uint8 teach_status
/*
0x00 关闭
0x01 开始示教记录（进入拖动示教模式）
0x02 结束示教记录（退出拖动示教模式）
0x03 执行示教轨迹（拖动示教轨迹复现）
0x04 暂停执行
0x05 继续执行（轨迹复现继续）
0x06 终止执行
0x07 运动到轨迹起点*/
uint8 motion_status
/*
0x00 到达指定点位
0x01 未到达指定点位*/
uint8 trajectory_num
/*0~255 (离线轨迹模式下反馈)*/
int64 err_code//故障码
bool joint_1_angle_limit//1号关节通信异常（0：正常 1：异常）
bool joint_2_angle_limit//2号关节通信异常（0：正常 1：异常）
bool joint_3_angle_limit//3号关节通信异常（0：正常 1：异常）
bool joint_4_angle_limit//4号关节通信异常（0：正常 1：异常）
bool joint_5_angle_limit//5号关节通信异常（0：正常 1：异常）
bool joint_6_angle_limit//6号关节通信异常（0：正常 1：异常）
bool communication_status_joint_1//1号关节角度超限位（0：正常 1：异常）
bool communication_status_joint_2//2号关节角度超限位（0：正常 1：异常）
bool communication_status_joint_3//3号关节角度超限位（0：正常 1：异常）
bool communication_status_joint_4//4号关节角度超限位（0：正常 1：异常）
bool communication_status_joint_5//5号关节角度超限位（0：正常 1：异常）
bool communication_status_joint_6//6号关节角度超限位（0：正常 1：异常）

Agilex-College/piper/recordAndPlayTraj at master · agilexrobotics/Agilex-College

连续轨迹示教代码：

#!/usr/bin/env python3
# -*-coding:utf8-*-
# 录制连续轨迹
import os, time
from piper_sdk import *

if __name__ == "__main__":
    # 是否有夹爪
    have_gripper = True
    # 最大录制时间，单位：秒，0表示无限长（强制关闭程序可结束录制）
    record_time = 10.0
    # 示教模式检测超时时间，单位：秒
    timeout = 10.0
    # 保存轨迹的CSV文件路径
    CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")
    # 初始化并连接机械臂
    piper = C_PiperInterface_V2("can0")
    piper.ConnectPort()
    time.sleep(0.1)

    def get_pos():
        '''获取机械臂当前关节弧度和夹爪张开距离'''
        joint_state = piper.GetArmJointMsgs().joint_state
        joint_state = tuple(getattr(joint_state, f"joint_{i+1}") / 1e3 * 0.0174533 for i in range(6))
        if have_gripper:
            return joint_state + (piper.GetArmGripperMsgs().gripper_state.grippers_angle / 1e6, )
        return joint_state
    
    print("step 1: 请点击示教按钮进入示教模式")
    over_time = time.time() + timeout
    while piper.GetArmStatus().arm_status.ctrl_mode != 2:
        if over_time < time.time():
            print("ERROR: 示教模式检测超时，请检查示教模式是否开启")
            exit()
        time.sleep(0.01)

    input("step 2: 回车开始录制轨迹")
    csv = open(CSV_path, "w")
    last_pos = get_pos()
    last_time = time.time()
    over_time = last_time + record_time
    while record_time == 0 or time.time() < over_time:
        current_pos = get_pos()
        if current_pos != last_pos: # 发生变化时记录
            wait_time = time.time() - last_time
            print(f"INFO: 等待时间:{wait_time : 0.4f}s，当前位置: {current_pos}")
            last_pos = current_pos
            last_time = time.time()
            csv.write(f"{wait_time}," + ",".join(map(str, current_pos)) + "\n")
        time.sleep(0.01)
    csv.close()
    print("INFO: 录制结束，再次点击示教按钮退出示教模式")

轨迹再现：

#!/usr/bin/env python3
# -*-coding:utf8-*-
# 播放连续轨迹
import os, time, csv
from piper_sdk import *

if __name__ == "__main__":
    # 是否有夹爪
    have_gripper = True
    # 播放次数 0表示无限循环
    play_times = 1
    # 播放间隔，单位：秒
    play_interval = 1.0
    # 运动速度百分比，建议范围：10-100
    move_spd_rate_ctrl = 100
    # 播放倍速，建议范围：0.1-2
    play_speed = 1.0
    # 切换CAN模式超时时间，单位：秒
    timeout = 5.0
    # 保存轨迹的CSV文件路径
    CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")
    # 读取轨迹文件
    try:
        with open(CSV_path, 'r', encoding='utf-8') as f:
            track = list(csv.reader(f))
            if not track:
                print("ERROR: 轨迹文件为空")
                exit()
            track = [[float(j) for j in i] for i in track]    # 转换为浮点数列表
    except FileNotFoundError:
        print("ERROR: 轨迹文件不存在")
        exit()

    # 初始化并连接机械臂
    piper = C_PiperInterface_V2("can0")
    piper.ConnectPort()
    time.sleep(0.1)

    def get_pos():
        '''获取机械臂当前关节弧度和夹爪张开距离'''
        joint_state = piper.GetArmJointMsgs().joint_state
        joint_state = tuple(getattr(joint_state, f"joint_{i+1}") / 1e3 * 0.0174533 for i in range(6))
        if have_gripper:
            return joint_state + (piper.GetArmGripperMsgs().gripper_state.grippers_angle / 1e6, )
        return joint_state
    
    def stop():
        '''停止机械臂；初次退出示教模式需先调用此函数才能使用CAN模式控制机械臂'''
        piper.EmergencyStop(0x01)
        time.sleep(1.0)
        limit_angle = [0.1745, 0.7854, 0.2094]  # 2、3、5关节弧度在限制范围内时才恢复机械臂，防止大弧度直接掉落造成损坏
        pos = get_pos()
        while not (abs(pos[1]) < limit_angle[0] and abs(pos[2]) < limit_angle[0] and pos[4] < limit_angle[1] and pos[4] > limit_angle[2]):
            time.sleep(0.01)
            pos = get_pos()
        # 恢复机械臂
        piper.EmergencyStop(0x02)
        time.sleep(1.0)
    
    def enable():
        '''使能机械臂和夹爪'''
        while not piper.EnablePiper():
            time.sleep(0.01)
        if have_gripper:
            time.sleep(0.01)
            piper.GripperCtrl(0, 1000, 0x01, 0x00)
        piper.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)
        print("INFO: 使能成功")

    print("step 1: 播放前请确保机械臂已退出示教模式")
    if piper.GetArmStatus().arm_status.ctrl_mode != 1:
        stop()  # 初次退出示教模式需先调用此函数才能切换至CAN模式
    over_time = time.time() + timeout
    while piper.GetArmStatus().arm_status.ctrl_mode != 1:
        if over_time < time.time():
            print("ERROR: CAN模式切换失败，请检查是否退出示教模式")
            exit()
        piper.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)
        time.sleep(0.01)
    
    enable()
    count = 0
    input("step 2: 回车开始播放轨迹")
    while play_times == 0 or abs(play_times) != count:
        for n, pos in enumerate(track):
            joints = [round(i / 0.0174533 * 1e3) for i in pos[1:-1]]
            piper.MotionCtrl_2(0x01, 0x01, move_spd_rate_ctrl, 0x00)
            piper.JointCtrl(*joints)
            if have_gripper and len(pos) == 8:
                piper.GripperCtrl(round(pos[-1] * 1e6), 1000, 0x01, 0x00)
            print(f"INFO: 第{count + 1}次播放，等待时间:{pos[0] / play_speed : 0.4f}s，目标位置: {pos[1:]}")
            if n == len(track) - 1:
                time.sleep(play_interval)
            else:
                time.sleep(pos[0] / play_speed)
        count += 1

2.3 开源案例集

Piper 系列机械臂

标题	描述
固定点位录制与播放	使用Piper录制固定点位运动并播放
连续轨迹录制与播放	使用Piepr录制连续运动的轨迹并播放
机械臂识别方块与曲线	使用相机识别方块和曲线；并使Piper机械臂跟随曲线
手机陀螺仪遥操机械臂	使用手机陀螺仪遥操机械臂臂
手势遥操机械臂	使用手势遥操作Piper机械臂末端六自由度位姿
Piper_kinematics	机械臂逆解数值教学与Piper底层解析解的调用
游戏手柄	使用游戏手柄遥操机械臂
手眼标定	Piper手眼标定教程
GraspGen	位姿生成与抓取
Piper_rl	PiPER强化学习demo
Isaac sim 导入piper	在Isaac sim 中导入piper并添加摄像头
复现RDA_planner	复现RDA_planner
Piper_moveit	从零到玩转Moveit 机械臂控制（ROS1）
Nero_moveit	从零到玩转Moveit 机械臂控制（ROS2）

3. 功能测试