（来源：AGV网）
本文基于松灵PIPER机械臂实现了固定点位录制与播放功能。通过对Python SDK的应用，可以实现机械臂特定位置的记录和重复执行，为教学演示和自动化操作提供了强有力的技术支撑。
本文将分析代码实现细节，并提供完整代码、使用指南和步骤演示。
代码仓库
Github链:https://github.com/agilexrobotics/Agilex-College.git
【功能展示】
一、使用前准备
1.1. PIPER 机械臂硬件准备
工作空间内无障碍物，为机械臂提供充足的运动空间
确认机械臂电源正常，所有指示灯显示正常状态
照明条件良好，便于观察机械臂位置和状态
如果配备夹爪，检查夹爪动作是否正常
地面平稳，避免振动影响录制精度
验证示教按钮功能正常
1.2.环境配置
操作系统：Ubuntu（推荐Ubuntu 18.04或更高版本）
Python环境：Python 3.6或更高版本
git代码管理工具：用于克隆远程的代码仓库
sudo apt install gitpip包管理器：用于安装Python依赖包
sudo apt install python3-pip安装CAN工具
sudo apt install can-utils ethtool安装官方提供的Python SDK包，其中1_0_0_beta是带API的版本
git clone -b 1_0_0_beta https://github.com/agilexrobotics/piper_sdk.gitcd piper_sdkpip3 install .
参考文档：
https://github.com/agilexrobotics/piper_sdk/blob/1_0_0_beta/README(ZH).MD
二、固定点位录制与播放功能操作步骤
机械臂上电，将USB转CAN模块与电脑连接（确保只连接一个CAN模块）
打开终端，激活CAN模块
sudo ip link set can0 up type can bitrate 1000000克隆远程代码仓库
git clone https://github.com/agilexrobotics/Agilex-College.git切换至recordAndPlayPos目录
cd Agilex-College/piper/recordAndPlayPos/运行录制程序
python3 recordPos.py短按示教按钮进入示教模式
摆放好机械臂的位置，终端回车进行点位的录制，输入'q'可结束录制
录制结束后，再次短按示教按钮退出示教模式
播放前须知：初次退出示教模式时，需要经过特定的初始化过程才能从示教模式切换到CAN模式，因此播放程序会自动执行复位操作，将2、3、5号关节回到安全位置（零点），防止机械臂在重力作用下突然下落造成损坏，在特殊情况下需要人工辅助2、3、5关节回到零点
运行播放程序
python3 playPos.py使能成功后，终端回车即可播放点位
三、过程中遇到的问题及解决方法
没有Piper类。
原因：当前安装的SDK不是带API的版本。
解决方法：执行pip3 uninstall piper_sdk卸载当前的SDK，然后按照1.2.环境配置的方法安装1_0_0_beta版本的SDK。
机械臂不动，终端输出如下。
原因：程序运行过程中，短按了示教按钮。
解决方法：检查示教按钮的指示灯是否处于熄灭状态，如果是重新运行程序即可，不是则需先短按示教按钮退出示教模式再运行程序。
四、实现代码/原理解读与参数说明
4.1. 点位录制程序的实现
点位录制程序是系统的数据采集模块，负责在示教模式下捕获机械臂的关节位置信息。
4.1.1. 程序初始化与配置
4.1.1.1. 参数配置设计
# 是否有夹爪have_gripper = True# 示教模式检测超时时间，单位：秒timeout = 10.0# 保存点位的CSV文件路径CSV_path = os.path.join(os.path.dirname(__file__), "pos.csv")
配置参数分析：
have_gripper参数类型为布尔类型，True时代表有夹爪。
timeout参数为设定示教模式检测超时时间，启动程序后，如果10s没有进入示教模式，程序会退出。
CSV_path参数为设定轨迹文件保存路径，默认为与程序当前同路径，文件名为pos.csv
4.1.1.2. 机械臂连接与初始化
# 初始化并连接机械臂piper = Piper("can0")interface = piper.init()piper.connect()time.sleep(0.1)
连接机制分析：
Piper()是API的核心类，该类在interface的基础上简化了通用的一些方法。
init()会创建并返回interface实例，可用于调用Piper的一些特殊方法。
connect()会开启一个线程来连接CAN端口以及处理CAN数据。
time.sleep(0.1)的添加是为了确保连接完全建立。在嵌入式系统中，硬件初始化通常需要一定的时间，这个短暂的延时确保了后续操作的可靠性。
4.1.2. 位置获取与数据存储
4.1.2.1. 位置获取函数实现
def get_pos():'''获取机械臂当前关节弧度和夹爪张开距离'''joint_state = piper.get_joint_states()[0]if have_gripper:return joint_state + (piper.get_gripper_states()[0][0], )return joint_state
4.1.2.2. 模式检测与切换
print("INFO: 请点击示教按钮进入示教模式")over_time = time.time() + timeoutwhile interface.GetArmStatus().arm_status.ctrl_mode != 2:if over_time < time.time():print("ERROR: 示教模式检测超时，请检查示教模式是否开启")exit()time.sleep(0.01)
状态轮询策略：程序采用轮询方式检测控制模式，这种方法的特点：
实现简单，逻辑清晰
对系统资源要求较低
超时保护机制：10秒的超时设置考虑了实际操作的需要：
用户理解提示信息的时间
找到并按下示教按钮的时间
系统响应和状态切换的时间
异常情况下的容错处理
示教模式的安全特性：
关节力矩释放，允许手动操作
保持位置反馈，实时监控位置变化
4.1.2.3. 数据录制与存储
count = 1csv = open(CSV_path, "w")while input("INPUT: 输入 q 退出，直接回车录制: ") != "q":current_pos = get_pos()print(f"INFO: 第{count}个点位，记录位置: {current_pos}")csv.write(",".join(map(str, current_pos)) + "\n")count += 1csv.close()print("INFO: 录制结束，再次点击示教按钮退出示教模式")
数据完整性保证：每次录制后立即写入文件并刷新缓冲区，确保数据不会因为程序异常退出而丢失。
数据格式选择：选择CSV格式存储数据的原因包括：
通用性强，几乎所有数据处理工具都支持CSV格式
人类可读性强，便于调试和验证
结构简单，解析效率高
广泛支持，便于与其他工具集成
数据列属性：
第1~6列：关节电机弧度
第7列：夹爪张开距离，单位m
4.1.3. 点位录制程序的完整代码实现
#!/usr/bin/env python3# -*-coding:utf8-*-# 录制点位import os, timefrom piper_sdk import *if __name__ == "__main__":# 是否有夹爪have_gripper = True# 示教模式检测超时时间，单位：秒timeout = 10.0# 保存点位的CSV文件路径CSV_path = os.path.join(os.path.dirname(__file__), "pos.csv")# 初始化并连接机械臂piper = Piper("can0")interface = piper.init()piper.connect()time.sleep(0.1)def get_pos():'''获取机械臂当前关节弧度和夹爪张开距离'''joint_state = piper.get_joint_states()[0]if have_gripper:return joint_state + (piper.get_gripper_states()[0][0], )return joint_stateprint("INFO: 请点击示教按钮进入示教模式")over_time = time.time() + timeoutwhile interface.GetArmStatus().arm_status.ctrl_mode != 2:if over_time < time.time():print("ERROR: 示教模式检测超时，请检查示教模式是否开启")exit()time.sleep(0.01)count = 1csv = open(CSV_path, "w")while input("INPUT: 输入 q 退出，直接回车录制: ") != "q":current_pos = get_pos()print(f"INFO: 第{count}个点位，记录位置: {current_pos}")csv.write(",".join(map(str, current_pos)) + "\n")count += 1csv.close()print("INFO: 录制结束，再次点击示教按钮退出示教模式")
4.2. 点位播放程序的实现
点位播放程序是系统的执行模块，负责读取录制的点位数据并控制机械臂重现这些位置。
4.2.1. 参数配置与数据加载
4.2.1.1. 播放参数配置
# 播放次数，0表示无限循环play_times = 1# 播放间隔，单位：秒，负数表示人工按键控制play_interval = 0# 运动速度百分比，建议范围：10-100move_spd_rate_ctrl = 100
参数设计分析：
play_times参数支持三种播放模式：
单次播放（play_times = 1）：适用于演示和测试
多次播放（play_times > 1）：适用于重复性任务
无限循环（play_times = 0）：适用于连续作业
play_interval的负值设计是一个巧妙的用户界面设计：
正值：自动播放模式，系统按设定间隔自动执行
零值：连续播放模式，点位间无延迟
负值：手动控制模式，用户按键控制播放节奏
move_spd_rate_ctrl参数提供了速度控制功能，这对于不同应用场景非常重要：
高速模式（80-100%）：适用于空载快速移动
中速模式（50-80%）：适用于一般操作任务
低速模式（10-50%）：适用于精密操作和安全要求高的场合
4.2.1.2. 数据文件读取
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()
异常处理策略：
FileNotFoundError：处理文件不存在的情况
空文件检查：防止读取空数据文件
数据格式验证：确保数据能够正确转换为数值类型
数据类型转换：将字符串数据转换为浮点数的过程中，程序使用了列表推导式。
4.2.2. 安全停止函数
def stop():'''停止机械臂；初次退出示教模式需先调用此函数才能使用CAN模式控制机械臂'''interface.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.disable_arm()time.sleep(1.0)
分阶段停止策略：停止函数采用了分阶段的安全停止策略
紧急停止阶段：EmergencyStop(0x01)发送紧急停止指令，立即停止所有关节运动（关节带阻抗）
安全位置等待：等待关键关节（2、3、5号关节）移动到安全范围内
系统恢复阶段：发送恢复指令，重新激活控制系统
安全范围设计：程序特别关注2、3、5号关节的位置，这是基于PIPER机械臂的机械结构特点
2号关节（肩关节）：控制大臂的俯仰运动，影响整体稳定性
3号关节（肘关节）：控制小臂角度，直接影响末端位置
5号关节（腕关节）：控制末端姿态，影响夹爪方向
安全角度范围的设定（10°、45°、12°）基于以下考虑：
避免机械臂在重力作用下快速下落
确保关节不会碰撞到机械限位
为后续运动提供足够的操作空间
实时监控机制：程序采用实时轮询方式监控关节位置，确保只有在安全条件满足时才进行下一步操作。
4.2.3. 系统使能函数
def enable():'''使能机械臂和夹爪'''while not piper.enable_arm():time.sleep(0.01)if have_gripper:time.sleep(0.01)piper.enable_gripper()interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)print("INFO: 使能成功")
机械臂使能：enable_arm()
夹爪使能：enable_gripper()
控制模式设置：ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)控制模式参数
第一个参数（0x01）：设置为CAN指令控制模式
第二个参数（0x01）：设置为关节运动模式
第三个参数（0至100）：设置运动速度百分比
第四个参数（0x00）：设置为位置速度模式
4.2.4. 播放控制逻辑
count = 0input("step 2: 回车开始播放点位")while play_times == 0 or abs(play_times) != count:for n, pos in enumerate(track):while True:piper.move_j(pos[:-1], move_spd_rate_ctrl)time.sleep(0.01)current_pos = get_pos()print(f"INFO: 第{count + 1}次播放，第{n + 1}个点位，当前位置: {current_pos}，目标位置: {pos}")if all(abs(current_pos[i] - pos[i]) < 0.0698 for i in range(6)):breakif have_gripper and len(pos) == 7:piper.move_gripper(pos[-1], 1)time.sleep(0.5)if play_interval < 0:if n != len(track) - 1 and input("INPUT: 输入 q 退出，直接回车播放: ") == 'q':exit()else:time.sleep(play_interval)count += 1
关节控制：move_j()
第一个参数：包含六个关节电机弧度的元组
第二个参数：运动速度百分比，范围0~100
夹爪控制：move_gripper()
第一个参数：夹爪张开距离，单位m
第二个参数：夹爪力矩，单位N/m
位置控制闭环系统：
目标设定：通过move_j()函数向各关节发送目标位置指令
状态反馈：通过get_pos()函数获取当前实际位置
误差计算：比较目标位置与实际位置的差值
收敛判断：当误差小于阈值时认为到达目标
多关节协调控制：all(abs(current_pos[i] - pos[i]) < 0.0698 for i in range(6))确保所有六个关节都达到目标位置后才进行下一步操作。
夹爪控制策略：夹爪控制采用独立的控制逻辑：
只有在数据包含夹爪信息时才执行夹爪控制
夹爪动作在关节运动完成后执行，避免干扰
0.5秒的延时确保夹爪动作完全完成
播放节奏控制：程序支持三种播放节奏：
自动连续播放：play_interval >= 0
手动步进播放：play_interval < 0
实时调整：用户可以随时中断播放
4.2.5. 点位播放程序的完整代码实现
#!/usr/bin/env python3# -*-coding:utf8-*-# 播放点位import os, time, csvfrom piper_sdk import Piperif __name__ == "__main__":# 是否有夹爪have_gripper = True# 播放次数，0表示无限循环play_times = 1# 播放间隔，单位：秒，负数表示人工按键控制play_interval = 0# 运动速度百分比，建议范围：10-100move_spd_rate_ctrl = 100# 切换CAN模式超时时间，单位：秒timeout = 5.0# 保存点位的CSV文件路径CSV_path = os.path.join(os.path.dirname(__file__), "pos.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 = Piper("can0")interface = piper.init()piper.connect()time.sleep(0.1)def get_pos():'''获取机械臂当前关节弧度和夹爪张开距离'''joint_state = piper.get_joint_states()[0]if have_gripper:return joint_state + (piper.get_gripper_states()[0][0], )return joint_statedef stop():'''停止机械臂；初次退出示教模式需先调用此函数才能使用CAN模式控制机械臂'''interface.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.disable_arm()time.sleep(1.0)def enable():'''使能机械臂和夹爪'''while not piper.enable_arm():time.sleep(0.01)if have_gripper:time.sleep(0.01)piper.enable_gripper()interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)print("INFO: 使能成功")print("step 1: 播放前请确保机械臂已退出示教模式")if interface.GetArmStatus().arm_status.ctrl_mode != 1:stop() # 初次退出示教模式需先调用此函数才能切换至CAN模式over_time = time.time() + timeoutwhile interface.GetArmStatus().arm_status.ctrl_mode != 1:if over_time < time.time():print("ERROR: CAN模式切换失败，请检查是否退出示教模式")exit()interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)time.sleep(0.01)enable()count = 0input("step 2: 回车开始播放点位")while play_times == 0 or abs(play_times) != count:for n, pos in enumerate(track):while True:piper.move_j(pos[:-1], move_spd_rate_ctrl)time.sleep(0.01)current_pos = get_pos()print(f"INFO: 第{count + 1}次播放，第{n + 1}个点位，当前位置: {current_pos}，目标位置: {pos}")if all(abs(current_pos[i] - pos[i]) < 0.0698 for i in range(6)):breakif have_gripper and len(pos) == 7:piper.move_gripper(pos[-1], 1)time.sleep(0.5)if play_interval < 0:if n != len(track) - 1 and input("INPUT: 输入 q 退出，直接回车播放：") == 'q':exit()else:time.sleep(play_interval)count += 1
五、小结
以上基于松灵PIPER机械臂实现了固定点位录制与播放功能。通过对Python SDK的应用，可以实现机械臂特定位置的记录和重复执行，为教学演示和自动化操作提供了强有力的技术支撑.有任何相关使用的疑问都可以联系我们：support@agilex.ai
